How to Set Up the MoveIt 2 Task Constructor – ROS 2 Lyrical

ros2-jazzy-moveit-task-constructor

In this tutorial, I will guide you through the core concepts of the MoveIt Task Constructor for ROS 2. You will also install it on your computer.

The MoveIt 2 Task Constructor (MTC) is designed to make complex robot tasks easier to program and manage. 

At its core, the Task Constructor breaks big tasks into smaller, more manageable pieces called stages. These stages can be mixed and matched like building blocks. They can also share information with each other, making it easier to create smooth, coordinated robot actions.

Overall, the goal of the MTC is to give robot programmers a set of tools that make their job more efficient and help them create more reliable and adaptable robot behaviors.

Prerequisites

We will just focus on concepts in this tutorial. In future tutorials, you will learn how to build your own applications using the MTC. You can check out my sample code here on GitHub. Don’t worry. I will go through all that in a later tutorial.

MTC Core Concepts

The MoveIt Task Constructor (MTC) is built around three core concepts that work together in harmony to create a comprehensive system for robot task planning.

Task

A Task is like a big plan for a robot. It is made up of a series of smaller steps that the robot needs to do to complete a job. 

Think of a task as a recipe for the robot to follow. Each Task is built from a sequence of Stages, which are the individual steps or actions the robot needs to take.

Stage

A Stage is a single step within a Task. It is a specific action or calculation the robot needs to perform. 

For example, a Stage might be “move the arm to a certain position” or “open the gripper.” 

Stages can be simple or complex, and they work together to form the complete Task. 

Each Stage produces what’s called a SubSolution, which is the result of that particular step.

Planning Scene

In the previous tutorial, you had experience adding a collision object to the planning scene.

The Planning Scene is the virtual model of the robot’s world. It includes information about the robot itself, any objects in its environment, and any constraints or rules the robot needs to follow. 

When the robot is planning its movements or actions, MoveIt uses this Planning Scene to figure out what the robot can and cannot do. 

The Planning Scene gets updated as the robot moves through its Task, keeping track of changes in the environment.

Stage Types

Let’s take a closer look at stages in the MTC

The MTC uses different types of Stages to handle various aspects of task planning. These Stage types are designed to work together, providing a versatile toolkit for creating complex robot behaviors. The three main types of Stages in the MTC are Generator Stage, Propagator Stage, and Connector Stage. 

1-stage-types

Source: PickNik Inc.

Generator Stage

An MTC task starts with a Generator stage. This stage receives no input from adjacent stages. It doesn’t care what happens before it or after it.

The results that are computed in this stage are passed forward and backward.

Let’s look at some examples of Generator Stage types.

Examples

CurrentState is the most important generator stage. This is like taking a snapshot of where the robot is right now. It captures the robot’s current position, the state of its joints, and any objects it might be holding or interacting with. 

CurrentState generates the starting point for planning what the robot will do next.

GeneratePose is another type of generator stage. GeneratePose is about coming up with possible positions for the robot’s arm or gripper. 

For example, if the robot needs to pick up an object, GeneratePose might generate different ways the gripper could be positioned to grab the object. It’s like brainstorming potential positions for the robot.

Propagator Stage

A propagator stage is like a one way street. It receives an input from an adjacent stage, processes that input, and then generates an output for the next stage.

Examples

MoveTo is a propagating stage type because it takes the robot arm’s current state as input and produces a new state as output. 

For example, the MoveTo stage might start with the arm at rest and end with the arm reaching into a parts bin. 

MoveRelative is another propagating stage type because it advances the robot’s state incrementally. It takes the current arm position as input and outputs a new position based on a relative movement. 

For example, during the MoveRelative stage, the arm might move 10cm upward relative to where it currently is in the world, propagating the state forward by this small, relative change.

ModifyPlanningScene takes the current planning scene (the virtual model of the robot’s environment) as input, along with pre-programmed instructions for how to modify that scene (e.g. add or remove objects from the virtual environment). It then outputs an updated planning scene.

Connector Stage (i.e. Connectors)

Unlike Propagators, Connectors do not send results to other stages. Instead, they are like bridges that plan movements between two different arbitrary stages.

An example of a Connector Stage is finding a way to move the arm from its current position to a specific point.

Bringing It All Together

The three stage classes we mentioned above are called “primitive” stage classes. They are the basic building blocks for tasks in MoveIt 2.

Below is an example of how the stages might be structured for a sample pick and place task. The arrow and bar symbols represent the different stage types.

2-example-stages

Source: PickNik Inc.

Generator Stage (↕️):

  • This stage creates new possibilities.
  • It is like coming up with ideas for the robot to try.
  • Examples: Figuring out different poses the robot could use, or setting a specific position for the robot.

Propagator Stage (↓ or ↑ or both):

  • This stage takes an input and figures out what to do next.
  • It can work forward (↓), backward (↑), or both ways.
  • Examples: Planning how to move in a straight line, or checking if a movement is safe.

Connector Stage (||):

  • This stage links two different states or positions.
  • It’s like finding a path between where the robot is and where it needs to go.
  • Example: Planning how to move freely from a starting position to an end position.

So there you have it. Stages either generate, propagate, or connect.

Containers (grouping primitive stages to perform a task)

MoveIt Task Constructor (MTC) has a concept called Containers. A Container is like a box that can hold one or more stages or even other containers. It helps group related stages together and create a hierarchy or structure for complex tasks. 

There are three types of container stages: wrapper, serial, and parallel. Let’s go through each one.

3-containers

Source: PickNik Inc.

Wrapper Container

This container is used to modify or filter the solutions of a stage it contains. The wrapper modifies how a single task is performed without changing the core action.

Example: Wrap a pose generator with an IK (Inverse Kinematics) solver

  • A pose generator is a component that comes up with possible positions and orientations for the robot’s end-effector (like its hand or gripper) to grasp an object.
  • An IK (Inverse Kinematics) solver figures out how the robot’s joints should be positioned to achieve the specific end-effector (gripper) pose calculated in #1.
  • By wrapping the pose generator in step #1 with an IK solver in step #2, you’re creating a system that not only generates potential grasping poses but also immediately checks if the robot can actually achieve those poses, ensuring that only reachable and feasible grasping positions are considered.

Serial Containers

This container contains a sequence of stages that must be followed in order. 

Example:

  1. Approach the object (stage 1 – propagator)
  2. Generate the grasp pose (stage 2 – generator)
  3. Grasp the object (stage 3 – propagator)
  4. Lift the object (stage 4 – propagator)

The robot needs to complete all these steps in sequence to successfully complete the task.

Parallel Containers

This container allows for the simultaneous or alternative execution of multiple stages. It can include:

  • Alternative Stages:
    • Alternative Stages are different options to achieve the same goal, where only one needs to succeed. 
    • For example, you might have stages for picking an object with either the left hand or the right hand. The task will be successful if the robot can pick with either hand.
  • Fallback Stages:
    • Fallback Stages are backup options that are tried in order if earlier ones fail. For instance, you might have a default motion planner, and if that doesn’t work, it will try alternative planners. This ensures that even if the preferred method fails, there are other ways to complete the task.
  • Merger Stages:
    • Merger Stages allow multiple distinct actions to happen at the same time. 
    • For example, the robot could open its gripper while simultaneously moving its arm to a new position. This can make tasks more efficient by combining compatible actions.

Parallel Containers give flexibility in how tasks are performed, allowing for multiple approaches, backup plans, and simultaneous actions. This makes robotic tasks more adaptable and robust.

Example: Pick and Place

Have a look at this diagram and see how containers and stage types could be organized in a sample pick and place application.

4-have-a-look-pick-place

Source: PickNik Inc.

That covers the basic concepts of the MTC.

If you want to do further reading, check out this link at the official MoveIt 2 website.

Create a New Package

Let’s start our hands-on experience by creating a new package for our MoveIt 2 Task Constructor (MTC) code. Open a terminal and navigate to the mycobot_ros2 directory in your ROS 2 workspace:

cd ~/ros2_ws/src/mycobot_ros2/

Now, let’s create a new package using the ROS 2 command line tools:

ros2 pkg create --build-type ament_cmake \
--dependencies generate_parameter_library moveit_core moveit_ros_planning_interface rclcpp \
--license BSD-3-Clause \
--maintainer-name ubuntu \
--maintainer-email automaticaddison@todo.com \
--node-name alternative_path_costs \
mycobot_mtc_demos
cd ~/ros2_ws/
rosdep install --from-paths src --ignore-src -r -y

Type in your password, and install any missing dependencies.

Now build.

colcon build && source ~/.bashrc

Install the MoveIt Task Constructor

This part used to be a long slog. On Lyrical it is one command.

Open a terminal window, and type:

sudo apt-get install -y ros-${ROS_DISTRO}-moveit-task-constructor-core ros-${ROS_DISTRO}-moveit-task-constructor-msgs ros-${ROS_DISTRO}-moveit-task-constructor-visualization

That is the whole install. The MoveIt Task Constructor is a regular package now, so you no longer download the source, pin it to a commit, build it, and then patch two files by hand to get it working.

If you followed the older version of this tutorial, you may remember installing MongoDB and a package called warehouse_ros_mongo along the way. You can skip all of that. It was only there for a database feature none of these tutorials use, and the library it depended on is not part of Ubuntu 26.04 anymore.

Let’s make sure it worked. Type:

ros2 pkg list | grep moveit_task_constructor

You should see three packages listed: moveit_task_constructor_core, moveit_task_constructor_msgs, and moveit_task_constructor_visualization.

Configure Your MTC Package

Edit package.xml

cd ~/ros2_ws/src/mycobot_ros2/mycobot_mtc_demos/
gedit package.xml

Your dependencies should look like this:

  <depend>generate_parameter_library</depend>
  <depend>moveit_core</depend>
  <depend>moveit_ros_planning_interface</depend>
  <depend>moveit_task_constructor_core</depend>
  <depend>rclcpp</depend>

cd ~/ros2_ws/
rosdep install --from-paths src --ignore-src -r -y

Create a Launch File for the Demos

Move inside the package.

cd ~/ros2_ws/src/mycobot_ros2/mycobot_mtc_demos/

Create a folder named launch.

mkdir launch

Now let’s add a launch file:

cd launch
touch mtc_demos.launch.py

Add this code.

Save the file, and close it.

Create an RViz Config File

cd ~/ros2_ws/src/mycobot_ros2/mycobot_mtc_demos/
mkdir rviz
cd rviz
gedit mtc_demos.rviz

Add this code.

Save the file, and close it.

Edit CMakeLists.txt

Let’s add the new folders.

cd ~/ros2_ws/src/mycobot_ros2/mycobot_mtc_demos

Open CMakeLists.txt.

Add this block:

# Copy necessary files to designated locations in the project
install (
  DIRECTORY launch rviz
  DESTINATION share/${PROJECT_NAME}
)

Add Demonstration Code

cd ~/ros2_ws/src/mycobot_ros2/mycobot_mtc_demos/src/

Add all the files you see in this directory into the src folder.

Edit CMakeLists.txt

Now let’s edit CMakeLists.txt.

cd ~/ros2_ws/src/mycobot_ros2/mycobot_mtc_demos/
gedit CMakeLists.txt

Add this code.

Save the file, and close it.

Build the Package

Now let’s build.

cd ~/ros2_ws/
colcon build
source ~/.bashrc

Add a Quick Launch Script

cd ~/ros2_ws/src/mycobot_ros2/mycobot_bringup/scripts
touch mycobot_280_mtc_demos.sh

Add this placeholder code:

#!/bin/bash
# Single script to launch the mycobot with Gazebo, RViz, and MoveIt 2

cleanup() {
    echo "Cleaning up..."
    sleep 5.0
    pkill -9 -f "ros2|gazebo|gz|nav2|amcl|bt_navigator|nav_to_pose|rviz2|assisted_teleop|cmd_vel_relay|robot_state_publisher|joint_state_publisher|move_to_free|mqtt|autodock|cliff_detection|moveit|move_group|basic_navigator"
}

# Set up cleanup trap
trap 'cleanup' SIGINT SIGTERM

echo "Launching Gazebo simulation..."
ros2 launch mycobot_gazebo mycobot.gazebo.launch.py \
    load_controllers:=true \
    world_file:=pick_and_place_demo.world \
    use_camera:=true \
    use_rviz:=false \
    use_robot_state_pub:=true \
    use_sim_time:=true \
    x:=0.0 \
    y:=0.0 \
    z:=0.05 \
    roll:=0.0 \
    pitch:=0.0 \
    yaw:=0.0 &

sleep 15
ros2 launch mycobot_moveit_config move_group.launch.py \
    rviz_config_file:=mtc_demos.rviz \
    rviz_config_package:=mycobot_mtc_demos &

echo "Adjusting camera position..."
gz service -s /gui/move_to/pose \
    --reqtype gz.msgs.GUICamera \
    --reptype gz.msgs.Boolean \
    --timeout 2000 \
    --req "pose: {position: {x: 1.36, y: -0.58, z: 0.95} orientation: {x: -0.26, y: 0.1, z: 0.89, w: 0.35}}"

# Keep the script running until Ctrl+C
wait

Save the file, and close it.

Now add an alias called mtc_demos to your bashrc file:

echo "alias mtc_demos='bash ~/ros2_ws/src/mycobot_ros2/mycobot_bringup/scripts/mycobot_280_mtc_demos.sh'" >> ~/.bashrc && source ~/.bashrc

That’s it! You are now ready to start taking a closer look at the demo applications. We will do just that in the following tutorials. Keep building!