MoveIt Studio Behavior  2.5.2
Core Behaviors for the MoveIt Studio Application
moveit_studio_behavior

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

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

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

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

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

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