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

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

Install warehouse_ros_mongo

First, we need to install the warehouse_ros_mongo package.

The warehouse_ros_mongo package is a database tool for ROS 2 that allows you to save, retrieve, and manage important information about your robot’s positions and configurations, acting like a digital memory bank for your robot.

Open a terminal window, and let’s install some system-level dependencies needed by the warehouse_ros_mongo package.

sudo apt-get install gnupg curl

We will now install mongo_db. Follow these official instructions.  We will also walk through it together now (we are doing version 8.0 now below, so the exact version you need to download will likely change in the future).

cd ~/Downloads
curl -fsSL https://www.mongodb.org/static/pgp/server-8.0.asc | \
   sudo gpg -o /usr/share/keyrings/mongodb-server-8.0.gpg \
   --dearmor
echo "deb [ arch=amd64,arm64 signed-by=/usr/share/keyrings/mongodb-server-8.0.gpg ] https://repo.mongodb.org/apt/ubuntu noble/mongodb-org/8.0 multiverse" | sudo tee /etc/apt/sources.list.d/mongodb-org-8.0.list
sudo apt-get update
sudo apt-get install -y mongodb-org

Now start the MongoDB service.

sudo systemctl start mongod

Now run this command to enable MongoDB to start when you boot up your machine:

sudo systemctl enable mongod

Verify everything is up and running fine:

sudo systemctl status mongod

If MongoDB is running, it should show the service as active.

1-verify-mongodb-up-and-running

Press q to quit.

Now let’s install the warehouse_ros_mongo package.

Open a terminal window, and navigate to your ROS 2 workspace’s source folder:

cd ~/ros2_ws/src
git clone https://github.com/moveit/warehouse_ros_mongo.git -b ros2
cd warehouse_ros_mongo/
git reset --hard 32f8fc5dd245077b9c09e93efc8625b9f599f271
cd ~/ros2_ws
rosdep update
rosdep install --from-paths src --ignore-src -r -y

Type your password and press Enter to install any missing packages.

colcon build
source ~/.bashrc (OR source ~/ros2_ws/install/setup.bash if you haven’t set up your bashrc file to source your ROS distribution automatically with “source ~/ros2_ws/install/setup.bash”)

Install the MoveIt Task Constructor Package

Open a new terminal window, and type:

cd ~/ros2_ws/src
git clone https://github.com/moveit/moveit_task_constructor.git -b jazzy
cd moveit_task_constructor

So that we make sure we get a specific version of the package on GitHub, we will add the latest commit hash, which I found on GitHub.

git reset --hard 9ced9fc10a15388224f0741e5a930a33f4ed6255
cd ~/ros2_ws
rosdep install --from-paths src --ignore-src -r -y

Type your password and press Enter to install any missing packages.

cd ~/ros2_ws/
colcon build

You can ignore this error:

--- stderr: rviz_marker_tools                                                                                            
rviz_marker_tools: You did not request a specific build type: Choosing 'Release' for maximum performance
---

Now source your workspace.

source ~/.bashrc

You will probably see some build errors now. Build again:

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

Remember that you can safely ignore errors that look like this:

--- stderr: moveit_task_constructor_core                                 
lto-wrapper: warning: using serial compilation of 12 LTRANS jobs
lto-wrapper: note: see the ‘-flto’ option documentation for more information

Fix Other Issues

The pick and place demo works during the planning phase but fails during execution. This issue has been documented at this link. If you execute the plan via RViz, everything will work fine. If you execute it via the code, execution fails. The error you will see is this: “Stopping execution because the path to execute became invalid(probably the environment changed)”

To fix this issue, you need to go to this file:

cd ~/ros2_ws/src/moveit_task_constructor/core/src/
gedit storage.cpp

Change these lines:

if (this->end()->scene()->getParent() == this->start()->scene())
	this->end()->scene()->getPlanningSceneDiffMsg(t.scene_diff);
else
	this->end()->scene()->getPlanningSceneMsg(t.scene_diff);

To this:

this->end()->scene()->getPlanningSceneDiffMsg(t.scene_diff);

cd ~/ros2_ws/
colcon build
source ~/.bashrc (OR source ~/ros2_ws/install/setup.bash)

Congratulations! You have now successfully installed the MoveIt Task Constructor.

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

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!