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