Provides Studio's core Behavior plugins, which enable fundamental planning, execution, and perception processing capabilities.

List of Behaviors

This is a brief overview of the behaviors built into the core behaviors plugin. For details on each type of built in behavior, refer to the class's documentation.

Generic Behaviors

InitializeMotionConstraints

Creates a shared pointer to a new moveit_msgs::msg::Constraints and writes it to the Blackboard.

Data Port Name	Port Type	Object Type
constraints_name	input	std::string
constraints	output	std::shared_ptr<moveit_msgs::msg::Constraints>

CallTriggerService

Calls a service that accepts a std_srvs::srv::Trigger message. The name of the service is set through the service_name parameter.

Data Port Name	Port Type	Object Type
service_name	Input	std::string

IsForceWithinThreshold

A condition node that monitors a wrench topic and returns BT::NodeStatus::FAILURE when ticked if the magnitude of the force components has exceeded a specified threshold for some number of consecutive observations.

Data Port Name	Port Type	Object Type
parameters	Input	YAML::Node

Parameter Name	Parameter Type
wrench_topic_name	std::string
hand_frame_name	std::string
wrench_frame_name	std::string
minimum_consecutive_wrench_values	std::size_t
force_threshold	double

GetPlanUsingTAMP

Perform Task and Motion Planning (TAMP) to generate a plan, given a PDDL problem.

Data Port Name	Port Type	Object Type
domain_service_name	Input	std::string
planner_service_name	Input	std::string
problem_file_name	Input	std::string
plan	Output	plansys2_msgs::msg::Plan

CreateBehaviorTreeFromTaskPlan

Create and execute a Behavior Tree from a task plan. action_mappings_file is a path to a YAML file that provides mappings between actions in the PDDL domain and Subtrees that can actually be executed. The resulting Behavior Tree will be saved in the Objectives directory as <behavior_tree_name>.xml

Data Port Name	Port Type	Object Type
plan	Input	plansys2_msgs::msg::Plan
behavior_tree_name	Input	std::string
action_mappings_file	Input	std::string

MoveGripperAction

Actuate a gripper through its driver node's GripperCommand action server. Given the name of the action topic and a target gripper position, move the gripper to the specified position.

Data Port Name	Port Type	Object Type
gripper_command_action_name	Input	std::string
position	Input	double

TransformPose

Transforms a stamped pose given an input translation and orientation.

The transformation occurs in the reference frame specified by the pose stamp. For example, if the input pose is expressed in the "world" frame, the translation and orientation are also applied in the "world" frame to produce the output pose.

Data Port Name	Port Type	Object Type
input_pose	input	geometry_msgs::msg::PoseStamped
position_xyz	input	std::vector<double>
orientation_xyzw	input	std::vector<double>
output_pose	output	geometry_msgs::msg::PoseStamped

TransformPoseFrame

Transforms the reference frame of an input stamped pose to the frame specified by a frame ID, along with the position/orientation transformation that entails.

Data Port Name	Port Type	Object Type
input_pose	input	geometry_msgs::msg::PoseStamped
target_frame_id	input	std::string
output_pose	output	geometry_msgs::msg::PoseStamped

AppendOrientationConstraint

Given an existing moveit_msgs::msg::Constraints, creates a moveit_msgs::msg::OrientationConstraint from an input yaml file and appends this orientation constraint to the existing constraints message.

Data Port Name	Port Type	Object Type
config_file_name	Input	std::string
constraints	Bidirectional	std::shared_ptr<moveit_msgs::msg::Constraints>

MoveToWaypoint

Given a named waypoint, move to that waypoint.

Data Port Name	Port Type	Object Type
waypoint_name	Input	std::string
planning_group_name	Input	std::string
controller_names	Input	std::string
use_all_planners	Input	bool
constraints	Input	std::shared_ptr<moveit_msgs::msgs::Constraints>

BiasedCoinFlip

Simulates flipping a biased coin with the specified probability of success provided via the input port.

Data Port Name	Port Type	Object Type
success_probability	Input	double

MoveIt Task Constructor (MTC) Behaviors

InitializeMTCTask

Creates a shared pointer to a new MTC Task object, populates it with global settings (for example, the names of controllers to enable by default when executing trajectories planned by this Task), and sets it as an output data port.

Data Port Name	Port Type	Object Type
task_id	Input	std::string
controller_names	Input	std::string
task	Output	std::shared_ptr<moveit::task_constructor::Task>

PlanMTCTask

Takes a shared pointer to an existing MTC Task object via an input data port, plans the Task, and sets the resulting MTC Solution object as an output data port.

Data Port Name	Port Type	Object Type
task	Input	std::shared_ptr<moveit::task_constructor::Task>
solution	Output	std::shared_ptr<const moveit::task_constructor::SolutionBase>

ExecuteMTCTask

Takes a shared pointer to an MTC Solution object via an input data port, and executes the lowest-cost trajectory in that Solution using the MTC ExecuteTaskSolution MoveGroup capability's /execute_task_solution action server.

Data Port Name	Port Type	Object Type
solution	Input	std::shared_ptr<const moveit::task_constructor::SolutionBase>

SetupMTCCurrentState

Given an existing MTC Task object, appends an MTC CurrentState Stage to the Task.

Data Port Name	Port Type	Object Type
task	Bidirectional	std::shared_ptr<moveit::task_constructor::Task>

SetupMTCApproachGrasp

Given an existing MTC Task object and a target object, appends MTC stages to describe a motion plan to approach the object.

This Behavior must be followed by another Behavior that adds MTC stages for grasp pose generation. The monitored_stage output provides the name of the MTC stage containing the initial (pre-aproach) planning scene, which can be used by the grasp generation stage(s).

Data Port Name	Port Type	Object Type
task	Bidirectional	std::shared_ptr<moveit::task_constructor::Task>
target_object	Input	moveit_studio_vision_msgs::msg::GraspableObject
parameters	Input	YAML::Node
monitored_stage	Output	std::string

SetupMTCGenerateCuboidGrasps

Given an existing MTC Task object and a target object, appends MTC stages to generate cuboid grasp poses.

The monitored_stage input provides the name of the MTC stage containing the initial (pre-aproach) planning scene, which can be used by the grasp generation stage(s) in this Behavior.

Data Port Name	Port Type	Object Type
task	Bidirectional	std::shared_ptr<moveit::task_constructor::Task>
target_object	Input	moveit_studio_vision_msgs::msg::GraspableObject
monitored_stage	Input	std::string
parameters	Input	YAML::Node

SetupMTCGenerateVacuumGrasps

Given an existing MTC Task object and a target object, appends MTC stages to generate vacuum grasp poses on its planar surfaces.

The monitored_stage input provides the name of the MTC stage containing the initial (pre-aproach) planning scene, which can be used by the grasp generation stage(s) in this Behavior.

Data Port Name	Port Type	Object Type
task	Bidirectional	std::shared_ptr<moveit::task_constructor::Task>
target_object	Input	moveit_studio_vision_msgs::msg::GraspableObject
monitored_stage	Input	std::string
parameters	Input	YAML::Node

SetupMTCRetractFromGrasp

Given an existing MTC Task object and a target object, appends MTC stages to describe a motion plan to retract after grasping the object.

Data Port Name	Port Type	Object Type
task	Bidirectional	std::shared_ptr<moveit::task_constructor::Task>
target_object	Input	moveit_studio_vision_msgs::msg::GraspableObject
parameters	Input	YAML::Node

SetupMTCMoveToJointState

Given an existing MTC Task object and a joint state, appends MTC stages to describe a freespace motion plan to that joint state.

Data Port Name	Port Type	Object Type
task	Bidirectional	std::shared_ptr<moveit::task_constructor::Task>
planning_group_name	Input	std::string
joint_state	Input	sensor_msgs::msg::JointState
use_all_planners	Input	bool
constraints	Input	std::shared_ptr<moveit_msgs::msgs::Constraints>

SetupMTCMoveToNamedState

Given an existing MTC Task object and the name of a known state, appends MTC stages to describe a freespace motion plan to that state.

Data Port Name	Port Type	Object Type
task	Bidirectional	std::shared_ptr<moveit::task_constructor::Task>
planning_group_name	Input	std::string
goal_state_name	Input	std::string

SetupMTCInterpolateToJointState

Given an existing MTC Task object and a joint state, appends MTC stages to describe a joint-interpolated motion plan to that joint state.

Data Port Name	Port Type	Object Type
task	Bidirectional	std::shared_ptr<moveit::task_constructor::Task>
planning_group_name	Input	std::string
joint_state	Input	sensor_msgs::msg::JointState

SetupMTCUpdateGroupCollisionRule

Add an MTC Stage to an MTC Task that modifies the planning scene's Allowed Collision Matrix to permit or forbid collision between a planning scene object and the links of a named robot planning group while planning subsequent Stages.

Data Port Name	Port Type	Object Type
task	Bidirectional	std::shared_ptr<moveit::task_constructor::Task>
parameters	Input	YAML::Node

Parameter Name	Parameter Type
group_name	std::string
object_name	std::string
allow_collision	bool

SetupMTCMoveAlongFrameAxis

Given an existing MTC Task object, frame name, axis, and move distance, appends MTC stages to describe a cartesian motion plan in the frame's axis with the specified distance.

Data Port Name	Port Type	Object Type
parameters	Input	YAML::Node
task	Bidirectional	std::shared_ptr<moveit::task_constructor::Task>
axis	Input	std::string
move_distance	Input	double

Parameter Name	Parameter Type
arm_group_name	std::string
hand_frame_name	std::string
velocity_acceleration_scaling_factor	double

SetupMTCPickObject

Given an existing MTC Task object and a target grasp pose, appends MTC stages to describe a motion plan to approach, grasp and lift an object at that pose.

Data Port Name	Port Type	Object Type
task	Bidirectional	std::shared_ptr<moveit::task_constructor::Task>
grasp_pose	Input	geometry_msgs::msg::PoseStamped
parameters	Input	YAML::Node

SetupMTCAffordanceTemplate

Configures MTC stages to perform a motion that can be parameterized as a screw trajectory. Examples in practice include opening doors and drawers.

The input data ports are generally calculated by separate perception processing Behaviors in a previous step of the Objective.

Given an existing MTC Task object and input parameters that configure a screw motion affordance template, perform the following steps:

Move to a pre-grasp pose offset from the specified grasp pose.
Approach the grasp pose.
Close the gripper.
Move along a screw-parameterised trajectory.
Open the gripper.
Retreat from the grasp pose.

Data Port Name	Port Type	Object Type
task	Bidirectional	std::shared_ptr<moveit::task_constructor::Task>
grasp_pose	Input	geometry_msgs::msg::PoseStamped
screw_origin_pose	Input	geometry_msgs::msg::PoseStamped
screw_axis_pose	Input	geometry_msgs::msg::PoseStamped
translation_distance	Input	double
rotation_distance	Input	double
use_circular_arc	Input	bool

SetupMTCOpenLeverHandleDoor

Configures MTC stages to open a lever-handled door by turning the handle and pushing the door away from the robot.

The input data ports are generally calculated by separate perception processing Behaviors in a previous step of the Objective.

Data Port Name	Port Type	Object Type
task	Bidirectional	std::shared_ptr<moveit::task_constructor::Task>
handle_pose	Input	geometry_msgs::msg::PoseStamped
handle_z_offset	Input	double
handle_length	Input	double
approach_distance	Input	double
push_open_distance	Input	double
move_distance	Input	double
use_circular_arc	Input	bool

User Input Behaviors

GetDrawerAxisFromSelection

Given an input PoseStamped representing a grasp pose selected on a drawer handle, output three PoseStampeds that define a screw motion to open the drawer.

Assumes that the Z- axis of the grasp pose matches the normal vector of the front face of the drawer.

Data Port Name	Port Type	Object Type
target_grasp_pose	Input	geometry_msgs::msg::PoseStamped
grasp_rotation_z_radians	Input	double
grasp_pose	Output	geometry_msgs::msg::PoseStamped
screw_origin_pose	Output	geometry_msgs::msg::PoseStamped
screw_axis_pose	Output	geometry_msgs::msg::PoseStamped

GetHingeAxisFromSurfaceSelection

Arrange input poses into the order expected by the affordance template behavior.

Given three input poses generated from surface-normal calculation of a point cloud, where the first represents the location of the door handle and the second and third are points on the axis of the hinge, determine which hinge axis pose should be the origin of the hinge vector so that a positive rotation about the hinge axis opens the door towards the user's viewpoint.

Assumes that the Z+ axis of the two poses on the hinge axis are oriented facing into the surface of the door.

The direction of the door relative to the surface normal is determined by calculating the vector cross product of the vector from the hinge origin pose to the grasp pose and the vector from the hinge origin pose to the hinge axis pose, and then calculating the dot product of the resulting vector and the Z-axis of the hinge origin pose. If the dot product is positive, the hinge origin and hinge axis poses are correctly ordered. If the dot product is negative, the hinge origin and hinge axis poses need to be reversed for positive door rotation to open the door towards the viewpoint.

Data Port Name	Port Type	Object Type
target_grasp_pose	Input	geometry_msgs::msg::PoseStamped
hinge_axis_pose_start	Input	geometry_msgs::msg::PoseStamped
hinge_axis_pose_end	Input	geometry_msgs::msg::PoseStamped
grasp_pose	Output	geometry_msgs::msg::PoseStamped
screw_origin_pose	Output	geometry_msgs::msg::PoseStamped
screw_axis_pose	Output	geometry_msgs::msg::PoseStamped

IsUserAvailable

Checks for the presence of a user interface by checking if the /trajectory_bridge ROS node exists.

Data Port Name	Port Type	Object Type
n/a	n/a	n/a

WaitForUserTrajectoryApproval

Takes a shared pointer to an MTC Solution object via an input data port, and publishes the lowest-cost trajectory in that Solution on MTC's /solution introspection topic. Creates a SetBool service server on the /execute_behavior_solution topic and waits to receive a request containing data: true before succeeding.

Data Port Name	Port Type	Object Type
solution	Input	std::shared_ptr<const moveit::task_constructor::SolutionBase>

RetrieveWaypoint

Given a named waypoint, sends a service request to the Agent WaypointManager to retrieve the joint state associated with that waypoint.

Data Port Name	Port Type	Object Type
waypoint_name	Input	std::string
waypoint_joint_state	Output	sensor_msgs::msg::JointState

WaitForDuration

Wait for a specified duration before succeeding.

Data Port Name	Port Type	Object Type
delay_duration	Input	double

Teleoperate

This is a special Behavior to run human-in-the-loop teleoperation using MoveIt Servo through the Objective Server.

When started, this Behavior will run INDEFINITELY until it is halted. This will happen either when the root node of the behavior tree is halted as the Objective is canceled, or when this Behavior's parent control node halts it. When this Behavior first transitions from IDLE to RUNNING, it starts and unpauses Servo control using the services advertised by the Servo server node. While this Behavior is RUNNING, it subscribes to JointJog and TwistStamped command messages that originate in the user interface, and republishes these messages to the command topics advertised by the Servo server node. When this Behavior is halted, it pauses Servo control using the server's services.

Data Port Name	Port Type	Object Type
controller_name	Input	std::string

Vision Behaviors

DetectApriltags

Detects AprilTag markers from an image.

Data Port Name	Port Type	Object Type
image	input	sensor_msgs::msg::Image
camera_info	input	sensor_msgs::msg::CameraInfo
parameters	input	YAML::Node
detections	output	moveit_studio_vision_msgs::msg::ObjectDetectionArray

GetCameraInfo

Captures camera information and makes it available on an output port.

Data Port Name	Port Type	Object Type
topic_name	input	std::string
message_out	output	sensor_msgs::msg::CameraInfo

GetDetectionPose

Gets the stamped pose of an object detection given a label or ID, if one exists.

The target_id and target_label ports define the match criteria for the detection. A target_id of -1 denotes any ID is allowed. Similarly, a target_label that is an empty string denotes any label is allowed.

If multiple detections that match the criteria exist in the list, return the first one.

Data Port Name	Port Type	Object Type
detections	input	moveit_studio_vision_msgs::msg::ObjectDetectionArray
target_id	input	int
target_label	input	std::string
detection_pose	output	geometry_msgs::msg::PoseStamped

GetDoorHandle

Calculates the pose, length, and width of a door handle. By convention, the z-axis of target_handle_pose is aligned with the handle's axis of rotation, and he x-axis points along the handle toward the door hinge.

Data Port Name	Port Type	Object Type
point_cloud	input	sensor_msgs::msg::PointCloud2
handle_pivot_pose	input	geometry_msgs::msg::PoseStamped
handle_tip_pose	input	geometry_msgs::msg::PoseStamped
target_output_frame_id	input	std::string
target_handle_pose	output	geometry_msgs::msg::PoseStamped
target_handle_z_offset	output	double
target_handle_length	output	double

GetImage

Captures an image and makes it available on an output port.

Data Port Name	Port Type	Object Type
topic_name	input	std::string
message_out	output	sensor_msgs::msg::Image

GetPointCloud

Captures a point cloud and makes it available on an output port.

Data Port Name	Port Type	Object Type
topic_name	input	std::string
message_out	output	sensor_msgs::msg::PointCloud2

PublishPointCloud

Publishes a point cloud on a ROS topic (typically used for debugging purposes).

Data Port Name	Port Type	Object Type
point_cloud_topic	Input	std::string
point_cloud	Input	sensor_msgs::msg::PointCloud2

SaveImageToFile

Save the contents of an image on the blackboard to a file. Filename will follow the syntax of file_prefix_YYYYMMDD_HHMMSS.png.

Data Port Name	Port Type	Object Type
file_path	input	std::string
file_prefix	input	std::string
image	input	sensor_msgs::msg::Image

SavePointCloudToFile

Save the contents of a point cloud on the blackboard to a pcd file using the pcl::PointXYZRGB point type. Filename will follow the syntax of file_prefix_YYYYMMDD_HHMMSS.pcd.

Data Port Name	Port Type	Object Type
file_path	input	std::string
file_prefix	input	std::string
point_cloud	input	sensor_msgs::msg::PointCloud2