2. Perception & Advanced Motion Planning

🕒 Duration: 1-2 hours

Tutorial Overview

This tutorial dives into the advanced perception and motion planning capabilities of MoveIt Pro. Building on the foundational skills from Tutorial 1, you'll learn how to use computer vision tools like AprilTags, point cloud registration, and segmentation to enable your robot to perceive its environment. You'll also explore how to structure complex task plans using behavior trees and leverage the MoveIt Task Constructor (MTC) to handle sophisticated manipulation tasks. Whether you're stacking blocks, identifying parts, or debugging planning failures, this tutorial will guide you through practical workflows and best practices.

Pre-reqs

You should have already installed MoveIt Pro. We will assume you have already completed Tutorial 1 and have basic familiarity with the software.

Start MoveIt Pro

Launch the application using:

moveit_pro run -c lab_sim

Stacking Blocks with AprilTags

In this exercise we will build a new objective that will automatically detect and stack the blocks. To keep the computer vision simple to start with, we will use AprilTag fiducials to detect the 3D position of the blocks.

info

AprilTags are a type of fiducial marker system commonly used in robotics and computer vision. They consist of black-and-white square patterns that can be easily detected and uniquely identified by a camera. In robotics, AprilTags are often used for precise localization and pose estimation of objects in the environment. AprilTags are particularly useful in applications requiring low-cost, reliable visual tracking without the need for complex sensors.

At a high level, this is the overall flow of the objective:

1. Initialize robot and scene
2. Get object pose using AprilTag-based computer vision
3. Pick from pose
4. Place object

You’ll create four subtrees for this objective based on the flow above. Subtrees are behavior trees that can be instantiated inside of other behavior trees.

info

Subtrees are modular components within a larger behavior tree that encapsulate a set of actions or decision logic. By allowing one behavior tree to reference another, subtrees enable greater reusability, organization, and maintainability in complex robotic applications. They help abstract away lower-level details, reduce duplication of nodes, and make it easier to manage common sequences or behaviors across different objectives. In MoveIt Pro, subtrees are especially useful for structuring sophisticated task plans in a clean and scalable way.

“Initialize Robot and Scene” Subtree

First, create a new objective called Stack Blocks. If you’re unsure how to create a new objective, please refer to the Tutorial 1.

The first subtree in this application consists of four steps:

1. Open Gripper
2. Look at Table
3. Clear Snapshot
4. Take wrist camera snapshot

To start creating the subtree, remove the AlwaysSuccess behavior from your new empty objective and drag a second Sequence behavior into your new objective:

tip

There are several approaches to create subtrees, and we will demonstrate them in the following sections.

Next, choose Create Subtree from the icons above the newly added sequence.

Name the subtree Initialize Robot and Scene. Note the Subtree-only Objective is checked by default. This means that this objective can only be run as a subtree within another objective, not as a stand-alone objective.

Your Stack Blocks Objective should now look like this:

Next, edit the subtree using the edit pencil icon.

Once inside that subtree, add the following four (sub-)subtrees to the Initialize Robot and Scene subtree as pictured:

When finished, run the Stack Blocks objective. You should see the robot move to the Look at Table waypoint and then take a snapshot, adding a point cloud to the Visualization pane.

“Get Object Pose” Subtree

The next step in the Stack Blocks objective is to get the pose of a block, by using the block's AprilTags to locate them.

First, edit the Stack Blocks objective.

Add a new Sequence to the objective.

Next, add the following behaviors and ports. The ports listed below are the most relevant and important for this objective to work, so they are listed here even if the value is unchanged from the default.

1. LoadObjectiveParameters - loads parameters from a Yaml file a. config_file_name: apriltag_detection_config.yaml b. parameters: {parameters}

2. GetCameraInfo - Gets the camera information from a ROS topic a. topic_name: /wrist_camera/camera_info - note that this is different from the default port value b. message_out: {camera_info}

3. GetImage a. camera: /wrist_camera/color - note that this is different from the default port value

4. DetectAprilTags a. detections: {detections} b. parameters: {parameters} c. camera_info: {camera_info}

5. GetDetectionPose a. detections: {detections} b. target_id: -1 c. detection_pose: {detection_pose}

6. TransformPoseFrame a. input_pose: {detection_pose} b. target_frame_id: world c. output_pose: {output_pose}

7. VisualizePose a. pose: {output_pose}

   

tip

If you want to know more about how each behavior works, see the built-in descriptions when you hover over a behavior in the sidebar of MoveIt Pro.

Your complete subtree should look like this:

Now run your Stack Blocks objective, and you should see a 3-axis colored pose marker appear on the detected block:

Great job so far! Now we will convert this into a subtree:

Edit the Stack Blocks objective again, and convert your new sequence to a subtree called Get Object Pose using the hover-over button.

note

In the previous example, we created a subtree first and then populated it with behaviors, but that isn’t required. You can convert any Sequence node to a subtree at any time, which enables you to reuse as much as possible!

Port Remapping

Next we will learn about port remapping.

info

Port remapping in behavior trees refers to the process of connecting input and output ports of behavior tree nodes to specific data sources or targets within the tree. It allows you to reuse generic nodes by dynamically assigning their input/output variables (or "ports") to different values or blackboard entries, depending on the context in which they're used. This makes behavior trees more flexible and modular, enabling the same node or subtree to function differently across various parts of the tree without changing its internal logic.

Edit the Get Object Pose subtree by clicking on the edit icon.

Choose the root node called Get Object Pose.

In the popup sidebar, add an output port, to allow the the sharing of the AprilTag pose that was detected.

Name the port output_pose, and place it on the parent tree’s blackboard as {object_pose}.

The Stack Blocks objective should now look like this.

Pick from Pose

The next subtree you will build is going to pick the block specified in the {object_pose} blackboard variable. This step requires using MoveIt Task Constructor (MTC) behaviors.

info

What is MoveIt Task Constructor (MTC)?

MTC enables you to break down complex planning tasks into multiple interdependent steps for use by motion planners. For example, to pick an object a robot must plan for multiple dependent goals in an exact order:

1. Open the gripper
2. Move to a pre-grasp position
3. Approach the object
4. Close the gripper
5. Lift the object

MTC is used to plan a solution to the multi-task problem, given an object’s pose, and execute the plan.

Benefits of MoveIt Pro Behaviors

MoveIt Pro provides a library of behaviors to make using MTC easier. They provide a simplified way to create, plan, and execute MTC tasks. You can:

• Use Behaviors to set up and extend the task with common building blocks
• Choose from a variety of building blocks
• Reuse components of existing tasks much easier than writing low-level C++

Relevant MTC behaviors

There are many built-in MTC behaviors, you will use these to build your picking objective:

• InitializeMTCTask creates a task object and initializes the common global properties like trajectory execution info. The task object is then stored on the blackboard to be modified by following behaviors.
• SetupMTCCurrentState takes the created task and sets up a generator stage corresponding to the current state as the task's start state.

danger

The following SetupMTCPickObject behavior will be removed in 8.0 and replaced with a subtree in the example_ws.

• SetupMTCPickObject is our custom Behavior which adds the stages required to describe the pick-planning problem to the task.
• PlanMTCTask calls the plan() function of the given MTC task and stores the solution to the blackboard.
• ExecuteMTCTask reads an MTC solution from the blackboard and executes it. :::

Let's dive in. First edit your Stack Blocks objective.

Add the Pick from Pose subtree.

Confirm that the port remapping value for {grasp_pose} on this node is {object_pose}.

The contents of the Pick from Pose subtree look like this:

Place the object

Finally, put the finishing touches on your Stack Blocks objective by adding these existing, pre-built subtrees:

1. Look at Table
2. Place Object
3. Open Gripper

note

Place Object is an example subtree that will put something in the robot's end effector at a pre-defined waypoint. Adding the Look at Table subtree before placing the object is needed so that the planner will approach the placement point from above.

Your finished Stack Blocks objective should look like this:

Run the objective and you should see the robot pick up a block, and move to the Look at Table waypoint, then plan a placement trajectory and ask for approval:

info

User Approval

The approval step is optional but can be nice for visualizing what the robot is about to do. MoveIt Pro provides various built-in behaviors for user interaction for applications that require human in the loop. In our example, your Place Object subtree includes this capability by using the Wait for Trajectory Approval if User Available subtree, which checks if there is a UI attached, and if so asks the user to approve the trajectory.

Once you approve the trajectory, the robot should stack the blocks like this!

tip

You won’t see the blocks being stacked in the “Visualization” pane, it is only shown in the simulated camera feeds, for example under /scene_camera/color and /wrist_camera/color.

Now let's learn how to debug planning failures.

Add a Breakpoint

To debug what is occurring in an objective, insert a breakpoint using a BreakpointSubscriber.

Edit your Stack Blocks objective, and add a BreakpointSubscriber in the middle.

Run the objective, and you should see the robot pick the cube, then the objective will wait at the breakpoint until the Resume button in the top right corner is pressed.

At this point, you can move the visualization to get a better look at the scene, and if there was a real failure, you could determine the root cause before resuming the objective.

Press Resume to finish running the objective.

Resetting the Simulation

In the previous section, the robot was moving the blocks around on the table. If your blocks get into a state you don’t want them in you have two options for resetting the scene. First, you can restart MoveIt Pro. The second option is to run the MuJoCo viewer to reset the scene.

info

Performance Note Running the MuJoCo viewer can impact system performance, and may not be feasible for lower-powered systems.

To enable the MuJoCo viewer, exit MoveIt Pro using CTRL-C, then follow this MuJoCo configuration guide under the section Optional Params -> MuJoCo Viewer. For the lab_sim configuration, the ros2_control tag can be found in the lab_sim/description/picknik_ur.xacro file.

Re-launch MoveIt Pro and the MuJoCo viewer should launch beside MoveIt Pro.

Within the viewer, you can move objects manually by double-clicking the object you want to move, and then using the following:

• Lift and move: CTRL+Right Mouse
• Drag horizontally: CTRL+SHIFT+Right Mouse

You can also reset the simulation using the Reset button on the bottom left menu in the viewer.

More Perception Objectives

In the Stack Blocks objective, you used AprilTags to locate the blocks on the table so that you could stack them. There are other perception capabilities within MoveIt Pro, for example, Point Cloud Registration and Point Cloud Segmentation. In this section, you’ll run some objectives that demonstrate those capabilities and learn how they work.

Point Cloud Registration

Point cloud registration is the process of localizing an object within a point cloud, given a CAD mesh file as an input. This is used in robotics for locating a part within a workspace, as an input to manipulation flows like polishing and grinding parts.

Registering Point Clouds in MoveIt Pro

Typically, point cloud registration starts with an initial guess pose, which might be from an ML perception model, or based on where an object should be by the design of the robot workspace. This initial guess pose should be close to the object being registered, but not exact. The registration process then will find the exact pose using one of several algorithms, such as Iterative Closest Point (ICP).

In Moveit Pro, the RegisterPointClouds behavior does this matching, given three inputs: 1) an initial guess point cloud, 2) the maximum ICP correspondence distance, and 3) a maximum number of iterations. The output is called the “registered pose”, and is the pose relative to the initial guess point cloud. The following screenshot shows an example usage of this behavior:

Try it yourself

Select the Build tab. In the Application - Advanced Examples section, select the Register CAD Part objective to begin editing.

You’ll see the following overall flow:

1. Move the camera on the end effector to look at the area of interest
2. Create an initial guess pose (CreateStampedPose)
3. Load a mesh point cloud at guess pose (visualized as the red point cloud)
4. Get the camera point cloud
5. Register (using ICP) the initial guess point cloud to the actual camera point cloud (visualized as the green point cloud)

Next, run the objective, and you should see two point clouds appear, first a red one above the table (the initial guess), then a green one that matches the closest cube to the initial guess.

Now edit the objective, and modify the guess pose by changing the x, y, and z values in the CreateStampedPose behavior to (0.2, 0.75, 0.6).

Run the objective again and see how the new guess will register a different cube.

As an additional hands-on exercise, you can replace the Get Object Pose subtree in the Stack Blocks objective with this Register CAD Part objective.

Point Cloud Segmentation

Point cloud segmentation is the process of grouping individual points in a point cloud based on shared characteristics or belonging to the same object. The Segment Point Cloud from Clicked Point objective demonstrates how to segment an object from a point cloud. It uses the GetMasks2DFromPointQuery behavior which calls a machine learning model, called the Segment Anything Model (SAM) to segment the object.

The objective:

• Prompts the user to click an object in the color wrist camera image
• Creates a 2D mask of the object using the Segment Anything Model (SAM)
  • The 2D mask is the x,y location of the object, and all the color pixels within the object
• Converts the 2D mask to a 3D mask, mapping the object into the point cloud
• Applies the 3D mask to the point cloud, removing everything except the chosen object

In the Application - Advanced Examples section, locate and run the Segment Point Cloud from Clicked Point objective. Click on the objects in the camera pane and it will segment out the point cloud for those objects.

Select the burner on the right side and you will see the visualization pane update with only that object in the point cloud.

You can use the Clear Snapshot objective to clear the snapshot, or run the objective again and select another object to segment.

For another hands-on exercise, you can use GetGraspableObjectsFromMasks3D to convert the 3D mask to a graspable object, then ExtractGraspableObjectPose to get a pose that can be used with your existing Pick from Pose subtree.

Using the MTC Debugger

In order to demonstrate the MTC debugger, we will leverage keep-out zones. Create a new keep out zone, but in the popup box set the Size to 1m so that a large cube is added.

Run Teleoperate, and choose the Workspace Right waypoint. You should see a message that the motion planning failed.

This is expected since you added a keep-out zone so large that the robot could not complete the command without violating its collision checker.

Next, try to use the Interactive Marker Teleoperation to move the robot arm into the red cube area. You should see a message that PlanMTCTask has failed.

To debug this further, zoom into the behavior tree and find the failing node. It will be highlighted in red.

Entering Debug Mode

Click on the bug symbol to open the MTC Debugger and see what failed.

Highlight the failing stage to see a comment that explains why it failed. In this case, it says “eef in collision” which means the robot’s end effector is in collision with the keepout zone.

Choose Return to Objective in the top right corner to close the MTC Debugger view.

Summary

By completing this tutorial, you've gained hands-on experience with powerful tools in MoveIt Pro for perception-driven manipulation. You've learned how to integrate fiducial detection, point cloud processing, and modular behavior trees to create reusable, intelligent robotic objectives. You also explored debugging techniques and simulation reset tools that support more robust development workflows.

🎉 Congratulations, we're now ready to move to the next tutorial!