How to Add and Plan Around Objects Using MoveIt 2

In this tutorial, we will build upon our previous “Hello World” project to learn how to plan around objects in a simulated environment using C++ and MoveIt 2. I will guide you through the process of inserting collision objects into the planning scene and programming the robot to plan its movements while avoiding these obstacles.

The official instructions for this tutorial are available on the MoveIt 2 website, but we’ll walk through everything together, step by step. We’ll cover how to create and add collision objects to the scene, update target poses, and use the Planning Scene Interface to communicate changes to MoveGroup.

By the end of this tutorial, you will be able to create a program that allows a robot to plan and execute movements while avoiding obstacles in its environment. The result will look something like this:

Prerequisites

You have completed this tutorial.

All my code for this project is located here on GitHub.

Add moveit_visual_tools as a Dependency

First let’s add moveit_visual_tools as a dependency inside the hello_moveit package.

The moveit_visual_tools package is a visualization toolkit for MoveIt that provides tools to visualize robot states, trajectories, collision objects, and planning scenes in ROS 2. It integrates with RViz and is particularly useful for debugging motion planning issues, offering functions to generate markers, display text, and visualize trajectories in the 3D environment.

Open a terminal window, and type:

cd ~/ros2_ws/src/mycobot_ros2/hello_moveit/

gedit package.xml

Add this dependency:

<depend>moveit_visual_tools</depend>

Save the file, and close it.

gedit CMakeLists.txt

Add this:

find_package(moveit_visual_tools REQUIRED)

ament_target_dependencies(
  moveit_ros_planning_interface
  moveit_visual_tools
  rclcpp
)

Save the file, and close it.

Now let’s build the package.

cd ~/ros2_ws/

rosdep install --from-paths src --ignore-src -y

You will see a prompt to install moveit-visual-tools.

Type your password, and press Enter.

I got an error that the ros-<ros_distro>-warehouse-ros-mongo package wasn’t found. Don’t worry about it.

colcon build

source ~/.bashrc

Create a New File

We’ll start our tutorial by creating a new C++ source file in the appropriate directory of our ROS 2 workspace. Here’s how to do it…

Open a terminal window on your computer. Navigate to the correct directory in your ROS 2 workspace by entering this command:

cd ~/ros2_ws/src/mycobot_ros2/hello_moveit/src

Now that we’re in the right location, let’s create our new file. We’ll name it “plan_around_objects.cpp”. You can create this file using a text editor of your choice. For example, if you’re using the gedit text editor, you would type:

gedit plan_around_objects.cpp

With our new file open, add this code:

/**
 * @file plan_around_objects.cpp
 * @brief Demonstrates basic usage of MoveIt with ROS 2 and collision avoidance.
 *
 * This program initializes a ROS 2 node, sets up a MoveIt interface for a robot manipulator,
 * creates a collision object in the scene, plans a motion to a target pose while avoiding
 * the collision object, and executes the planned motion.
 *
 * @author Addison Sears-Collins
 * @date August 8, 2024
 */

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_visual_tools/moveit_visual_tools.h>

#include <geometry_msgs/msg/pose_stamped.hpp>

#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <thread>

/**
 * @brief The main function that starts our program.
 *
 * This function sets up our ROS 2 environment and prepares it for robot control.
 *
 * @param argc The number of input arguments our program receives.
 * @param argv The list of input arguments our program receives.
 * @return int A number indicating if our program finished successfully (0) or not.
 */
int main(int argc, char * argv[])
{
  // Start up ROS 2
  rclcpp::init(argc, argv);
  
  // Creates a node named "plan_around_objects". The node is set up to automatically
  // handle any settings (parameters) we might want to change later without editing the code.
  auto const node = std::make_shared<rclcpp::Node>(
    "plan_around_objects",
    rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)
  );

  // Creates a "logger" that we can use to print out information or error messages
  // as our program runs.
  auto const logger = rclcpp::get_logger("plan_around_objects");
  
  // This code creates a separate thread, which is like a mini-program running alongside our main program. 
  // This thread uses a ROS 2 "executor" to continuously process information about the robot's state. 
  // By running this in its own thread, our ROS 2 node can keep getting updates about the robot without 
  // interrupting the main flow of our program.
  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(node);
  auto spinner = std::thread([&executor]() { executor.spin(); });

  // Create the MoveIt MoveGroup Interfaces
  // These interfaces are used to plan and execute movements, set target poses,
  // and perform other motion-related tasks for each respective part of the robot.
  // The use of auto allows the compiler to automatically deduce the type of variable.
  // Source: https://github.com/moveit/moveit2/blob/main/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h
  using moveit::planning_interface::MoveGroupInterface;
  auto arm_group_interface = MoveGroupInterface(node, "arm");

  // Specify a planning pipeline to be used for further planning 
  arm_group_interface.setPlanningPipelineId("ompl");
  
  // Specify a planner to be used for further planning
  arm_group_interface.setPlannerId("RRTConnectkConfigDefault");  

  // Specify the maximum amount of time in seconds to use when planning
  arm_group_interface.setPlanningTime(1.0);
  
  // Set a scaling factor for optionally reducing the maximum joint velocity. Allowed values are in (0,1].
  arm_group_interface.setMaxVelocityScalingFactor(1.0);
  
  //  Set a scaling factor for optionally reducing the maximum joint acceleration. Allowed values are in (0,1].
  arm_group_interface.setMaxAccelerationScalingFactor(1.0);

  // Display helpful logging messages on the terminal
  RCLCPP_INFO(logger, "Planning pipeline: %s", arm_group_interface.getPlanningPipelineId().c_str());    
  RCLCPP_INFO(logger, "Planner ID: %s", arm_group_interface.getPlannerId().c_str());
  RCLCPP_INFO(logger, "Planning time: %.2f", arm_group_interface.getPlanningTime());
  
  // Construct and initialize MoveItVisualTools
  auto moveit_visual_tools =
      moveit_visual_tools::MoveItVisualTools{ node, "base_link", rviz_visual_tools::RVIZ_MARKER_TOPIC,
                                              arm_group_interface.getRobotModel() };
											  
  // Erase all existing visual markers from the RViz visualization. 
  moveit_visual_tools.deleteAllMarkers();

  // Load the remote control interface for MoveIt visual tools. 
  moveit_visual_tools.loadRemoteControl();
  
  // Lambda function to draw a title in the RViz visualization
  auto const draw_title = [&moveit_visual_tools](auto text) {
    // Nested lambda to create a pose for the text
    auto const text_pose = [] {
        // Create an identity transform
        auto msg = Eigen::Isometry3d::Identity();
        // Set the z-coordinate to 1.0 (1 meter above the origin)
        msg.translation().z() = 1.0;
        return msg;
    }();
    
    // Publish the text to RViz
    // Parameters: pose, text content, color (WHITE), and size (XLARGE)
    moveit_visual_tools.publishText(text_pose, text, rviz_visual_tools::WHITE, rviz_visual_tools::XLARGE);
  };
 
  // Lambda function to display a prompt message in RViz
  auto const prompt = [&moveit_visual_tools](auto text) { 
    moveit_visual_tools.prompt(text); 
  };

  // Lambda function to visualize a trajectory as a line in RViz
  auto const draw_trajectory_tool_path = 
    [&moveit_visual_tools, jmg = arm_group_interface.getRobotModel()->getJointModelGroup("arm")](
	  auto const trajectory) {moveit_visual_tools.publishTrajectoryLine(trajectory, jmg);};

  // Set a target pose for the end effector of the arm 
  auto const arm_target_pose = [&node]{
    geometry_msgs::msg::PoseStamped msg;
    msg.header.frame_id = "base_link";
    msg.header.stamp = node->now(); 
    msg.pose.position.x = 0.128;
    msg.pose.position.y = -0.266;
    msg.pose.position.z = 0.111;
    msg.pose.orientation.x = 0.635;
    msg.pose.orientation.y = -0.268;
    msg.pose.orientation.z = 0.694;
    msg.pose.orientation.w = 0.206;
    return msg;
  }();
  arm_group_interface.setPoseTarget(arm_target_pose);
  
  // Create collision object for the robot to avoid
  auto const collision_object = [frame_id = arm_group_interface.getPlanningFrame(), &node, &logger] {
  
    // Print the planning frame for debugging purposes
    RCLCPP_INFO(logger, "Planning frame: %s", frame_id.c_str());

    // Initialize a CollisionObject message
    moveit_msgs::msg::CollisionObject collision_object;
  
    // Set the frame ID for the collision object
    collision_object.header.frame_id = frame_id;
  
    // Set the timestamp to the current time
    collision_object.header.stamp = node->now();
  
    // Assign a unique ID to the collision object
    collision_object.id = "box1";

    // Define the shape of the collision object as a box
    shape_msgs::msg::SolidPrimitive primitive;
    primitive.type = primitive.BOX;
    primitive.dimensions.resize(3);
  
    // Set the dimensions of the box (in meters)
    primitive.dimensions[primitive.BOX_X] = 0.2;  // Width
    primitive.dimensions[primitive.BOX_Y] = 0.05;  // Depth
    primitive.dimensions[primitive.BOX_Z] = 0.50;  // Height

    // Define the pose (position and orientation) of the box
    geometry_msgs::msg::Pose box_pose;
  
    // Set the position of the box center
    box_pose.position.x = 0.22;  // meters in x-direction
    box_pose.position.y = 0.0;   // Centered in y-direction
    box_pose.position.z = 0.25;  // meters in z-direction
  
    // Set the orientation of the box (no rotation in this case)
    box_pose.orientation.x = 0.0;
    box_pose.orientation.y = 0.0;
    box_pose.orientation.z = 0.0;
    box_pose.orientation.w = 1.0;

    // Add the shape and pose to the collision object
    collision_object.primitives.push_back(primitive);
    collision_object.primitive_poses.push_back(box_pose);
  
    // Set the operation to add the object to the planning scene
    collision_object.operation = collision_object.ADD;

    // Log information about the created collision object
    RCLCPP_INFO(logger, "Created collision object: %s", collision_object.id.c_str());
	
    RCLCPP_INFO(logger, "Box dimensions: %.2f x %.2f x %.2f", 
      primitive.dimensions[primitive.BOX_X],
      primitive.dimensions[primitive.BOX_Y],
      primitive.dimensions[primitive.BOX_Z]);
	  
    RCLCPP_INFO(logger, "Box position: (%.2f, %.2f, %.2f)",
      box_pose.position.x, box_pose.position.y, box_pose.position.z);

    // Return the fully defined collision object
    return collision_object;
  }();
  
  // Set up a virtual representation of the robot's environment
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
  
  // Add an object to this virtual environment that the robot needs to avoid colliding with
  planning_scene_interface.applyCollisionObject(collision_object);
  
  // Ask the user to press 'next' to continue
  prompt("Press 'next' in the RvizVisualToolsGui window to plan");
  
  // Set up a title for the next visualization step
  draw_title("Planning");
  
  // Update the visualization to show the new title and wait for user input
  moveit_visual_tools.trigger();

  // Create a plan to that target pose
  // This will give us two things:
  // 1. Whether the planning was successful (stored in 'success')
  // 2. The actual motion plan (stored in 'plan')
  auto const [success, plan] = [&arm_group_interface] {
    moveit::planning_interface::MoveGroupInterface::Plan msg;
    auto const ok = static_cast<bool>(arm_group_interface.plan(msg));
    return std::make_pair(ok, msg);
  }();

  // Try to execute the movement plan if it was created successfully
  // If the plan wasn't successful, report an error
  // Execute the plan
  if (success)
  {
	// Visualize the planned trajectory in RViz
    draw_trajectory_tool_path(plan.trajectory);
	
    // Trigger the visualization update
    moveit_visual_tools.trigger();
	
    // Prompt the user to continue to execution
    prompt("Press 'next' in the RvizVisualToolsGui window to execute");
	
    // Update the title in RViz to show we're executing the plan
    draw_title("Executing");
	
    // Trigger another visualization update
    moveit_visual_tools.trigger();
	
    // Execute the planned motion
    arm_group_interface.execute(plan);
  }
  else
  {
    // If planning failed, update the title in RViz
    draw_title("Planning Failed!");
	
    // Trigger the visualization update
    moveit_visual_tools.trigger();
	
    // Log an error message
    RCLCPP_ERROR(logger, "Planning failed!");
  }
  
  // Clean up and shut down the ROS 2 node
  rclcpp::shutdown();
  
  // Wait for the spinner thread to finish
  spinner.join();
  
  // Exit the program
  return 0;
}

Save the file, and close it.

Understanding the Plan Around Objects Code

Let’s walk through the code before we see it in action.

The code starts with some introductory comments explaining what the program does.

Next, we see a bunch of include statements. These include statements tell the program which tools it needs to use. In this case, it’s bringing in libraries for controlling the robot, planning movements, and visualizing what’s happening.

The main function is where the actual program starts. It begins by initializing ROS 2.

The code sets up a separate thread, which is like a mini-program running alongside our main one. This thread keeps track of what’s happening with the robot in real-time.

Next, it creates interfaces for controlling the robot arm. These interfaces are how our program talks to the robot, telling it where to move and how to plan its movements. The code specifies some settings for these movements, like how fast the arm can move and how long it can take to plan a path.

The program then sets up tools for visualizing what’s happening. This allows us to see a 3D representation of the robot and its planned movements in RViz.

After that, the code specifies a target pose for the robot arm. This is the position and orientation we want the arm to move to.

One of the key parts of this program is creating a virtual obstacle. It defines a box in the robot’s workspace that the arm needs to avoid. This box is given a specific size and position.

With the target and obstacle set up, the program then asks the robot to plan a path to the target that avoids the obstacle. If it successfully creates a plan, it shows this plan in RViz and then tells the robot to execute the movement.

Throughout this process, the code includes various prompts and visual cues to keep you informed about what’s happening. It also has error handling to deal with situations where the robot can’t find a safe path to the target.

Finally, the program cleans up after itself, shutting down ROS 2 and ending the separate thread it created at the beginning.

Understanding Virtual Joints

One more topic before we run our code. Let’s discuss virtual joints.

If you look at my code, you will see the following call:

move_group_interface.getPlanningFrame()

This code retrieves the parent frame of the “virtual joint” defined in the SRDF file. In our case, this is the “world” frame.

A virtual joint is like an imaginary connection between your robot and the world around it. It’s not a real, physical joint, but rather a way to tell the robot how it can move in relation to its environment.

Think of it like this: Imagine you have a toy robot on a table. The robot itself can move its arms and legs, but it can also slide around on the table. The sliding motion isn’t due to any actual joint in the robot, but you need a way to describe this movement. That’s where a virtual joint comes in.

For a mobile manipulator, you will often see the following in an SRDF:

<virtual_joint name="virtual_joint" type="planar" parent_frame="odom" child_link="base_footprint" />

“odom” is used as the world frame because it represents the robot’s odometry – essentially, its position in the world as the robot understands it based on its movement.

“base_footprint” is often chosen as the child link because it represents the base of the robot – imagine the point where the robot touches the ground.

The planar virtual joint tells MoveIt that the robot’s base can move in a plane. It doesn’t actually move the robot, but allows MoveIt to consider the base’s potential positions when planning arm movements.

Also, remember that MoveIt maintains a planning scene that includes the robot’s current state and the environment.

The virtual joint allows the planning scene to be updated with base movements, even though MoveIt isn’t controlling the base directly.

I will also note that for a mobile manipulator, the URDF describes just the physical structure of the robot. It does not include the virtual joint information. The URDF would typically start with the base_footprint link and describe the physical joints and links from there.

The SRDF is where the virtual joint is defined for use with MoveIt. It adds semantic information on top of the URDF, including things like virtual joints, group definitions, and end effectors (i.e. grippers).

Edit CMakeLists.txt

Now let’s build our code.

Open a terminal window, and type:

cd ~/ros2_ws/src/mycobot_ros2/hello_moveit/

gedit CMakeLists.txt

Add this code.

Save the file, and close it.

Edit package.xml

Open a terminal window, and type:

cd ~/ros2_ws/src/mycobot_ros2/hello_moveit/

gedit package.xml

Add this code.

Save the file, and close it.

Build the Package

cd ~/ros2_ws

colcon build

source ~/.bashrc

Run the Program

Now that we’ve set up our target pose and added a collision object to the planning scene, it’s time to run our program and see the results.

ros2 launch mycobot_gazebo mycobot_280_arduino_bringup_ros2_control_gazebo.launch.py use_rviz:=false

Launch MoveIt and RViz.

ros2 launch mycobot_moveit_config_manual_setup move_group.launch.py

Run your node:

ros2 run hello_moveit plan_around_objects

When you run the program, you should observe the following in RViz:

The robot model should be visible.
A box representing your collision object should appear in the planning scene.

Now click Next under RVizVisualToolsGui on the bottom-left of RViz.

The robot should plan a path to the target pose you specified, avoiding the collision object.

If the planning is successful, you’ll see a visualization of the planned path in RViz. The path should clearly avoid the collision object you added.

You will also see a “Planning request complete!” message in the logs.

If you don’t see the collision object or if the robot’s planned path seems to ignore it, double-check that your collision object’s dimensions and position are appropriate for your robot’s workspace.

Now type Next again to execute.

You should see your robot move to the goal.

5-robot-reached-the-goal-successfully-moveit-rviz

That’s it. Keep building!

How to Create Your First C++ MoveIt 2 Project

In this tutorial, we will create a basic “Hello World” project to get your feet wet with C++ and MoveIt 2. I will show you how to move the end of the robotic arm (where the gripper is located) to a desired position and orientation.

The official instructions are on this page, but we will walk through everything together, step by step.

By the end of this tutorial, you will be able to create this:

Prerequisites

You have completed this tutorial.

All my code for this project is located here on GitHub.

Create a Package

Let’s start by creating a new package for our project. 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 moveit_ros_planning_interface rclcpp --node-name hello_moveit_v1 --license BSD-3-Clause hello_moveit

This command creates a new package named “hello_moveit” with the necessary dependencies and a node named “hello_moveit_v1”. The package will be created inside the mycobot_ros2 directory.

Next, let’s move into our newly created package:

cd hello_moveit

Create a ROS Node and Executor

Now that we have our package in place, let’s set up our C++ file to work with MoveIt 2. We’ll start by editing the main source file.

Open the new source code file you created.

cd src

gedit hello_moveit_v1.cpp

Replace the existing content with the following code:

/**
 * @file hello_moveit_v1.cpp
 * @brief A simple program that sets up a ROS 2 node to work with MoveIt for robot control.
 *
 * This program creates a basic setup for controlling a robot using ROS 2 and MoveIt.
 * It doesn't actually move the robot yet, but it prepares everything we need to start
 * planning robot movements in future steps.
 *
 * @author Addison Sears-COllins
 * @date August 07, 2024
 */

#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>

/**
 * @brief The main function that starts our program.
 *
 * This function sets up our ROS 2 environment and prepares it for robot control.
 *
 * @param argc The number of input arguments our program receives.
 * @param argv The list of input arguments our program receives.
 * @return int A number indicating if our program finished successfully (0) or not.
 */
int main(int argc, char * argv[])
{
  // Start up ROS 2
  rclcpp::init(argc, argv);
  
  // Creates a node named "hello_moveit". The node is set up to automatically
  // handle any settings (parameters) we might want to change later without editing the code.
  auto const node = std::make_shared<rclcpp::Node>(
    "hello_moveit",
    rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)
  );

  // Creates a "logger" that we can use to print out information or error messages
  // as our program runs.
  auto const logger = rclcpp::get_logger("hello_moveit");

  // We'll add code here later to actually plan robot movements
  RCLCPP_INFO(logger, "We'll add code here later to actually plan robot movements");

  // Shut down ROS 2 cleanly when we're done
  rclcpp::shutdown();
  return 0;
}

This code sets up the basic structure we need to work with MoveIt 2. It initializes ROS 2, creates a node, sets up logging, and creates a MoveGroupInterface for our robot’s arm.

Let’s compile our code to make sure everything is set up correctly. Go back to your workspace directory:

cd ~/ros2_ws

To build the package with debug information, we’ll use the following command:

colcon build --packages-select hello_moveit

This command builds only our hello_moveit package.

After the build completes successfully, open a new terminal and source your workspace:

source ~/.bashrc

Now you can run your program:

ros2 run hello_moveit hello_moveit_v1

At this point, the program should run and exit without any errors, though it won’t perform any robot movements yet. This step simply confirms that our basic setup is working correctly.

Plan and Execute Using MoveGroupInterface

Now that we’ve confirmed our basic setup is working, let’s add the code to plan and execute a motion for our robotic arm.

Create a new file called hello_moveit_v2.cpp

cd ~/ros2_ws/src/mycobot_ros2/hello_moveit/src

gedit hello_moveit_v2.cpp

Add the following code:

/**
 * @file hello_moveit_v2.cpp
 * @brief A simple ROS 2 and MoveIt 2 program to control a robot arm
 *
 * This program demonstrates how to use ROS 2 and MoveIt 2 to control a robot arm.
 * It sets up a node, creates a MoveGroupInterface for the arm, sets a target pose for the gripper_base,
 * plans a trajectory, and executes the planned motion.
 *
 * @author Addison Sears-Collins
 * @date August 07, 2024
 */

#include <geometry_msgs/msg/pose_stamped.hpp>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>

/**
 * @brief The main function that starts our program.
 *
 * This function sets up our ROS 2 environment and prepares it for robot control.
 *
 * @param argc The number of input arguments our program receives.
 * @param argv The list of input arguments our program receives.
 * @return int A number indicating if our program finished successfully (0) or not.
 */
int main(int argc, char * argv[])
{
  // Start up ROS 2
  rclcpp::init(argc, argv);
  
  // Creates a node named "hello_moveit". The node is set up to automatically
  // handle any settings (parameters) we might want to change later without editing the code.
  auto const node = std::make_shared<rclcpp::Node>(
    "hello_moveit",
    rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)
  );

  // Creates a "logger" that we can use to print out information or error messages
  // as our program runs.
  auto const logger = rclcpp::get_logger("hello_moveit");

  // Create the MoveIt MoveGroup Interfaces
  // These interfaces are used to plan and execute movements, set target poses,
  // and perform other motion-related tasks for each respective part of the robot.
  // The use of auto allows the compiler to automatically deduce the type of variable.
  // Source: https://github.com/moveit/moveit2/blob/main/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h
  using moveit::planning_interface::MoveGroupInterface;
  auto arm_group_interface = MoveGroupInterface(node, "arm");

  // Specify a planning pipeline to be used for further planning 
  arm_group_interface.setPlanningPipelineId("ompl");
  
  // Specify a planner to be used for further planning
  arm_group_interface.setPlannerId("RRTConnectkConfigDefault");  

  // Specify the maximum amount of time in seconds to use when planning
  arm_group_interface.setPlanningTime(1.0);
  
  // Set a scaling factor for optionally reducing the maximum joint velocity. Allowed values are in (0,1].
  arm_group_interface.setMaxVelocityScalingFactor(1.0);
  
  //  Set a scaling factor for optionally reducing the maximum joint acceleration. Allowed values are in (0,1].
  arm_group_interface.setMaxAccelerationScalingFactor(1.0);

  // Display helpful logging messages on the terminal
  RCLCPP_INFO(logger, "Planning pipeline: %s", arm_group_interface.getPlanningPipelineId().c_str());    
  RCLCPP_INFO(logger, "Planner ID: %s", arm_group_interface.getPlannerId().c_str());
  RCLCPP_INFO(logger, "Planning time: %.2f", arm_group_interface.getPlanningTime());

  // Set a target pose for the end effector of the arm 
  auto const arm_target_pose = [&node]{
    geometry_msgs::msg::PoseStamped msg;
    msg.header.frame_id = "base_link";
    msg.header.stamp = node->now(); 
    msg.pose.position.x = 0.061;
    msg.pose.position.y = -0.176;
    msg.pose.position.z = 0.168;
    msg.pose.orientation.x = 1.0;
    msg.pose.orientation.y = 0.0;
    msg.pose.orientation.z = 0.0;
    msg.pose.orientation.w = 0.0;
    return msg;
  }();
  arm_group_interface.setPoseTarget(arm_target_pose);

  // Create a plan to that target pose
  // This will give us two things:
  // 1. Whether the planning was successful (stored in 'success')
  // 2. The actual motion plan (stored in 'plan')
  auto const [success, plan] = [&arm_group_interface] {
    moveit::planning_interface::MoveGroupInterface::Plan msg;
    auto const ok = static_cast<bool>(arm_group_interface.plan(msg));
    return std::make_pair(ok, msg);
  }();

  // Try to execute the movement plan if it was created successfully
  // If the plan wasn't successful, report an error
  // Execute the plan
  if (success)
  {
    arm_group_interface.execute(plan);
  }
    else
  {
    RCLCPP_ERROR(logger, "Planning failed!");
  }
  
  // Shut down ROS 2 cleanly when we're done
  rclcpp::shutdown();
  return 0;
}

This code does the following:

Sets a target pose for the end effector (gripper_base) of the robotic arm.
Plans a path to reach that target pose.
Executes the plan if it was successful, or logs an error if planning failed.

Now, let’s build our updated code.

Go to CMakeLists.txt

cd ~/ros2_ws/src/mycobot_ros2/hello_moveit/

gedit CMakeLists.txt

Update the file with the hello_moveit_v2.cpp code.

Save the file, and close it.

cd ~/ros2_ws

colcon build

source ~/.bashrc

Launch the controller and Gazebo:

ros2 launch mycobot_gazebo mycobot_280_arduino_bringup_ros2_control_gazebo.launch.py use_rviz:=false

Start the MoveGroup with RViz.

ros2 launch mycobot_moveit_config_manual_setup move_group.launch.py

In another terminal, run your program to move the robotic arm from one place to another:

ros2 run hello_moveit hello_moveit_v2

You can safely ignore this warning:

[moveit_ros.robot_model_loader]: No kinematics plugins defined. Fill and load kinematics.yaml!

If everything is set up correctly, you should see the robot’s gripper move in RViz to the specified pose.

You can confirm the gripper is in the correct pose by typing the following command and comparing it to the pose you set in your hello_moveit_v2.cpp file.

ros2 run tf2_ros tf2_echo base_link gripper_base

Note: If you run the hello_moveit_v2 node without first launching the demo launch file, it will wait for 10 seconds and then exit with an error message. This is because the MoveGroupInterface looks for a node publishing the robot description topic, which is provided by the demo launch file.

Let’s break down the key components of our hello_moveit_v2.cpp file to understand how it works with MoveIt 2 for controlling the arm.

MoveGroupInterface Creation:

This part sets up a control interface for the robot’s arm. It’s like creating a special remote control that lets us send commands to move the arm.

Setting the Target Pose:

Here, we’re telling the arm where we want it to go.

Planning the Motion:

This is where the robot figures out how to move from its current position to the target we set. It’s like planning a route on a map – the robot calculates all the steps needed to get from point A to point B safely.

Executing the Plan:

Finally, if the planning was successful, we tell the robot to actually perform the movement we planned. If the planning failed for some reason, we just report an error instead of trying to move.

These steps work together to allow us to control the robot arm in a safe and precise manner, moving it to exactly where we want it to go.

That’s it! Keep building.

Difference between Pilz, STOMP, and OMPL Planners

When programming a robotic arm using MoveIt 2, choosing the right motion planner is important for optimal performance. This tutorial compares Pilz (Pilz Industrial Motion Planner), STOMP (Stochastic Trajectory Optimization for Motion Planning), and OMPL planners, focusing on when to use each one over the others.

We’ll use a cooking robot as an example to illustrate their applications.

Note: CHOMP is excluded from this comparison as (according to the literature) STOMP typically outperforms CHOMP in most scenarios.

Pilz (Pilz Industrial Motion Planner)

When to use Pilz

For simple, predefined movements, especially linear (LIN), circular (CIRC), or point-to-point (PTP) motions
When speed and predictability are more important than path optimization
In well-structured environments with few obstacles
For repetitive tasks that don’t require complex trajectory planning

Example

Use Pilz when your cooking robot needs to perform a stirring motion in a bowl.

The CIRC (circular) motion type of Pilz is perfect for this task. It can efficiently execute a predefined circular path for stirring, maintaining a consistent speed and radius. This is more suitable than STOMP or OMPL for several reasons:

Simplicity: The stirring motion is a basic, repetitive circular movement that doesn’t require complex path planning.
Speed: Pilz can execute this predefined motion quickly and efficiently, which is important for tasks like mixing ingredients.
Predictability: The stirring motion needs to be consistent, which Pilz’s CIRC motion ensures.

In this case, STOMP would be unnecessarily complex and potentially slower, while OMPL’s adaptability isn’t needed for this fixed, repetitive task.

Pilz’s straightforward approach makes it the ideal choice for such structured, repetitive movements in cooking tasks.

STOMP (Stochastic Trajectory Optimization for Motion Planning)

When to use STOMP

For tasks requiring smooth, precise movements
When path quality is more critical than planning speed
To incorporate specific constraints (e.g., maintaining orientation)
When combining obstacle avoidance with smooth motion

Example

Choose STOMP for a robot pouring olive oil on a frying pan.

STOMP is ideal here because:

Smooth trajectory: It generates a fluid motion to move the oil bottle over the pan and tilt it gradually.
Orientation control: It can maintain the bottle’s orientation throughout the motion, ensuring controlled pouring.
Path optimization: It optimizes the entire path, allowing for a consistent oil distribution across the pan.
Splash prevention: Its smooth motion helps prevent splashing by avoiding sudden accelerations or jerky movements.

Pilz would be less suitable as its predefined motions might result in abrupt tilting or stopping, increasing the risk of oil splashing.

OMPL, while capable of finding a path to the pan, doesn’t inherently optimize for the smooth, controlled motion needed for pouring oil. It might produce a valid but less refined trajectory, potentially leading to uneven oil distribution or splashing due to sudden changes in velocity or direction.

STOMP’s ability to create a fluid, optimized motion while considering the task’s constraints makes it the best choice for this precise cooking operation, minimizing the risk of splashing and ensuring even oil distribution.

OMPL (Open Motion Planning Library)

When to use OMPL

In complex, cluttered, or changing environments
When a path needs to be found quickly in unpredictable scenarios
For high-dimensional planning problems
When adaptability to changing environments is important
OMPL is the default planner for MoveIt2

Example

Use OMPL when your cooking robot needs to reach into a cluttered refrigerator to retrieve a specific ingredient.

OMPL is the best choice for this task because:

Adaptability: It can quickly generate a new path if items in the fridge have moved since last access.
Complex environment handling: OMPL efficiently navigates around various obstacles (other ingredients, containers) in the crowded space.
Speed in unpredictable scenarios: It rapidly computes a valid path even if the target ingredient’s location has changed.
High-dimensional planning: It can handle the multiple degrees of freedom of the robot arm required to maneuver in the confined, complex space of a refrigerator.

Pilz would struggle in this scenario as its predefined motions aren’t suitable for navigating a complex, changing environment like a crowded fridge.

STOMP, while capable of producing smooth trajectories, would be slower in finding an initial valid path in this complex space and might struggle to quickly adapt if the environment changes mid-motion.

OMPL‘s strength in rapidly finding feasible paths in complex, dynamic environments makes it the optimal choice for this type of task, where adaptability and quick path generation are more critical than having a perfectly smooth trajectory.

That’s it. Keep building!