## Cartesian Path Planning Using the MoveIt Task Constructor

In this tutorial, we will create a Cartesian path planning demo from scratch using the MoveIt Task Constructor. This demo showcases how to plan a series of movements for a robotic arm in Cartesian space, including linear motions, rotations, and joint movements. Here is what you will create:

cartesian.cpp demonstrates a sequence of planned movements:

1. Straight-line movements of the arm’s gripper (end-effector) relative to the base_link coordinate frame:
• 5 cm in the positive x direction
• 2 cm in the negative y direction
2. A twisting movement of the arm relative to the base_link coordinate frame:
• 18-degree rotation around the z-axis
3. Moving specific joints of the arm by absolute amounts:
4. Returning to the starting “ready” position smoothly

# Real-World Use Cases

The code you will develop in this tutorial can serve as a template for various practical applications:

• Pick and Place Operations
• Precise linear movements for accurate object placement
• Rotation capabilities for orienting objects correctly
• Combine linear motions and rotations for complex assembly procedures
• Fine-tune joint movements for intricate manipulations
• Welding or Painting
• Use linear movements to follow seams or contours
• Maintain consistent orientation during the process

# Prerequisites

All the code is here on my GitHub repository. Note that I am working with ROS 2 Iron, so the steps might be slightly different for other versions of ROS 2.

# Create the Code

Open a new terminal window, and type:

``cd ~/ros2_ws/src/mycobot_ros2/hello_moveit_task_constructor/src/``
``gedit cartesian.cpp``

```/**
* @file cartesian.cpp
* @brief Demonstrates Cartesian path planning using MoveIt Task Constructor
*
* This program shows how to use MoveIt Task Constructor to plan a series of
* movements for a robot arm. It includes linear motions, rotations, and joint
* movements, all planned in Cartesian space.
*
* @date August 16, 2024
*/

#include <rclcpp/rclcpp.hpp>
#include <moveit/planning_scene/planning_scene.h>

// Use the moveit::task_constructor namespace to avoid typing it out each time

/**
*
* This function sets up a series of movements (called stages) for the robot to follow.
* Each stage represents a specific motion, like moving in a straight line or rotating.
* Logging statements are added to track the creation and addition of each stage.
*
* @param node A shared pointer to a ROS 2 node, used for accessing ROS functionality
*/

// Create a new Task object

// Set a name for the task (useful for debugging and visualization)
t.stages()->setName("Cartesian Path");

// Define the names of the robot's planning groups as defined in the SRDF
const std::string arm = "arm";
const std::string arm_with_gripper = "arm_with_gripper";

// Create solvers for Cartesian and joint interpolation
// Solvers are algorithms that figure out how to move the robot
auto cartesian_interpolation = std::make_shared<solvers::CartesianPath>();
auto joint_interpolation = std::make_shared<solvers::JointInterpolationPlanner>();

RCLCPP_INFO(node->get_logger(), "Created Cartesian and Joint interpolation solvers");

// Load the robot model (this contains information about the robot's structure)

// Create a planning scene (this represents the robot and its environment)
auto scene = std::make_shared<planning_scene::PlanningScene>(t.getRobotModel());
RCLCPP_INFO(node->get_logger(), "Created planning scene");

// Set the initial state of the robot
{
RCLCPP_INFO(node->get_logger(), "Setting initial state");

// Get the current state of the robot and modify it
auto& state = scene->getCurrentStateNonConst();

// Set the robot arm to its "ready" position as defined in the SRDF

// Create a FixedState stage to represent this initial state
auto fixed = std::make_unique<stages::FixedState>("initial state");
fixed->setState(scene);

}

// Stage 1: Move 0.05 meters in the positive x direction relative to the base_link frame
{
RCLCPP_INFO(node->get_logger(), "Creating stage: Move 0.05m in +x direction");

// Create a MoveRelative stage using Cartesian interpolation
auto stage = std::make_unique<stages::MoveRelative>("x +0.05", cartesian_interpolation);

// Specify which group of joints to move
stage->setGroup(arm);

// Set the Inverse Kinematic frame to the end-effector

// Create a Vector3Stamped message to specify the direction of movement
geometry_msgs::msg::Vector3Stamped direction;

// Set the frame of reference for this movement (the "base_link" frame)

// Set the x component to 0.05 meters (move 5 cm in the x direction)
direction.vector.x = 0.05;

// Set this direction as the movement for this stage
stage->setDirection(direction);

}

// Stage 2: Move 0.02 meters in the negative y direction relative to the base_link frame
{
RCLCPP_INFO(node->get_logger(), "Creating stage: Move 0.02m in -y direction");

// Similar to the previous stage, but moving in the y direction
auto stage = std::make_unique<stages::MoveRelative>("y -0.02", cartesian_interpolation);

stage->setGroup(arm);

geometry_msgs::msg::Vector3Stamped direction;

// Set the y component to -0.02 meters (move 2 cm in the negative y direction)
direction.vector.y = -0.02;
stage->setDirection(direction);

}

// Stage 3: Rotate 18 degrees around the z-axis of the base_link frame
{
RCLCPP_INFO(node->get_logger(), "Creating stage: Rotate -18 degrees around z-axis");

auto stage = std::make_unique<stages::MoveRelative>("rz -18°", cartesian_interpolation);
stage->setGroup(arm);

// Create a TwistStamped message to specify a rotation
geometry_msgs::msg::TwistStamped twist;

// Set the angular z component to -pi/10 radians (-18 degrees)
twist.twist.angular.z = -M_PI / 10.;

stage->setDirection(twist);
}

// Stage 4: Move specific joints by specified angles
{
RCLCPP_INFO(node->get_logger(), "Creating stage: Move joints by specified angles");
auto stage = std::make_unique<stages::MoveRelative>("joint offset", cartesian_interpolation);
stage->setGroup(arm);

// Create a map of joint names to angle offsets
std::map<std::string, double> offsets = {
{ "link3_to_link4", -M_PI / 12. }   // Rotate this joint by -15 degrees
};

stage->setDirection(offsets);

}

// Stage 5: Connect the previous stages using joint interpolation
// This stage ensures smooth transitions between all the previous stages and from the
// previous stage to the final stage.
{
RCLCPP_INFO(node->get_logger(), "Creating connect stage");

// Create a vector of groups and their associated planners
stages::Connect::GroupPlannerVector planners = { { arm, joint_interpolation } };

// Create a Connect stage to smoothly link the previous stages
auto connect = std::make_unique<stages::Connect>("connect", planners);
}

// Set the final state of the robot
{
RCLCPP_INFO(node->get_logger(), "Setting final state");

// The final state is the same as the initial state
auto fixed = std::make_unique<stages::FixedState>("final state");
fixed->setState(scene);
}

// Return the fully configured task
return t;
}

/**
* @brief Main function to demonstrate Cartesian path planning.
*
* This function initializes ROS 2, creates a node, sets up the task,
* plans it, and attempts to execute it. Logging statements are added
* to track the program's progress and any errors that occur.
*
* @param argc Number of command-line arguments
* @param argv Array of command-line arguments
* @return int Exit status (0 for success, non-zero for failure)
*/
int main(int argc, char** argv) {

// Initialize ROS 2
rclcpp::init(argc, argv);

// Create a ROS 2 node named "cartesian_demo"
auto node = rclcpp::Node::make_shared("cartesian_demo");

RCLCPP_INFO(node->get_logger(), "Starting Cartesian path planning demo");

// Start a separate thread to handle ROS 2 callbacks
// This allows the node to process incoming messages and services
RCLCPP_INFO(node->get_logger(), "Started ROS 2 spinning thread");
rclcpp::spin(node);
});

// Attempt to plan and execute the task
try {

// If planning succeeds...

// ...publish the solution so it can be visualized or executed

} else {
}
} catch (const InitStageException& ex) {

// If planning fails, print an error message
RCLCPP_ERROR(node->get_logger(), "Planning failed with exception: %s", ex.what());
}

RCLCPP_INFO(node->get_logger(), "Waiting for ROS 2 spinning thread to finish");

// Keeps the program running so that you can inspect the results in RViz

RCLCPP_INFO(node->get_logger(), "Cartesian path planning demo completed");

// Exit the program
return 0;
}

```

Save the file, and close it.

# Build the Code

``cd ~/ros2_ws/``
``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”)``````

# Launch

Open two terminal windows, and run the following commands to launch our standard MoveIt 2 environment:

``ros2 launch mycobot_gazebo mycobot_280_arduino_bringup_ros2_control_gazebo.launch.py use_rviz:=false``
``ros2 launch hello_moveit_task_constructor demo.launch.py``

Now run the demo:

``ros2 launch hello_moveit_task_constructor run.launch.py exe:=cartesian``

Here is what you should see:

# Understanding the Motion Planning Results

## RViz – “Motion Planning Tasks” Panel

When running this demo, you’ll see a panel labeled “Motion Planning Tasks” on your screen. This panel shows the structure of each task. Clicking on a stage in this panel will display its outcomes – both successful and failed – in another window. You can then select and visualize individual solutions.

The Task Tree in the panel reflects the structure of our program. At the top, you’ll see “Motion Planning Tasks” with “Cartesian Path” underneath. This corresponds to our main task, which is designed to plan a series of Cartesian movements for the robot arm.

The task is divided into seven stages:

1. “initial state”: Sets the starting position of the robot.
2. “x +0.05”: Moves the end-effector 5 cm in the positive x direction.
3. “y -0.02”: Moves the end-effector 2 cm in the negative y direction.
4. “rz -18°”: Rotates the end-effector 18 degrees around the z-axis.
5. “joint offset”: Adjusts specific joint angles.
6. “connect”: Ensures smooth transitions between all the previous stages.
7. “final state”: Sets the ending position of the robot.

The “connect” stage is where the magic happens. In our code, we’ve created a Connect stage to smoothly link the previous stages using joint interpolation.

The green checkmarks and numbers in the second column indicate how many successful solutions were found for each part of the task. For instance, if you see a “1” next to each stage, it means our planner successfully found a solution for each movement.

The rightmost column in the Task Tree displays the planning time for each component in seconds. This helps us understand which movements might be more computationally intensive.

As you experiment with the demo, try clicking on the different stages under the Task Tree and then click the line in the right-most column of the Motion Planning Tasks panel. You’ll be able to visualize how each stage contributes to the overall motion of the robot arm, helping you understand the progression of movements in Cartesian space.

## Executing the Plan

If you want to execute the entire planned motion, you can use the “Execute” button in your control interface.

If everything worked fine during execution, you will see this line in the terminal window:

``[moveit_ros.trajectory_execution_manager]: Completed trajectory execution with status SUCCEEDED ...``

# Detailed Code Walkthrough

Now for the C++ part. Let’s go through each piece of this code, step by step.

``cd ~/ros2_ws/src/mycobot_ros2/hello_moveit_task_constructor/src/``
``gedit cartesian.cpp``

The code begins with a comprehensive comment block outlining the file’s purpose: demonstrating Cartesian path planning using MoveIt Task Constructor. It describes the program’s functionality, which includes planning linear motions, rotations, and joint movements in Cartesian space for a robotic arm. The file includes necessary headers for ROS 2, MoveIt, and the Task Constructor library, establishing the foundation for our Cartesian path planning demo.

This function is responsible for setting up the Task Constructor task with various stages of movement. Let’s break it down…

A Task object is created and named “Cartesian Path”. Two solvers are set up: one for Cartesian path planning and another for joint interpolation. These solvers will be used in different stages of the task.

### Robot Model and Planning Scene Setup

The robot model is loaded, and a planning scene is created. This part sets up the environment in which the robot will operate.

### Initial State Setup

The initial state of the robot is set to the “ready” position, and this state is added as the first stage of the task.

### Movement Stages

The function then sets up several movement stages:

• Move 0.05m in +x direction
• Move 0.02m in -y direction
• Rotate -18 degrees around z-axis
• Move specific joints by specified angles

Each of these stages is created using stages::MoveRelative, configured with the appropriate movement parameters, and added to the task.

### Connect Stage

A Connect stage is added to ensure smooth transitions between all the previous stages.

### Final State

The final state is set to be the same as the initial state, completing the movement cycle.

## Main Function

The main function orchestrates the entire demo.

### ROS 2 Initialization and Node Setup

ROS 2 is initialized, and a node named “cartesian_demo” is created.

A separate thread is created to handle ROS 2 callbacks, allowing the node to process incoming messages and services.

The task is created using the createTask function. The code then attempts to plan the task. If planning succeeds, the solution is published for visualization or execution.

### Error Handling

The code includes error handling to catch and report any exceptions that occur during the planning process.

### Completion

The program waits for the ROS 2 spinning thread to finish, allowing for inspection of the results in RViz before exiting.

That’s it! Keep building!

## Create Alternative Paths Using the MoveIt Task Constructor

There are six demos that come out of the box with the ROS 2 move_it_task_constructor_demo package. However, these demos are tailored for another robotic arm. We need to tailor these for our robotic arm.

In this tutorial, we will create the first demo from scratch…alternative_path_costs.cpp.

Here is what the output looks like:

alternative_path_costs.cpp showcases different ways to plan and evaluate robot arm movements. It goes through four different methods for moving the robotic arm from a starting position to a predefined goal position:

1. The shortest path
2. The quickest movement
3. A path focusing on minimal end-effector (gripper) motion
4. A path that minimizes movement of a specific part of the arm (like the elbow).

# Real-World Use Cases

The code you will develop in this tutorial can serve as a template for some practical real-world applications:

1. Precision Manufacturing
• Minimize end-effector motion for delicate assembly tasks
• Optimize trajectory duration to increase production speed
• Reduce wear on joints by minimizing overall path length
2. Collaborative Robotics
• Plan predictable movements by minimizing elbow motion, enhancing worker safety
• Adjust strategies based on the task: quick movements for time-sensitive operations or smooth motions for human comfort
3. Flexible Manufacturing Systems
• Quickly adapt arm movements to different product specifications
• Switch between optimization strategies (e.g., speed vs. precision) based on current production needs

# Prerequisites

All the code is here on my GitHub repository. Note that I am working with ROS 2 Iron, so the steps might be slightly different for other versions of ROS 2.

# Create the Code

Open a new terminal window, and type:

``cd ~/ros2_ws/src/mycobot_ros2/hello_moveit_task_constructor/src/``
``gedit alternative_path_costs.cpp``

```/**
* @file alternative_path_costs.cpp
* @brief Demonstrates different path planning strategies using MoveIt Task Constructor.
*
* This program sets up a Task Constructor task to plan robot motion using different
* cost optimization strategies. It creates multiple path planning alternatives and
* evaluates them based on different cost terms such as path length, trajectory duration,
* end-effector motion, and elbow motion.
*
* @date August 16, 2024
*/

#include <rclcpp/rclcpp.hpp>
#include <moveit/planning_scene/planning_scene.h>
#include <iostream>

/**
* @brief Main function to demonstrate alternative path costs.
*
* This function sets up a ROS 2 node, creates a Task Constructor task,
* and demonstrates different path planning strategies using various cost terms.
* FixedState (Generator Stage) - Connect (Connector Stage) - FixedState (Generator Stage)
*
* @param argc Number of command-line arguments
* @param argv Array of command-line arguments
* @return int Exit status
*/
int main(int argc, char** argv) {
// Initialize ROS 2
rclcpp::init(argc, argv);
std::cout << "ROS 2 initialized for alternative_path_costs_demo" << std::endl;

// Set up ROS 2 node options
rclcpp::NodeOptions node_options;
node_options.automatically_declare_parameters_from_overrides(true);

// Create a ROS 2 node
auto node = rclcpp::Node::make_shared("alternative_path_costs_demo", node_options);
std::cout << "Created ROS 2 node: alternative_path_costs_demo" << std::endl;

// Start a separate thread to handle ROS 2 callbacks

t.stages()->setName("alternative path costs");

std::cout << "Loaded robot model: " << t.getRobotModel()->getName() << std::endl;

// Ensure the correct robot model is loaded
assert(t.getRobotModel()->getName() == "mycobot_280");

// Create a planning scene
auto scene{ std::make_shared<planning_scene::PlanningScene>(t.getRobotModel()) };
std::cout << "Created planning scene" << std::endl;

// Get the current robot state and set it to default values
auto& robot_state{ scene->getCurrentStateNonConst() };
robot_state.setToDefaultValues();
robot_state.setToDefaultValues(robot_state.getJointModelGroup("arm"), "home");
std::cout << "Initialized robot state in planning scene to default values and 'home' position" << std::endl;

// This step gives MoveIt 2 a clear starting point from which to plan the robot's movements.
auto initial{ std::make_unique<stages::FixedState>("start") };
initial->setState(scene);

// Create a pipeline planner
// The "pipeline" part means it can chain together multiple planning attempts or strategies.
// If one method fails, it can try another
auto pipeline{ std::make_shared<solvers::PipelinePlanner>(node) };
std::cout << "Created pipeline planner" << std::endl;

// Create an Alternatives container for different path planning strategies
// Alternatives container is a type of Parallel container. It's used to group multiple stages that
// represent different ways to accomplish the same task.
// In this case, it's used to create different strategies for connecting the intial state to the goal state.
// The planner will try all these strategies and choose the best one based on their respective cost terms.
auto alternatives{ std::make_unique<Alternatives>("connect") };
std::cout << "Created Alternatives container for path planning strategies" << std::endl;

// Add different path planning strategies (Connect stages) to the Alternatives container
// Each strategy uses a different cost term to evaluate the path

// Strategy 1: Minimize path length
{
auto connect{ std::make_unique<stages::Connect>(
"path length", stages::Connect::GroupPlannerVector{ { "arm_with_gripper", pipeline } }) };
connect->setCostTerm(std::make_unique<cost::PathLength>()); //  Typically in radians, representing the total angular displacement of all joints.
std::cout << "Added 'path length' strategy" << std::endl;
}

// Strategy 2: Minimize trajectory duration
{
auto connect{ std::make_unique<stages::Connect>(
"trajectory duration", stages::Connect::GroupPlannerVector{ { "arm_with_gripper", pipeline } }) };
connect->setCostTerm(std::make_unique<cost::TrajectoryDuration>()); // Time it would take in seconds to execute the planned motion.
std::cout << "Added 'trajectory duration' strategy" << std::endl;
}

// Strategy 3: Minimize end-effector motion
{
auto connect{ std::make_unique<stages::Connect>(
"eef motion", stages::Connect::GroupPlannerVector{ { "arm_with_gripper", pipeline } }) };
std::cout << "Added 'end-effector motion' strategy" << std::endl;
}

// Strategy 4: Minimize elbow motion
{
auto connect{ std::make_unique<stages::Connect>(
"elbow motion", stages::Connect::GroupPlannerVector{ { "arm_with_gripper", pipeline } }) };
std::cout << "Added 'elbow motion' strategy" << std::endl;
}

// By adding this Alternatives container to the task, we're giving the task planner multiple options for
// solving a particular part of the robot's movement. The planner can now choose the best strategy based on the current situation.
std::cout << "Added all strategies in the Alternatives Container to the task" << std::endl;

// Create the goal scene as a diff from the current scene
// Think of this like taking a snapshot of the current room (scene) and then describing only what
// needs to change to reach the desired setup.
// In our goal setup, we want the robot's arm and gripper to be in the 'ready' position."
auto goal_scene{ scene->diff() };
std::cout << "Created goal scene with 'ready' position" << std::endl;

auto goal = std::make_unique<stages::FixedState>("goal");
goal->setState(goal_scene);

std::cout << "Starting task planning..." << std::endl;
try {
t.plan(0); // The 0 parameter means it will generate as many solutions as possible
std::cout << "Task planning completed successfully" << std::endl;

// Print the results
std::cout << "Planning results:" << std::endl;
t.printState();

} catch (const InitStageException& e) {
std::cout << "Task planning failed: " << e << std::endl;
}

// Keep the node alive for interactive inspection in RViz
std::cout << "Keeping node alive for RViz inspection. Press Ctrl+C to exit." << std::endl;

return 0;
}
```

Save the file, and close it.

# Build the Code

``cd ~/ros2_ws/``
``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”)``````

# Launch

Open two terminal windows, and run the following commands to launch our standard MoveIt 2 environment:

``ros2 launch mycobot_gazebo mycobot_280_arduino_bringup_ros2_control_gazebo.launch.py use_rviz:=false``
``ros2 launch hello_moveit_task_constructor demo.launch.py``

In separate terminal windows, run one of the demos. Let’s see what launch options we have:

``ros2 launch hello_moveit_task_constructor run.launch.py --show-args``

Now run the demo:

``ros2 launch hello_moveit_task_constructor run.launch.py exe:=alternative_path_costs``

Here is what you should see:

# Understanding the Motion Planning Results

## RViz – “Motion Planning Tasks” Panel

When running this demo, you’ll see a panel labeled “Motion Planning Tasks” on your screen. This panel shows the structure of each task. Clicking on a stage in this panel will display its outcomes – both successful and failed – in another window. You can then select and visualize individual solutions.

The Task Tree in the panel reflects the structure of our program. At the top, you’ll see “Motion Planning Tasks” with “alternative path costs” underneath. This corresponds to our main task, which is designed to explore different path planning strategies.

The task is divided into three main stages: “start,” “connect,” and “goal.” These stages represent the robot’s initial position (set to the “home” position in our code), the motion it needs to perform, and its final position (set to the “ready” position).

The “connect” stage is where the magic happens. In our code, we’ve created an Alternatives container with four different planning strategies:

1. “path length“: This strategy aims to minimize the total angular displacement of all joints, measured in radians.

2. “trajectory duration“: This strategy tries to find the quickest path, minimizing the time it would take to execute the planned motion.

3. “eef motion“: This strategy focuses on minimizing the distance traveled by the end-effector (the robot’s “hand”), which in our code is represented by the “link6_flange”.

4. “elbow motion“: Similar to the end-effector strategy, but focusing on minimizing the movement of the robot’s elbow (represented by “link3” in our code).

The green checkmarks and numbers in the second column indicate how many successful solutions were found for each part of the task.

For example, if you see a “4” next to “alternative path costs,” it means our planner successfully found solutions using all four strategies.

The rightmost column displays the planning time for each component in seconds. This column helps us understand which strategies might be more computationally intensive.

As you experiment with the demo, try clicking on the “alternative path costs” line under the Task Tree. Then select each of the four different solutions to see how the robotic arm moves. You’ll be able to visualize how each strategy results in different robot motions, helping you understand the trade-offs between path length, speed, and the movement of specific parts of the robot.

If you actually want to execute a particular solution, click the “Execute Solution” button in the upper-right part of the panel.

## Terminal Window – Planning Results

If you look at the terminal window, you will see the planning results.

MoveIt Task Constructor uses a bidirectional planning approach. This means it can work on a problem from both ends – the start stage and the goal stage – simultaneously.

• Forward: When the planner finds a good partial solution from the start, it sends this information forward to help plan the next stages.
• Backward: When it finds a good approach to the goal, it sends this information backward to influence earlier stages of the plan.

### Arrow Interpretation

• → (Right Arrow): Represents the forward flow of results from one stage to the next. Forward flow means that a stage has successfully generated a result and is passing that result to the next stage for further processing.
• ← (Left Arrow): Indicates a backward flow of results. In MTC, some stages may require feedback from later stages to adjust their own results or to optimize the plan. Therefore, the results flow backward to a previous stage for this adjustment.
• – (Dash): A dash indicates no information flowed in that direction.

When a solution is found in one stage, it might impose constraints on other stages. Propagating the solution backwards allows earlier stages to take these constraints into account, potentially leading to better overall solutions.

A good example of the benefits of bidirectional planning is a pick and place task. Imagine you want a robotic arm to pour water from a cup into a pot. You want to make sure that early stages don’t generate movements that would put the arm in a position where grasping the cup to do a pouring movement is difficult or infeasible.

This unidirectional approach could result in less efficient movements, more frequent replanning, or even failure to complete the task if a viable path from start to finish isn’t found. Overall, the lack of foresight into future stages could lead to suboptimal or unsuccessful attempts at completing the pick and place task.

In our particular application, for the “start” stage, we see 1 – ← 1 → – 1. This line indicates that our start state (the “home” position we set) was successfully created, with one solution propagated forward to the connect stage and one solution propagated backwards to the initial stage at t=0.

The “connect” stage, where our four strategies are implemented, displays – 1 → 4 ← 1 -. This line means it received one solution propagated forward from the start, generated four alternative solutions (one for each strategy), and received one solution propagated backward from the goal.

Looking at our individual strategies:

• “path length”: – 1 → 1 ← 1 –
• “trajectory duration”: 1 → 1 ← 1 –
• “eef motion”: 1 → 1 ← 1 –
• “elbow motion”: 1 → 1 ← 1 –

Each strategy successfully received one solution from the start, generated its own solution, and received one solution from the goal. These results indicate that each strategy successfully connected the start and goal stage.

The “goal” stage shows 1 – ← 1 → – 1, confirming that the final state (our “ready” position) was successfully incorporated into the planning process, propagating one solution backward to the connect stage and one solution forward to t=final.

These results demonstrate that our planner effectively generated and evaluated multiple paths using different optimization criteria. It successfully planned motions from the home position to the ready position using each strategy, considering both the start and goal states in its calculations. This bidirectional approach (planning from both start and goal) often leads to more efficient and robust motion planning.

By examining these different solutions in RViz, you can observe how prioritizing path length, duration, end-effector motion, or elbow motion affects the robot’s planned movement, providing valuable insights into the trade-offs between different motion planning approaches.

# Detailed Code Walkthrough

Now for the C++ part. Let’s go through each piece of this code, step by step.

``cd ~/ros2_ws/src/mycobot_ros2/hello_moveit_task_constructor/src/``
``gedit alternative_path_costs.cpp``

The code begins with a comprehensive comment block explaining the file’s purpose: demonstrating various path planning strategies using MoveIt Task Constructor. It outlines the program’s functionality, creating multiple planning alternatives and evaluating them based on different cost terms like path length and trajectory duration. The file includes necessary headers for ROS 2, MoveIt, and the Task Constructor library, setting up the foundation for our robot motion planning task.

## Main Function

The main function serves as the entry point and primary workflow of the program. It orchestrates the setup of the ROS 2 environment, task creation, and implementation of planning strategies.

## ROS 2 Initialization

The code initializes ROS 2, creating a computational environment for robot control. It then sets up a node named “alternative_path_costs_demo”. This node acts as a communication endpoint within the ROS 2 system.

A separate thread is created to handle ROS 2 callbacks. Callbacks are functions that are automatically called in response to specific events or messages in the ROS 2 system. By running callbacks in a separate thread, the program can continue its main planning tasks without interruption, while still responding to incoming messages or events. This approach enhances the system’s ability to multitask effectively.

The code creates a Task Constructor task, naming it “alternative path costs”. This task will encompass all the planning stages and strategies. It then loads the robot model for our robotic arm. This step provides the task with detailed information about the robot’s physical structure, joint limits, and other characteristics necessary for accurate motion planning.

## Planning Scene Setup

A planning scene is created based on the loaded robot model. This virtual environment represents the space in which the robot operates and where motion planning will occur. The robot’s initial state within this scene is set to default values, with the “arm” group specifically positioned in a predefined “home” position. This setup ensures a consistent starting point for all planning attempts.

## Initial State Creation

The code creates an initial state named “start” as a FixedState stage. This stage is added to the task and represents the starting configuration of the robot. It serves as a key reference point from which all motion plans will begin.

## Pipeline Planner Creation

The code creates a PipelinePlanner, a planning tool that interfaces with MoveIt’s PlanningPipeline. This planner can use multiple planning algorithms and post-processing steps. It includes features like planner caching for efficiency, customizable planning parameters (e.g., number of attempts, workspace parameters, goal tolerances), and options for displaying motion plans and publishing planning requests.

That’s it! Congratulations on getting your feet wet with a real application using the MoveIt Task Constructor. Stay tuned for more projects.

Keep building!

## Getting Started with the MoveIt 2 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.

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:

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.

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.

Now that we have covered the concepts, let’s get hands-on experience with the MTC.

## 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 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 7.0 now below, so the exact version you need to download will likely change in the future).

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

Now start the MongoDB service.

``sudo systemctl start mongod``

If you receive an error similar to the following when starting mongod:

Run the following command first:

``sudo systemctl daemon-reload``

Then run the start command above again.

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.

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/``
``gedit package.xml``

Remove this line:

``<depend>mongodb</depend>``

Save the file, and close it.

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

``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 git@github.com:moveit/moveit_task_constructor.git -b ros2``
``cd ~/ros2_ws``
``rosdep update``
``rosdep install --from-paths src --ignore-src -r -y``

``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

``source ~/.bashrc``

## Fix Build Errors

I had a lot of build errors at this stage. Build errors are common for open-source ROS 2 packages you pull directly off of GitHub, so no need to worry.

Note that the build errors I discuss below are for ROS 2 Iron. You will have different build errors for other versions of ROS 2.

I will show you how to troubleshoot these errors. Learning how to troubleshoot errors is a good skill to have for ROS 2 since you will often be using packages developed by third parties.

Let’s summarize the errors we have now:

• Error in cartesian_path.cpp: ‘relative’ is not a member of ‘moveit::core::JumpThreshold’
• Errors in pipeline_planner.cpp:
• ‘const class moveit::core::MoveItErrorCode’ has no member named ‘message’
• Could not convert from ‘<brace-enclosed initializer list>’ to ‘moveit::task_constructor::solvers::PlannerInterface::Result’

To fix these errors, we need to dive into the move_it_task_constructor source code.

Let’s go to the cartesian_path.cpp file.

``cd ~/ros2_ws/src/moveit_task_constructor/core/src/solvers/``
``gedit cartesian_path.cpp``

Change this line:

moveit::core::JumpThreshold::relative(props.get<double>(“jump_threshold”)), is_valid,

To this:

moveit::core::JumpThreshold(props.get<double>(“jump_threshold”)), is_valid,

We can see the JumpThreshold API was updated with a relative member not too long ago, so we need to revert to the older JumpThreshold API.

Now let’s look at pipeline_planner.cpp.

``cd ~/ros2_ws/src/moveit_task_constructor/core/src/solvers/``
``gedit pipeline_planner.cpp``

I can see that a change was made on March 9, 2024.

They added the following line, which is causing our issues:

return { false, solution.error_code.message };

Replace that line with this:

return { false, “Error code: ” + std::to_string(solution.error_code.val) };

Save the file, and close it.

Now build again.

``cd ~/ros2_ws/``
``colcon build``

The stderr output for moveit_task_constructor_core is just a warning about using serial compilation for LTRANS jobs, which is not a critical issue.

The other stderr outputs were mostly informing that a Release build type was chosen for maximum performance, which is generally a good thing.

``source ~/.bashrc (OR source ~/ros2_ws/install/setup.bash)``

Just to make sure everything is nice and clean, do a new build in a fresh terminal window:

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

## 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 this line:

``````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.

# Create a Package for Your MTC Code

Now that we have installed the MTC, let’s create a package to store our code.

MoveIt Task Constructor includes several demo programs. We will recreate these demos from scratch in future tutorials. For now, let’s just create a package  to hold the launch files for these demo programs.

## Create a Package

``cd ~/ros2_ws/src/mycobot_ros2/``
``ros2 pkg create --build-type ament_cmake --dependencies generate_parameter_library moveit_core moveit_ros_planning_interface moveit_task_constructor_core rclcpp --license BSD-3-Clause --node-name mtc_node hello_moveit_task_constructor``

## Create a Launch File for the Demos

Move inside the package.

``cd ~/ros2_ws/src/mycobot_ros2/hello_moveit_task_constructor/``

Create a folder named launch.

``mkdir launch``

Now let’s add a launch file:

``cd launch``
``gedit run.launch.py ``

```# Author: Addison Sears-Collins
# Date: August 26, 2024
# Description: ROS 2 launch file for MoveIt Task Constructor demo nodes.

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from moveit_configs_utils import MoveItConfigsBuilder

def generate_launch_description():
# Constants for paths to different files and folders
package_name_gazebo = 'mycobot_gazebo'
package_name_moveit_config = 'mycobot_moveit_config_manual_setup'

# Set the path to different files and folders
pkg_share_gazebo = FindPackageShare(package=package_name_gazebo).find(package_name_gazebo)
pkg_share_moveit_config = FindPackageShare(package=package_name_moveit_config).find(package_name_moveit_config)
pkg_share_mtc = FindPackageShare(package=package_name_mtc).find(package_name_mtc)

# Paths for various configuration files
urdf_file_path = 'urdf/ros2_control/gazebo/mycobot_280.urdf.xacro'
srdf_file_path = 'config/mycobot_280.srdf'
moveit_controllers_file_path = 'config/moveit_controllers.yaml'
joint_limits_file_path = 'config/joint_limits.yaml'
kinematics_file_path = 'config/kinematics.yaml'
pilz_cartesian_limits_file_path = 'config/pilz_cartesian_limits.yaml'
initial_positions_file_path = 'config/initial_positions.yaml'
mtc_node_params_file_path = 'config/mtc_node_params.yaml'

# Set the full paths
urdf_model_path = os.path.join(pkg_share_gazebo, urdf_file_path)
srdf_model_path = os.path.join(pkg_share_moveit_config, srdf_file_path)
moveit_controllers_file_path = os.path.join(pkg_share_moveit_config, moveit_controllers_file_path)
joint_limits_file_path = os.path.join(pkg_share_moveit_config, joint_limits_file_path)
kinematics_file_path = os.path.join(pkg_share_moveit_config, kinematics_file_path)
pilz_cartesian_limits_file_path = os.path.join(pkg_share_moveit_config, pilz_cartesian_limits_file_path)
initial_positions_file_path = os.path.join(pkg_share_moveit_config, initial_positions_file_path)
mtc_node_params_file_path = os.path.join(pkg_share_mtc, mtc_node_params_file_path)

# Launch configuration variables
use_sim_time = LaunchConfiguration('use_sim_time')
exe = LaunchConfiguration('exe')

# Declare the launch arguments
declare_use_sim_time_cmd = DeclareLaunchArgument(
name='use_sim_time',
default_value='true',
description='Use simulation (Gazebo) clock if true')

declare_exe_cmd = DeclareLaunchArgument(
name="exe",
default_value="pick_place_demo",
description="Which demo to run",
choices=["alternative_path_costs", "cartesian", "fallbacks_move_to",
"ik_clearance_cost", "modular", "mtc_node", "pick_place_demo"])

# Typically, you would also have this line in here: .robot_description(file_path=urdf_model_path)
# Another launch file is launching the robot description.
moveit_config = (
MoveItConfigsBuilder("mycobot_280", package_name=package_name_moveit_config)
.robot_description_semantic(file_path=srdf_model_path)
.trajectory_execution(file_path=moveit_controllers_file_path)
.joint_limits(file_path=joint_limits_file_path)
.robot_description_kinematics(file_path=kinematics_file_path)
.pilz_cartesian_limits(file_path=pilz_cartesian_limits_file_path)
.planning_pipelines(
pipelines=["ompl", "pilz_industrial_motion_planner", "stomp"],
default_planning_pipeline="ompl"
)
.planning_scene_monitor(
publish_robot_description=False,
publish_robot_description_semantic=True,
publish_planning_scene=True,
)
.to_moveit_configs()
)

node = Node(
executable=exe,
output="screen",
parameters=[
moveit_config.robot_description,
moveit_config.robot_description_semantic,
moveit_config.robot_description_kinematics,
moveit_config.joint_limits,
moveit_config.pilz_cartesian_limits,
moveit_config.planning_pipelines,
{'use_sim_time': use_sim_time},
mtc_node_params_file_path,
],
)

# Create the launch description and populate
ld = LaunchDescription()

# Declare the launch options

return ld
```

Save the file, and close it.

``gedit demo.launch.py ``

```# Author: Addison Sears-Collins
# Date: August 15, 2024
# Description: Launch MoveIt 2 for the myCobot robotic arm

import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, EmitEvent, RegisterEventHandler
from launch.conditions import IfCondition
from launch.event_handlers import OnProcessExit
from launch.events import Shutdown
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from moveit_configs_utils import MoveItConfigsBuilder
import xacro

def generate_launch_description():

# Constants for paths to different files and folders
package_name_gazebo = 'mycobot_gazebo'
package_name_moveit_config = 'mycobot_moveit_config_manual_setup'

# Set the path to different files and folders
pkg_share_gazebo = FindPackageShare(package=package_name_gazebo).find(package_name_gazebo)
pkg_share_moveit_config = FindPackageShare(package=package_name_moveit_config).find(package_name_moveit_config)

# Paths for various configuration files
urdf_file_path = 'urdf/ros2_control/gazebo/mycobot_280.urdf.xacro'
srdf_file_path = 'config/mycobot_280.srdf'
moveit_controllers_file_path = 'config/moveit_controllers.yaml'
joint_limits_file_path = 'config/joint_limits.yaml'
kinematics_file_path = 'config/kinematics.yaml'
pilz_cartesian_limits_file_path = 'config/pilz_cartesian_limits.yaml'
initial_positions_file_path = 'config/initial_positions.yaml'
rviz_config_file_path = 'rviz/mtc.rviz'

# Set the full paths
urdf_model_path = os.path.join(pkg_share_gazebo, urdf_file_path)
srdf_model_path = os.path.join(pkg_share_moveit_config, srdf_file_path)
moveit_controllers_file_path = os.path.join(pkg_share_moveit_config, moveit_controllers_file_path)
joint_limits_file_path = os.path.join(pkg_share_moveit_config, joint_limits_file_path)
kinematics_file_path = os.path.join(pkg_share_moveit_config, kinematics_file_path)
pilz_cartesian_limits_file_path = os.path.join(pkg_share_moveit_config, pilz_cartesian_limits_file_path)
initial_positions_file_path = os.path.join(pkg_share_moveit_config, initial_positions_file_path)
rviz_config_file = os.path.join(pkg_share_moveit_config, rviz_config_file_path)

# Launch configuration variables
use_sim_time = LaunchConfiguration('use_sim_time')
use_rviz = LaunchConfiguration('use_rviz')

# Declare the launch arguments
declare_use_sim_time_cmd = DeclareLaunchArgument(
name='use_sim_time',
default_value='true',
description='Use simulation (Gazebo) clock if true')

declare_use_rviz_cmd = DeclareLaunchArgument(
name='use_rviz',
default_value='true',
description='Whether to start RViz')

# Typically, you would also have this line in here: .robot_description(file_path=urdf_model_path)
# Another launch file is launching the robot description.
moveit_config = (
MoveItConfigsBuilder("mycobot_280", package_name=package_name_moveit_config)
.trajectory_execution(file_path=moveit_controllers_file_path)
.robot_description_semantic(file_path=srdf_model_path)
.joint_limits(file_path=joint_limits_file_path)
.robot_description_kinematics(file_path=kinematics_file_path)
.planning_pipelines(
pipelines=["ompl", "pilz_industrial_motion_planner", "stomp"],
default_planning_pipeline="ompl"
)
.planning_scene_monitor(
publish_robot_description=False,
publish_robot_description_semantic=True,
publish_planning_scene=True,
)
.pilz_cartesian_limits(file_path=pilz_cartesian_limits_file_path)
.to_moveit_configs()
)

# Enable the execution of tasks in the MoveIt Task Constructor (MTC).

# Start the actual move_group node/action server
start_move_group_node_cmd = Node(
package="moveit_ros_move_group",
executable="move_group",
output="screen",
parameters=[
moveit_config.to_dict(),
{'use_sim_time': use_sim_time},
{'start_state': {'content': initial_positions_file_path}},
move_group_capabilities,
],
)

# RViz
start_rviz_node_cmd = Node(
condition=IfCondition(use_rviz),
package="rviz2",
executable="rviz2",
arguments=["-d", rviz_config_file],
output="screen",
parameters=[
moveit_config.robot_description,
moveit_config.robot_description_semantic,
moveit_config.planning_pipelines,
moveit_config.robot_description_kinematics,
moveit_config.joint_limits,
{'use_sim_time': use_sim_time}
],
)

exit_event_handler = RegisterEventHandler(
condition=IfCondition(use_rviz),
event_handler=OnProcessExit(
target_action=start_rviz_node_cmd,
on_exit=EmitEvent(event=Shutdown(reason='rviz exited')),
),
)

# Create the launch description and populate
ld = LaunchDescription()

# Declare the launch options

# Clean shutdown of RViz

return ld
```

Save the file, and close it.

## Create an Include Folder

``cd ~/ros2_ws/src/mycobot_ros2/hello_moveit_task_constructor/``
``mkdir -p include/hello_moveit_task_constructor``
``cd include/hello_moveit_task_constructor``
``gedit pick_place_task.h``

Don’t worry about what the code means for now. We will go through it in a future tutorial.

Save the file, and close it.

## Create an RViz Config File

``cd ~/ros2_ws/src/mycobot_ros2/mycobot_moveit_config_manual_setup/rviz/``
``gedit mtc.rviz``

Save the file, and close it.

``cd ~/ros2_ws/src/mycobot_ros2/hello_moveit_task_constructor/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/hello_moveit_task_constructor/``
``gedit CMakeLists.txt``

Save the file, and close it.

## Edit package.xml

Now let’s edit the package.xml file.

``cd ~/ros2_ws/src/mycobot_ros2/hello_moveit_task_constructor/``
``gedit package.xml``

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