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
- You have completed this tutorial: Add and Plan Around Objects Using MoveIt 2 – ROS 2 Jazzy.
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.
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.
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.
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:
- Approach the object (stage 1 – propagator)
- Generate the grasp pose (stage 2 – generator)
- Grasp the object (stage 3 – propagator)
- 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.
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_demoscd ~/ros2_ws/rosdep install --from-paths src --ignore-src -r -yType in your password, and install any missing dependencies.
Now build.
colcon build && source ~/.bashrcInstall 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 curlWe 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 ~/Downloadscurl -fsSL https://www.mongodb.org/static/pgp/server-8.0.asc | \
sudo gpg -o /usr/share/keyrings/mongodb-server-8.0.gpg \
--dearmorecho "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.listsudo apt-get updatesudo apt-get install -y mongodb-orgNow start the MongoDB service.
sudo systemctl start mongodNow run this command to enable MongoDB to start when you boot up your machine:
sudo systemctl enable mongodVerify everything is up and running fine:
sudo systemctl status mongodIf MongoDB is running, it should show the service as active.
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/srcgit clone https://github.com/moveit/warehouse_ros_mongo.git -b ros2cd warehouse_ros_mongo/git reset --hard 32f8fc5dd245077b9c09e93efc8625b9f599f271cd ~/ros2_wsrosdep updaterosdep install --from-paths src --ignore-src -r -yType your password and press Enter to install any missing packages.
colcon buildsource ~/.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/srcgit clone https://github.com/moveit/moveit_task_constructor.git -b jazzycd moveit_task_constructorSo 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 9ced9fc10a15388224f0741e5a930a33f4ed6255cd ~/ros2_wsrosdep install --from-paths src --ignore-src -r -yType your password and press Enter to install any missing packages.
cd ~/ros2_ws/colcon buildYou 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 ~/.bashrcYou will probably see some build errors now. Build again:
cd ~/ros2_ws/ colcon buildsource ~/.bashrcRemember 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 informationFix 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.cppChange 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 buildsource ~/.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.xmlYour 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 -yCreate 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 launchNow let’s add a launch file:
cd launchtouch mtc_demos.launch.pyAdd this code.
Save the file, and close it.
Create an RViz Config File
cd ~/ros2_ws/src/mycobot_ros2/mycobot_mtc_demos/mkdir rvizcd rvizgedit mtc_demos.rvizAdd 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_demosOpen 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.txtAdd this code.
Save the file, and close it.
Build the Package
Now let’s build.
cd ~/ros2_ws/colcon buildsource ~/.bashrcAdd a Quick Launch Script
cd ~/ros2_ws/src/mycobot_ros2/mycobot_bringup/scriptstouch mycobot_280_mtc_demos.shAdd 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'" >> ~/.bashrcThat’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!
