MoveIt Studio Behavior  2.0.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.

moveit_studio::behaviors::CoreBehaviorsLoader

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
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>

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>

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>

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

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

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

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

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

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

GetPointCloud

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

Optionally takes a string uuid which can be used to identify the requester, and makes it available on an output port. The parameter is "optional", if it unset then no output will be forwarded. NOTE: No validation is done on the value of the UUID, so any string is provided (including the empty string) it will be set on the output port.

Data Port Name Port Type Object Type
target_point_cloud_topic input std::string
uuid input std::string (optional)
point_cloud output sensor_msgs::msg::PointCloud2
point_cloud_uuid output std::string

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

UpdatePlanningScene

Given a point cloud from the specified sensor, will sanitize the point cloud according to that sensor's params and publish the result to the relevant topic to update the occupancy map. It will also convert the resulting pointcloud to ASCII PCD format and publish it to the specified pcd_topic.

The UUID parameter can be used to track the pointcloud request through to other behaviors, if required. It is an optional input port, and if it unset then the published message will have an empty string in its UUID field. NOTE: No validation is done on the value of the UUID, so any string is provided (including the empty string) it will be set on the output port.

Data Port Name Port Type Object Type
point_cloud Input sensor_msgs::msg::PointCloud2
sensor_name Input std::string
point_cloud_uuid Input std::string (optional)
pcd_topic Input std::string