MoveIt Task Constructor (MTC) in MoveIt Pro

Overview

MoveIt Task Constructor (MTC) decomposes a complex manipulation job into small, composable stages that exchange rich state snapshots. Instead of relying on a single long motion plan, MTC explores a graph of alternatives and keeps full planning-scene diffs for every branch. In MoveIt Pro, you assemble that graph from Behavior Tree nodes in the SetupMTC... family, then call PlanMTCTask and ExecuteMTCTask to solve and run the task as a single, coherent pipeline.

Why graph-based planning matters

Consider moving a cup of liquid through a cluttered workcell. A grasp that looks collision-free during approach might violate constraints later in the transfer. With MTC you can sample multiple grasp candidates, carry each through approach / lift / transfer / place, and let later stages prune any branch that cannot keep the cup upright. When the workspace changes mid-run, a MoveIt Pro Behavior Tree can immediately re-run the MTC planning subtree, so you only recompute the parts that matter while the rest of the objective keeps executing.

Stages and interface states

Every pipeline is a directed graph of stages linked by InterfaceState objects. An InterfaceState is a full PlanningScene snapshot (robot state, world objects, Allowed Collision Matrix, and inherited properties), which lets each branch plan with the exact context it needs.

Stage type	What it does	Example behaviors
Generator	Produces new interface states without an incoming connection.	SetupMTCCurrentState creates a stage matching the current Planning Scene; SetupMTCFromSolution uses from the final scene of a previous solution.
Propagator	Transforms one state into another, often by planning motion or editing the scene.	SetupMTCMoveAlongFrameAxis is a stage for Cartesian approaches; SetupMTCAttachObject edits the planning scene at execution time.
Connector	Bridges two existing states by finding a motion between them.	SetupMTCConnectWithProRRT adds a Connect (free-space motion) stage using the MoveIt Pro RRTConnect planner.

Wrappers, containers, and cost

• Wrappers post-process child results. SetupMTCBatchPoseIK bundles a GeneratePoses stage and a ComputeIK wrapper to turn pose samples into IK-valid joint states.
• Containers (Serial, Alternatives, Fallbacks, Merger) let you compose stages hierarchically. The default task root is a SerialContainer, so Behaviors simply append stages in order; custom C++ stages can use Alternatives or Fallbacks when you need branching.
• Cost: Every stage contributes a scalar cost. MTC sorts solutions by total cost, and PlanMTCTask selects the lowest-cost solution. The ProRRT planner exposes an explicit seed, giving you repeatable freespace plans when you keep the inputs and seed constant.

What each stage sees: PlanningScene and collisions

Stages receive the full PlanningScene they operate on, including allowed-collision overrides. Propagators can emit scene diffs that attach objects, toggle collision permissions, or reposition world geometry. Behaviors like SetupMTCAttachObject and SetupMTCUpdateObjectCollisionRule rely on that to temporarily relax contact constraints during a grasp and then restore the baseline scene.

MTC inside a MoveIt Pro Behavior Tree

MoveIt Pro ships with a set of Behaviors for the entire lifecycle: initialize, populate, plan, and execute a task.

1. Initialize the task

InitializeMTCTask creates a fresh moveit::task_constructor::Task, loads the robot model, and stores controller and monitoring defaults inside the task's trajectory_execution_info property. The Behavior writes the task handle to the blackboard so downstream Behaviors can append stages.

2. Seed the starting point

• SetupMTCCurrentState inserts a CurrentState stage, capturing the live robot and planning scene.
• SetupMTCFromSolution rebuilds the final scene of a prior solution before adding new stages. Combined with the joint trajectory admittance controller's trajectory stitching support, you can execute the first segment while planning the next one and seamlessly stitch them together.

3. Add stages with SetupMTC... behaviors

Common building blocks include:

• Motion: SetupMTCPlanToPose, SetupMTCMoveAlongFrameAxis, and SetupMTCConnectWithProRRT.
• IK & sampling: SetupMTCBatchPoseIK and SetupMTCPathIK for Cartesian-following motions.
• Scene edits: SetupMTCAttachObject, SetupMTCDetachObject, SetupMTCUpdateObjectCollisionRule, or SetupMTCUpdateGroupCollisionRule when the grasp needs temporary collision allowances or objects need to be attached top the end-effector, robot base, or world.

Order matters. For example, a grasp pipeline usually samples poses, validates IK, reserves collisions for fingertips, closes the gripper, and finally attaches the object so only valid branches flow into downstream motion stages. Note that SetupMTC... behaviors will add to the MTC task when ticked by the behavior tree, but motion planning does not happen until the PlanMTCTask behavior is ticked, and planning scene changes do not happen until that particular stage of the MTC solution is executed. If behaviors unrelated to MTC task setup, planning, or execution are ticked while the MTC task is being set up or planned, they will execute immediately.

4. Plan

PlanMTCTask performs the motion planning, validates the result through, and publishes the best solution to its solution output port. During planning, MTC introspection automatically publishes to the description, statistics, and solution topics so the Solution Manager and visualization plugins can display every attempt.

5. Execute

ExecuteMTCTask sends the chosen solution to the execute_task_solution action. As each sub-trajectory runs, attached scene_diff messages are applied to the global planning scene, so scene edits from ModifyPlanningScene stages take effect exactly when their motion executes.

Debugging and introspection

If a pipeline fails, or simply to understand why a branch was selected, you can rely on the built-in introspection topics and services:

1. Task outcomes - Subscribe to statistics (moveit_task_constructor_msgs/msg/TaskStatistics). Each message includes the task_id, per-stage success and failure counts, and timing information. In a running MoveIt Pro system you can inspect one snapshot with ros2 topic echo /statistics --once.
2. Stage metadata - description (moveit_task_constructor_msgs/msg/TaskDescription) lists every stage's ID, parent, and configured properties.
3. Per-solution details - Call get_solution_<task_id> (moveit_task_constructor_msgs/srv/GetSolution) with a solution_id reported in the statistics message. The response contains ordered sub_trajectory entries, each annotated with the originating stage_id, human-readable comments (for example "EEF in collision with table"), the motion trajectory, and the accumulated scene_diff.

Writing your own SetupMTC... behaviors (C++ primer)

1. Read the task from the Behavior's input port.
2. Add stage(s) that capture a single responsibility like IK solving, motion planning, or scene edits-and configure them with inherited properties. Use containers to make multi-stage behaviors.
3. Expose tunable ports (groups, frames, tolerances) so the behavior can be used in multiple ways.
4. Test in plan-only mode. Run PlanMTCTask in your objective and confirm the resulting solution shows your stage names, comments, and costs in the published introspection data before enabling execution. Add a WaitForUserTrajectoryApproval to preview the motion in the MoveIt Pro UI.

Summary

Concept	MoveIt Pro behavior
Create task and global settings	InitializeMTCTask
Seed initial state	SetupMTCCurrentState or SetupMTCFromSolution
Add stages	Any SetupMTC... behavior (motion, IK, scene edits)
Plan	PlanMTCTask -> lowest-cost solution on the blackboard; publishes introspection data
Execute	ExecuteMTCTask -> sends solution to execute_task_solution
Debug	statistics/description topics + get_solution_<task_id> service

Together, these behaviors turn MoveIt Pro's MTC integration into a repeatable, reactive, and debuggable manipulation pipeline for demanding production workflows.