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
pilz

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!

Complete Guide to the Moveit Setup Assistant for Moveit 2

In the previous tutorial, we created the configuration files for MoveIt manually from scratch. Creating the MoveIt configuration files manually provides a deeper understanding of the system’s structure and enables you to configure MoveIt just how you want. 

An alternative way of creating these configuration files is to use the MoveIt Setup Assistant. 

I strongly recommend you create the MoveIt config package manually if you are using Gazebo because the MoveIt Setup Assistant demo.launch.py file doesn’t work straight out of the box (as of the time of this writing). You can also use MoveIt Setup Assistant to generate the configuration files, and then you can copy the files over to your own manually created package.

I’ll walk you through the configuration file generation process in this tutorial.

The official tutorial is here, but I will walk you through all the steps below. 

Follow along with me step by step, mouse click by mouse click, keystroke by keystroke as we explore how to use this tool to integrate your robotic arm with MoveIt 2 and ROS 2.

Prerequisites

  • You have completed this tutorial (strongly recommended).
  • You have a URDF/XACRO file for your robotic arm.

Here is the code on GitHub.

Directions

Load the URDF

Everything starts with the URDF/XACRO file. Once you have your URDF, you are ready to use the MoveIt Assistant.

If you don’t have a URDF, you will need to create one.

Open a terminal window, and launch the MoveIt Setup Assistant:

ros2 launch moveit_setup_assistant setup_assistant.launch.py
1-create-new-moveit-configuration-package

You now have two choices: 

  1. Create New MoveIt Configuration Package
  2. Edit Existing MoveIt Configuration Package.

Click on the Create New MoveIt Configuration Package button to bring up the following screen:

Click Browse, and find the XACRO file for the robotic arm.

/home/ubuntu/ros2_ws/src/mycobot_ros2/mycobot_gazebo/urdf/ros2_control/gazebo/mycobot_280.urdf.xacro

Choose the file.

Click Load Files.

Wait for the Setup Assistant to load the files. 

When it is done, you will see this screen:

2-successful-load-screen

Generate the Self-Collision Matrix

In this section, we will generate the self-collision matrix. 

The Self-Collision matrix generator in MoveIt 2 helps speed up motion planning by identifying which parts of the robotic arm are safe to ignore when checking for collisions. It does this by randomly putting the robot in different positions and checking which parts collide with each other, are always colliding, never collide, or are next to each other. 

By increasing the number of random positions checked (sampling density), the collision checking results become more accurate, but the process takes longer.

Select the Self-Collisions pane on the left-hand side of the MoveIt Setup Assistant.

Let’s set the self-collision sampling density to somewhere in the middle of the slider.

Now click Generate Collision Matrix. This step will check for pairs of links on your robotic arm that can be safely ignored for collision checking.

Let’s take a look at the results.

3a-optimize-self-collision-checking

Collision by Default: This means the system assumes these parts might collide unless told otherwise. It’s like being extra careful by default.

Adjacent Links: These are parts that are right next to each other on the robot. The system usually doesn’t need to check if they’ll collide because they’re designed to move together.

Never in Collision: This means these parts are so far apart or positioned in a way that they can never hit each other, so the system doesn’t need to check them.

Click matrix view.

If you see a checkmark in the box, that means the robot won’t check for collisions between those two parts. But if you want it to start checking, just click the checkbox to remove the checkmark. Doing this will turn collision checking on for that pair of parts.

I am really concerned about the big parts of the robot (i.e. link 3 and 4, which are the “meat” of the arm) colliding with the base of the robot. You can see my final matrix here:

3-optimize-self-collision-checking

Add Virtual Joints

Now let’s add a virtual joint. A virtual joint’s purpose is to connect the robotic arm to the world.

For our robotic arm, we will attach the base_link of the robotic arm to the world coordinate frame. This virtual joint means that the base of the robotic arm will remain stationary in the world frame.

Click on the Virtual Joints pane selector on the left-hand side of the window.

4-define-virtual-joints

Click Add Virtual Joint.

Set the Virtual Joint Name to virtual_joint.

Set the Child Link to base_link.

Set the Parent Frame Name to the world.

Set the Joint Type to fixed.

Click Save.

6-save-a-virtual-joint

There is a note on the MoveIt 2 website about how to define virtual joints for a mobile manipulator.

5-note-about-mobile-manipulator

For a mobile manipulator, we would create a virtual joint to connect the mobile robot’s base to the odometry frame

Here is how you might configure this:

  • Parent Frame: This would be the odometry frame, usually named odom.
  • Child Frame: This would be the mobile robot base (usually called base_link)

Here is an example configuration for this type of virtual joint:

  • Joint Name: virtual_joint
  • Parent Frame: odom
  • Child Link: base_link
  • Joint Type: planar

The planar joint allows for motion in the x and y directions and rotation about the z-axis, which is typical for mobile bases operating on a plane.

This configuration effectively models the motion of the robot’s mobile base relative to its odometry frame, which is important for accurate motion planning and control in a dynamic environment.

Add Planning Groups

Now it is time to add planning groups. 

When you want a robot to move a specific part of its body, like an arm, a gripper, or a hand, you need to tell the computer which parts of the robot should move together. This is where “planning groups” come into play.

A planning group is a way to describe a specific part of the robot to the computer. It’s like giving a name to a collection of robot parts that work together for a particular motion.

For example, if you have a robot with an arm and a gripper, you could create two planning groups: “arm” and “gripper.”

The arm group would include all the necessary links (the solid parts of the robot) and joints (the movable connections between the links) that make up the arm. By doing this, you’re telling the computer, “When I say ‘move the arm,’ I mean move all these specific parts together.”

Similarly, for the gripper group, you would include all the links and joints needed for the gripper to open and close.

Planning groups make it easier to control the robot’s motions by allowing you to give simple commands like “move the arm to the home position,” “open the gripper,” or “make a fist,” instead of having to control each individual link and joint separately. This way, you can focus on the overall movement you want the robot to make, and let the computer take care of the details of moving the right parts in the right way.

We are going to add three planning groups to our robot: arm, gripper, and arm_with_gripper.

First, click on the Planning Groups pane selector.

Click on Add Group.

Add the Arm Group

Let’s start by adding the arm group.

Enter the Group Name as arm.

Choose kdl_kinematics_plugin/KDLKinematicsPlugin as the kinematics solver. 

The KDLKinematicsPlugin helps determine how a robotic arm should move its joints to reach a certain position or follow a certain path. 

For example, if you want the robot’s arm to reach out and press a button or move to pick up an object, this plugin calculates the best way to achieve that by adjusting the angles of the joints.

7-moveit-arm-planning-group

Let me briefly explain all the options on the dropdown list.

KDLKinematicsPlugin 

The KDLKinematicsPlugin is the default for MoveIt2 due to its reliability and comprehensive support for various robot configurations. It ensures joint limit compliance as specified in URDF files, making it versatile for many applications.

CachedKDLKinematicsPlugin

This plugin is ideal for applications where IK solutions are frequently reused, such as in repetitive pick-and-place tasks, as it speeds up calculations by caching previous solutions.

CachedSrvKinematicsPlugin

This plugin would be useful when you have a custom kinematics service that you want to cache for improved performance. It is ideal when you have complex or custom kinematics that aren’t easily solved by standard libraries, but you still want to benefit from caching for faster repeated calculations.

LMAKinematicsPlugin

The LMAKinematicsPlugin is particularly useful for industrial robots with multiple redundant joints, such as 7-DOF robotic arms used in advanced manufacturing. It is valuable for robots that need to perform complex tasks in tight spaces, like welding in automotive assembly lines or intricate pick-and-place operations in electronics manufacturing. This plugin can also be beneficial for research robots or prototypes with unconventional designs where standard kinematics solvers struggle to find valid solutions.

SrvKinematicsPlugin

The SrvKinematicsPlugin is used when you have a custom kinematics solver that runs as a separate service outside of MoveIt 2. 

This setup is useful in two main scenarios:

  1. When working with proprietary robots where the manufacturer provides a “black box” kinematics solver that can’t be directly integrated into MoveIt’s codebase.
  2. In research environments where you’re experimenting with novel kinematics algorithms and want to keep them separate from MoveIt for easier development and testing.

By using this plugin, you can connect MoveIt 2 to your external kinematics service, allowing MoveIt to use your custom solver without needing to modify MoveIt’s core code.

Let Kin. Search Resolution be 0.005 and Kin. Search Timeout be 0.05.

The Kinematic Search Resolution parameter represents the precision with which the robotic arm searches for the correct joint positions. A value of 0.005 means that the robot makes adjustments in increments of 0.005 units (typically in radians or meters, depending on the context), meaning the robot will make small, precise adjustments to find the correct position for its movements.

The Kinematic Search Timeout represents the maximum amount of time the robotic arm will spend trying to find the correct joint positions before giving up. A timeout of 0.05 means that the robotic arm will only attempt to find the correct position for 0.05 seconds before stopping its search.

Set the Group Default Planner for OMPL Planning to RRTConnect.

RRTConnect stands for Rapidly-exploring Random Tree Connect. The algorithm works by growing two trees simultaneously – one from the start pose and one from the goal pose. It attempts to connect these trees to find a valid path.

RRTConnect is a good general-purpose planner, and you will see it as the default planner in a lot of MoveIt configuration packages you find on GitHub.

Now click on the Add Joints button.

8-arm-joints

You will see a list of the joints in the left column of the panel.

To choose all joints that belong to the robotic arm and add them to the right column, you need to:

  1. Click on virtual_joint.
  2. Hold down the shift button on your keyboard.
  3. Click on the last joint of the robotic arm group. It is called link6flange_to_gripper_base.
  4. Click the > button to add the joints to the Selected Joints column.

Click Save.

Here is the list of our arm group joints:

joints:

  • virtual_joint
  • link1_to_link2
  • link2_to_link3
  • link3_to_link4
  • link4_to_link5
  • link5_to_link6
  • link6_to_link6flange   

Add the Gripper Group

Now let’s add the group for the gripper on the robotic arm.

Click on the Add Group button.

Our Group Name will be gripper.

Our gripper is a simple open-close mechanism that doesn’t require complex motion planning. It can be controlled without needing a Kinematic Solver to calculate its trajectories.

Select None for Kinematic Solver.

Keep Kin. Search Resolution and Kin. Search Timeout at their default values.

Click the Add Links button.

9-add-gripper-links

Select the following links, and add them to the list of Selected Links on the right-hand side:

  • gripper_base
  • gripper_left1
  • gripper_left2
  • gripper_left3
  • gripper_right1
  • gripper_right2
  • gripper_right3

In this automatic setup, we will use links instead of joints for the gripper planning group because it’s easier to plan and control the gripper’s movements based on the positions of the gripper links. This approach simplifies grasping and collision checking.

Click Save.

Add the Arm with Gripper Group

Finally, let’s add the arm_with_gripper group.

10-arm-with-gripper

Click on the Add Group button.

Our Group Name will be arm_with_gripper.

Our gripper is a simple open-close mechanism that doesn’t require complex motion planning. It can be controlled without needing a Kinematic Solver to calculate its trajectories.

Choose kdl_kinematics_plugin/KDLKinematicsPlugin as the kinematics solver. 

Keep Kin. Search Resolution at 0.005.

Change Kin. Search Timeout to 0.05.

Select RRTConnect as the Group Default Planner.

Click the Add Subgroups button.

Select the following subgroups, and click the > button.

  • arm
  • gripper

Click Save.

Click Collapse All.

Here is what your screen should look like:

11-planning-groups

Add Robot Poses

Arm Pose

The Setup Assistant has a useful feature that lets you define and store predefined poses for the robot. 

Once these poses are set up, the robot can be instructed to move to these positions using the MoveIt 2 API.

Let’s add a home pose for the arm. Follow these steps…

Click on the Robot Poses pane.

Click the Add Pose button.

Assign a name to the pose, such as “home”.

Initially, the robot will be in its default pose, where all joints are set to their zero values. You can adjust the individual joints to create the desired pose. 

For the arm group, define a home pose using the following joint values:

  • joint name=”link1_to_link2″ value=”0″
  • joint name=”link2_to_link3″ value=”0″
  • joint name=”link3_to_link4″ value=”0″
  • joint name=”link4_to_link5″ value=”0″
  • joint name=”link5_to_link6″ value=”0″
  • joint name=”link6_to_link6flange” value=”0″
12-home-pose

Click the “Save” button to store it.

Remember that poses are linked to specific groups. You have the ability to save separate poses for each group.

Make sure to experiment with moving all the joints. If there are any issues with the joint limits defined in your URDF, you will be able to identify them at this stage of the process.

If you find that you move your robotic arm into positions where the assistant is telling you a collision will occur, go back to the Self-Collisions panel and check that link pair.

Gripper Pose

Now to the gripper. Let’s add an open, half-closed, and closed pose for the gripper. 

Follow these steps…

Click on the Robot Poses pane.

Click the Add Pose button.

Assign the name “open” to the pose.

13-gripper-open-pose

For the gripper group, define an open pose using the following joint value:

  •  joint name=”gripper_controller” value=”0.0000″

Click the “Save” button to store it.

Click the Add Pose button.

Assign the name “half_closed” to the pose.

For the gripper group, define a close pose using the following joint value:

  •  joint name=”gripper_controller” value=”-0.36″

Click the “Save” button to store it.

You will notice that only the gripper_controller appears in the list. The reason for this is that all the other gripper joints mimic the gripper_controller.

Click on the Robot Poses pane.

Click the Add Pose button.

Assign the name “closed” to the pose.

For the gripper group, define a closed pose using the following joint value:

  •  joint name=”gripper_controller” value=”-0.59″

Click the “Save” button to store it.

Click the Add Pose button.

Let’s take a look now at what robot poses we have so far.

14-poses-have-so-far

Arm with Gripper Pose

Now let’s define a pose for the combined arm_with_gripper group.

Click on the Robot Poses pane.

Click the Add Pose button.

Assign the name “ready” to the pose.

For the arm_with_gripper group, define a pose using the following joint values:

  • joint name=”link1_to_link2″ value=”0″
  • joint name=”link2_to_link3″ value=”0″
  • joint name=”link3_to_link4″ value=”1.5708″
  • joint name=”link4_to_link5″ value=”1.5708″
  • joint name=”link5_to_link6″ value=”0″
  • joint name=”link6_to_link6flange” value=”0″
  •  joint name=”gripper_controller” value=”0″

Click the “Save” button to store it.

Let’s take a look now at what robot poses we have now.

14-poses-have-so-far-2

Label End Effectors

Since we have added the gripper as a move group, we can set it as an end effector. By making a group an end effector, MoveIt can do special things with the group. For example, you can attach objects to the arm when doing pick-and-place tasks.

Click on the End Effectors pane.

Click Add End Effector.

Choose gripper as the End Effector Name for the gripper.

Select gripper as the End Effector Group.

Select link6_flange as the Parent Link for this end-effector.

15-define-end-effector

Leave the Parent Group field blank.

16-define-end-effector

Add Passive Joints

Click the Passive Joints pane.

Here we have a section called “Passive Joints.” This is where you tell the software about any parts of your robotic arm that can move but can’t be controlled directly. Think of these like loose joints or hinges that swing freely.

17-define-passive-joints

It is important to list these passive joints because the robot’s movement planning software needs to know about them. If it doesn’t, it might try to make plans that involve moving these parts in ways they can’t actually move, which would mess up the whole plan.

In the case of the myCobot 280, it doesn’t have any of these passive joints, so you can skip this part when setting it up.

ros2_control URDF Modification

This step prepares your robot’s URDF file for use with ROS 2’s control system. It adds information about how to command each joint and what information we can get from them.

Since our robot’s URDF file already includes this information (mycobot_280_ros2_control.xacro), you can skip this step. 

For each joint in your robot, this step will add:

command_interface:

  • The type of commands that can be sent to each joint. 
  • Position is the default command interface.
  • Position:
    • For revolute (rotating) joints, units are in radians
    • For prismatic (sliding) joints, units are in meters

state_interface:

  • The type of information that can be received from each joint.
  • Position and velocity are the default state interfaces.
  • Position:
    • For revolute joints, units are in radians
    • For prismatic joints, units are in meters
  • Velocity:
    • For revolute joints, units are in radians per second
    • For prismatic joints, units are in meters per second

If you want to add other command and state interfaces, choose your desired command or state interfaces for the joints on your robot, and then click “Add Interfaces.”

ROS 2 Controllers

This pane allows you to automatically create simulated controllers. These controllers are used to move (or “actuate”) the joints of your robot in a simulated environment.

Click on the ROS 2 Controllers pane selector.

Click Add Controller.

Let’s add the arm joint trajectory controller.

Enter Controller Name as arm_controller.

18-add-arm-controller

Choose joint_trajectory_controller/JointTrajectoryController as the controller type.

The joint trajectory controller is responsible for generating smooth motions of multiple joints over time. 

  • Input: A series of joint positions at specific points in time (a trajectory)
  • Output: Commands to move joints to follow this trajectory

Now click Add Planning Group Joints.

Choose the arm group from the Available Group tab, and add it to the Selected Groups.

Click Save to save this controller.

Now let’s add the gripper position controller.

Click on the ROS 2 Controllers pane selector.

Click Add Controller again.

Enter Controller Name as grip_action_controller.

Choose position_controllers/GripperActionController as the controller type.

Now click Add Planning Group Joints.

Choose the gripper group from the Available Group tab, and add it to the Selected Groups.

19-choose-gripper-group

Click Save to save this controller.

MoveIt Controllers

Now click the MoveIt Controllers tab.

MoveIt Controllers are like translators that MoveIt uses to communicate with your robot. They don’t do the actual controlling. ROS 2 Controllers (like Joint Trajectory Controller) are the ones that actually make the robot move by sending signals to its motors.

In practice:

  1. MoveIt plans a motion.
  2. The MoveIt Controller takes this plan and formats it for your robot.
  3. It sends this formatted plan to the appropriate ROS 2 Controller (e.g., Joint Trajectory Controller) via the FollowJointTrajectoryAction interface.
  4. The ROS 2 Controller then executes this plan by directly controlling the robot’s motors via the ROS 2 Control Hardware Interface.

This separation allows MoveIt to work with many different types of robots and control systems, while the ROS 2 Controllers handle the specifics of how each robot actually moves.

First let’s add the arm MoveIt controllers.

Click on Add Controller to create a new arm controller.

Enter the Controller Name. This name must match the ROS 2 Controller name, which is  arm_controller.

Choose FollowJointTrajectory as the Controller Type.

Click Add Planning Group Joints.

Select the arm group under Group Names and then press the > button to push it over to Selected Groups.

Click Save.

Now let’s add the gripper MoveIt controller.

Click on Add Controller to create a new controller.

Enter Controller Name as grip_action_controller.

20-grip-action-controller

Choose GripperCommand Controller Type.

Click Add Planning Group Joints.

Choose the controller joints with the gripper planning group.

Save the controller.

Here is what everything looks like now.

21-what-it-looks-like

Perception

Now let’s go to the Perception tab. This tab is where you will configure the settings for your depth camera. These settings will go inside a YAML configuration file called sensors_3d.yaml.

You can see the description of these settings on this page.

Let’s imagine we have an Intel RealSense depth camera.

Select Point Cloud from the dropdown menu.

For Point Cloud Topic, select: /camera/depth/color/points

This is the topic that our camera will publish the 3D point cloud to.

For Max Range, put 1.5.

Points further than 1.5 meters will not be used.

For Point Subsample, put 1.

The point_subsample parameter controls how many points are used from a 3D point cloud. It lets you reduce the density of points by only using every nth point, where n is the value of point_subsample. This allows you to balance between having detailed information and faster processing times, depending on your specific needs.

A point_subsample of 1 means you use every point from the point cloud.

For Padding Offset, put 0.1 (units are centimeters).

For robotic arms, padding offset means:

  • The arm treats objects as slightly larger than they actually are.
  • This extra “cushion” helps prevent the arm from accidentally bumping into things.
  • It allows the arm to plan safer movements, especially in tight spaces or when working close to objects.

For the Padding Scale, put 1.0.

Padding scale determines how much the virtual “cushion” around objects grows based on their size. A larger padding scale means bigger objects get proportionally more padding than smaller ones. This allows the robot to adjust its safety margin based on the size of objects it’s working around, potentially being more cautious around larger items.

For the Filtered Cloud Topic, put filtered_cloud.

This topic is used for debugging. It is the filtered point cloud that is produced after it is processed with the parameters we set above.

For Max Update Rate, let’s put 1.0. We can always increase this value later after testing. 

This max update rate indicates the number of times per second the system will refresh or update its understanding of the environment around the robot.

22-3d-perception-update-rate

Launch Files

Click the Launch Files pane.

Here is a default list of launch files that the MoveIt Setup Assistant will generate. You can keep the defaults. 

23-launch-files-panel

Here is a brief description of what each launch file does:

  • Robot State Publisher Launch: Publishes the robot description and transforms generated by the joint states.
  • RViz Launch and Config: Configures and launches RViz for visualizing the robot model and its planned movements.
  • MoveGroup Launch: Launches the MoveIt! MoveGroup node for motion planning.
  • Static TF Launch: Broadcasts static transforms needed for the robot model.
  • Spawn Controllers Launch: Launches and configures the robot’s controllers.
  • Demo Launch: Provides a sample launch file for demonstrating basic functionalities.
  • Setup Assistant Launch: Launches the MoveIt! Setup Assistant for configuring the robot.
  • Warehouse DB Launch: Launches the database used for storing complete planning scenes and robot states.

Add Author Information

Click on the Author Information pane.

Add your name and email address if you like.

24-author-information

Generate Configuration Files

Now let’s generate the configuration files you will need to launch MoveIt.

Click on the Configuration Files pane.

Write a location and a name for the ROS 2 package. I will write this in the line:

/home/ubuntu/ros2_ws/src/mycobot_ros2/mycobot_moveit_config
25-configuration-files

Click the Generate Package button.

26-successful-generation

Go into the package and have a look at the configuration files. To understand more about each configuration file, check out this link

Click Exit Setup Assistant to close the MoveIt Setup Assistant.

You can take a look at all your new files now:

cd /home/ubuntu/ros2_ws/src/mycobot_ros2/mycobot_moveit_config

Build the Workspace

cd ~/ros2_ws/
colcon build --packages-select mycobot_moveit_config
source ~/.bashrc
27-build-your-package

Launching the Demo

You can now try launching the demo, but you will likely get absolutely no output. I got a lot of warnings and depreciation errors and controllers not loading. Such is life with open source software. 

ros2 launch mycobot_moveit_config demo.launch.py
28-error-city
29-empty-rviz

How to Configure MoveIt 2 for a Simulated Robot Arm

In the previous tutorial, you learned how to leverage ROS 2 Control to move the robotic arm manually by sending raw joint commands to the robot. This exercise is great for learning, but it is not how things are done in a real-world robotics project.

In this tutorial, I will show you how to set up a ROS 2 software called MoveIt 2 to send those commands. MoveIt 2 is a powerful and flexible motion planning framework for robotic manipulators.

Using MoveIt 2 instead of sending manual commands through a terminal window allows for more sophisticated, efficient, and safer motion planning, taking into account obstacles, joint limits, and other constraints automatically.

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

moveit-2-animation-mycobot280-elephant-robotics

Prerequisites

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

How MoveIt 2 Works with ROS 2 Control

Before we dive into the tutorial, let me explain how MoveIt 2 works with ROS 2 Control by walking through a common real-world use case.

Suppose you have a mobile manipulator, which consists of a wheeled robot base with a robotic arm attached to it.

You want to develop an application that performs a pick and place task. 

Your cool app starts by telling MoveIt 2, “Pick up the object at location A and place it at location B.”

MoveIt 2, with help from Nav 2 (the navigation software stack for ROS 2), plans out the detailed movements needed – reaching for the object, grasping it, lifting it, moving to the new location, and placing it down.

These plans are sent to ROS 2 Control’s Controller Management system, which has specific controllers for the arm, gripper, and base.

The arm controller (e.g. JointTrajectoryController), for example, breaks down the plan into specific joint movements and sends these to the Resource Management system.

The Resource Management system communicates these commands through hardware interfaces to the actual robot parts (or their simulated versions).

As the robot executes the pick and place task, sensors in its joints and gripper send back information about position, force, and status.

This feedback flows back up through the system, allowing your app and MoveIt 2 to monitor progress and make adjustments if needed.

This setup allows you to focus on defining high-level tasks while MoveIt 2 and ROS 2 Control handle the complex details of robot control and movement coordination.

Now let’s look at another diagram which shows the specific controller and topic names. This gives us a more detailed view of how MoveIt 2, ROS 2 Control, and the robot components interact:

ros2_control_mobile_manipulator_control_arch_independent_hardware

Your cool application initiates the process, defining tasks like pick and place operations.

MoveIt 2 contains a path planning component that generates trajectory commands. These commands are sent to the Joint Trajectory Controller, which manages the arm’s movements.

For mobile robots, Nav 2 can send twist commands to the Diff Drive Controller, controlling the base’s motion.

The system includes several specialized controllers:

  • Joint Trajectory Controller for arm movements
  • Forward Velocity Controller for speed control (optional…only needed if you need direct velocity control for high-speed operations)
  • Gripper Controller for end-effector actions (e.g. open and close gripper)
  • Diff Drive Controller for mobile base navigation

These controllers communicate with Hardware Interfaces for the arm, gripper, mobile base, and the Gazebo simulation.

As the robot operates, sensor data flows back through these Hardware Interfaces. This information includes position, velocity, force, and status updates.

The Joint State Broadcaster and Force Torque State Broadcaster collect this feedback data.

This state information (e.g. joint angles) is then forwarded to RViz for visualization and to MoveIt 2 for real-time monitoring and adjustment.

Create a Package

Now that you understand how all the pieces work together on a high level, let’s start setting up our robot with MoveIt 2.

Create a new package called mycobot_moveit_config_manual_setup:

cd ~/ros2_ws/src/mycobot_ros2/
ros2 pkg create --build-type ament_cmake --license BSD-3-Clause mycobot_moveit_config_manual_setup
cd mycobot_moveit_config_manual_setup

Fill in the package.xml file with your relevant information.

gedit package.xml
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>mycobot_moveit_config_manual_setup</name>
  <version>0.0.0</version>
  <description>Contains the files that enable the robot to integrate with Move It 2.</description>
  <maintainer email="automaticaddison@todo.com">Automatic Addison</maintainer>
  <license>BSD-3-Clause</license>

  <buildtool_depend>ament_cmake</buildtool_depend>
  
  <depend>moveit_ros_planning_interface</depend>
  <depend>moveit_ros_move_group</depend>
  <depend>moveit_kinematics</depend>
  <depend>moveit_planners_ompl</depend>
  <depend>moveit_ros_visualization</depend>
  <depend>rviz2</depend>
  <depend>rviz_visual_tools</depend> 

  <exec_depend>moveit_simple_controller_manager</exec_depend>
  <exec_depend>moveit_configs_utils</exec_depend>
  <exec_depend>pilz_industrial_motion_planner</exec_depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

Save the file, and close it.

In a real robotics project, the naming convention for this package would be:

<robot_name>_moveit_config

Create the Configuration Files

Let’s create configuration files that will enable us to tailor our robot for MoveIt 2. We’ll do this process manually first (recommended), and then we will work through how to do this using the MoveIt Setup Assistant

You can see some official examples of configuration files for other robots on the MoveIt resources ROS 2 GitHub.

Create the Config Folder

MoveIt 2 requires configuration files to go into the config folder of the package you created.

Open a terminal window, and type:

cd ~/ros2_ws/src/mycobot_ros2/mycobot_moveit_config_manual_setup/
mkdir config && cd config

Add SRDF

The first thing we need to do is set up the SRDF File. 

Semantic Robot Description Format (SRDF) is a file that tells MoveIt how your robot prefers to move and interact with its environment. It is a supplement to the URDF file.

The SRDF adds semantic (meaning-based) information to the robot’s description that isn’t included in the URDF (Unified Robot Description Format). This file includes things like:

  • Naming groups of joints or links (e.g., “arm”, “gripper”)
  • Defining default robot poses (e.g., “home”, “closed”, etc.)
  • Which parts of the robot should be checked for colliding with each other during movement.
    • We need to do this step because checking every possible collision between all parts of the robot for each planned movement can be time-consuming, especially for complex robots.
cd ~/ros2_ws/src/mycobot_ros2/mycobot_moveit_config_manual_setup/config/
gedit mycobot_280.srdf

Add this code.

Save the file, and close it.

Let’s go through each section briefly:

  1. <group name=”arm”>: This section defines a group named “arm” that includes a list of joints. The joints listed in this group represent the robot arm.
  2. <group name=”gripper”>: This section defines a group named “gripper” that includes a list of joints related to the robot’s gripper or end-effector.
  3. <group_state name=”home” group=”arm”>: This section defines a named state called “home” for the “arm” group. It specifies the joint values for the arm joints when the robot is in its home position.
  4. <group_state name=”open” group=”gripper”>: This section defines a named state called “open” for the “gripper” group, specifying the joint value for the gripper when it is in its open position.
  5. <disable_collisions>: This section is used to disable collision checking between specific pairs of links. Each <disable_collisions> entry specifies two links and the reason for disabling collision checking between them. The reasons can be “Adjacent” (for adjacent links in the robotic arm), “Never” (for links that will never collide), or “Default” (for links that are not expected to collide in most configurations).

Add Initial Positions

Now let’s create a configuration file for the initial positions.

cd ~/ros2_ws/src/mycobot_ros2/mycobot_moveit_config_manual_setup/config/
gedit initial_positions.yaml

Add this code.

# Default initial position for each movable joint
initial_positions:
  link1_to_link2: 0.0
  link2_to_link3: 0.0
  link3_to_link4: 0.0
  link4_to_link5: 0.0
  link5_to_link6: 0.0
  link6_to_link6flange: 0.0
  gripper_controller: 0.0

Save the file. This code sets the default initial positions for each movable joint. 

The initial_positions.yaml file in MoveIt specifies the starting joint positions for motion planning, overriding any default positions set in the URDF. It allows you to set a specific initial configuration for your robot in MoveIt without modifying the URDF, providing flexibility in defining the starting point for motion planning tasks.

Add Joint Limits

gedit joint_limits.yaml

Add this code.

default_velocity_scaling_factor: 1.0
default_acceleration_scaling_factor: 1.0

joint_limits:
  link1_to_link2:
    has_velocity_limits: true
    max_velocity: 2.792527
    has_acceleration_limits: true
    max_acceleration: 5.0
    has_deceleration_limits: true
    max_deceleration: -5.0
    has_jerk_limits: false
    max_jerk: 6.0
  link2_to_link3:
    has_velocity_limits: true
    max_velocity: 2.792527
    has_acceleration_limits: true
    max_acceleration: 5.0
    has_deceleration_limits: true
    max_deceleration: -5.0
    has_jerk_limits: false
    max_jerk: 6.0
  link3_to_link4:
    has_velocity_limits: true
    max_velocity: 2.792527
    has_acceleration_limits: true
    max_acceleration: 5.0
    has_deceleration_limits: true
    max_deceleration: -5.0
    has_jerk_limits: false
    max_jerk: 6.0
  link4_to_link5:
    has_velocity_limits: true
    max_velocity: 2.792527
    has_acceleration_limits: true
    max_acceleration: 5.0
    has_deceleration_limits: true
    max_deceleration: -5.0
    has_jerk_limits: false
    max_jerk: 6.0
  link5_to_link6:
    has_velocity_limits: true
    max_velocity: 2.792527
    has_acceleration_limits: true
    max_acceleration: 5.0
    has_deceleration_limits: true
    max_deceleration: -5.0
    has_jerk_limits: false
    max_jerk: 6.0
  link6_to_link6flange:
    has_velocity_limits: true
    max_velocity: 2.792527
    has_acceleration_limits: true
    max_acceleration: 5.0
    has_deceleration_limits: true
    max_deceleration: -5.0
    has_jerk_limits: false
    max_jerk: 6.0
  gripper_controller:
    has_velocity_limits: true
    max_velocity: 2.792527
    has_acceleration_limits: true
    max_acceleration: 5.0
    has_deceleration_limits: true
    max_deceleration: -5.0
    has_jerk_limits: false
    max_jerk: 300.0

Save the file.

This code sets the velocity and acceleration limits for each joint in our robotic arm, which is used by MoveIt 2 to control the robot’s movements.

Even though we have joint limits for velocity already defined in the URDF, this YAML file allows for overriding. Defining joint limits specific to MoveIt 2 is useful if you want to impose stricter or different limits for particular applications without modifying the URDF. It provides flexibility in adjusting the robot’s behavior for different tasks.

Add Kinematics

gedit kinematics.yaml

Add this code, and save the file. 

# Configures how a robot calculates possible positions and movements of its joints 
# to reach a desired point. It specifies which mathematical methods (solvers) to use, 
# how long to try, and how precise these calculations should be.
arm: # Name of the joint group from the SRDF file
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin # Plugin used for solving kinematics
  kinematics_solver_search_resolution: 0.005 #  Granularity (in radians for revolute joints) used by the solver when searching for a solution.
  kinematics_solver_timeout: 0.05 # Maximum time the solver will spend trying to find a solution for each planning attempt
  position_only_ik: false # If true, the solver will only consider the position of the end effector, not its orientation
  kinematics_solver_attempts: 5 # How many times the solver will try to find a solution

# Typically we don't need a complex kinematics solver for the gripper. This is for demonstration purposes only.  
gripper:
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.05
  position_only_ik: false
  kinematics_solver_attempts: 3
  
arm_with_gripper:
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.05
  position_only_ik: false
  kinematics_solver_attempts: 5

This file configures how MoveIt 2 calculates possible positions and movements of the robot’s arm to reach a desired point in space. It specifies the mathematical methods, precision, and time limits for these calculations, which MoveIt 2 uses to plan the robot’s movements.

Add Controllers

gedit moveit_controllers.yaml

Add this code, and save the file. 

# This file configures the controller management for MoveIt, specifying which controllers are available,
# the joints each controller handles, and the type of interface (e.g. FollowJointTrajectory).
moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager  # Specifies the controller manager to use.

# Define the controllers configured using the ROS 2 Control framework.
moveit_simple_controller_manager:
  controller_names:
    - arm_controller   # Name of the controller for the robotic arm.
    - grip_controller  # Name of the controller for the gripper.
    - grip_action_controller # Other controller for the gripper (recommended)

  # Configuration for the arm_controller.
  arm_controller:
    action_ns: follow_joint_trajectory   # ROS action namespace to which MoveIt sends trajectory commands.
    type: FollowJointTrajectory          # Type of the controller interface; follows a joint trajectory.
    default: true                        # This controller is the default for the joints listed below.
    joints:                              # List of joints that the arm_controller manages.
      - link1_to_link2
      - link2_to_link3
      - link3_to_link4
      - link4_to_link5
      - link5_to_link6
      - link6_to_link6flange

  # Configuration for the grip_controller.
  grip_controller:
    action_ns: follow_joint_trajectory  
    type: FollowJointTrajectory         
    default: false                    
    joints:                           
      - gripper_controller      
  
  # Configuration for the grip_action_controller.    
  grip_action_controller:  
    action_ns: gripper_cmd 
    type: GripperCommand
    default: true
    joints:
      - gripper_controller       


This yaml file tells MoveIt 2 how to convert its motion plans into instructions that the ROS 2 controllers can understand and execute on the real (or simulated) robot. Without this, MoveIt 2 wouldn’t know how to make the robot actually move using the ROS 2 control system.

MoveIt 2 sends desired joint positions or trajectories to ROS 2 controllers, which then manage the low-level control of the robot’s motors. This integration allows MoveIt 2 to focus on high-level motion planning while relying on ROS 2 control for the actual execution on the robot hardware.

We see that three ROS 2 controllers are available:

  • arm_controller: This is a Joint Trajectory Controller for the robot arm
  • grip_controller: This is a Joint Trajectory Controller for the gripper (optional)
  • grip_action_controller: This is a Gripper Action Controller for the gripper

We also see a list of joints associated with each controller.

The arm controller uses the “FollowJointTrajectory” action interface. The FollowJointTrajectory action interface is a standardized interface in ROS (Robot Operating System) for controlling the joint positions of a robot over time. It allows MoveIt 2 to send a trajectory, which is a sequence of joint positions and their corresponding timestamps, to the robot’s joint controllers.

How it works:

  1. MoveIt 2 plans a movement, which results in a series of joint positions over time.
  2. This plan is packaged into a “trajectory” – think of it as a list of goal positions (in radians) for each joint.
  3. MoveIt 2 sends this trajectory to the appropriate ROS 2 controller using the FollowJointTrajectory action interface. This interface is like a protocol MoveIt 2 uses to communicate with the controllers.
  4. The ROS 2 controller receives this trajectory, processes it, and relays it to the hardware interface.
  5. The hardware interface sends the commands to the microcontroller (e.g. Arduino) to move the robot’s joints accordingly.

Add Cartesian Speed and Acceleration Limits

For motion planning for our robotic arm, we are going to use the Pilz industrial motion planner.

Pilz focuses on simple, predictable movements like straight lines, circles, and direct point-to-point paths. It works by calculating a clear, predefined route for each part of the robot to follow, ensuring smooth and safe motion from start to finish.

Think of Pilz as a careful driver that always takes known, reliable routes rather than trying to find creative shortcuts, making it perfect for precise tasks where you want consistent, controlled movements.

While default planners like OMPL (Open Motion Planning Library) work best for dynamic environments where you can have unexpected obstacles (people moving around, objects left on tabletops and counters, etc.), the structured and predictable nature of Pilz is advantageous in a scenario where the environment is well-structured and controlled.

Open a new file where we will define the Cartesian limits:

gedit pilz_cartesian_limits.yaml  
# Defines the Cartesian movement limits for a robotic arm using the Pilz Planner.
# The values are typically determined based on a combination of the robot manufacturer's 
# specifications, practical testing, and considerations for the specific tasks the robot will perform.
cartesian_limits:
  max_trans_vel: 1.0  # Maximum translational velocity in meters per second. Specifies how fast the end effector can move in space.
  max_trans_acc: 2.25 # Maximum translational acceleration in meters per second squared. Determines how quickly the end effector can increase its speed.
  max_trans_dec: -5.0 # Maximum translational deceleration in meters per second squared. Specifies how quickly the end effector can decrease its speed.
  max_rot_vel: 1.57   # Maximum rotational velocity in radians per second. Controls the speed at which the end effector can rotate.

Save the file, and close it.

This file sets speed and acceleration limits for a robotic arm’s movement in space. It defines how fast and quickly the robot’s end effector can move and rotate in Cartesian space (X, Y, Z direction) to ensure safe and efficient operation within the robot’s workspace.

Why do we need this file if we have already set velocity limits inside the URDF?

The URDF and the MoveIt configuration files serve different purposes when it comes to controlling the robot’s motion. The URDF contains velocity limits for each individual joint, which determine how fast each joint can move. These limits are important for controlling the speed of the individual joints. For example:

<joint name="joint_name" type="revolute">
  <limit lower="-2.879793" upper="2.879793" effort="5.0" velocity="2.792527"/>
</joint>

On the other hand, the cartesian_limits in the MoveIt configuration are used to control the speed and motion of the robot’s gripper in 3D space. These limits define how fast the gripper can move and rotate in the world coordinate frame (X, Y, Z axes). The cartesian_limits are essential for planning the overall motion of the robot and ensuring that it moves safely and efficiently when performing tasks.

So, while the URDF velocity limits are important for controlling individual joints, the MoveIt cartesian_limits are still necessary for proper motion planning and control of the gripper’s movement in 3D space.

By the way, in the context of the file name “pilz_cartesian_limits.yaml”, “pilz” refers to Pilz GmbH & Co. KG, a German automation technology company that specializes in safe automation solutions, including robotics.

Add a Planning Pipeline File for the Pilz Industrial Motion Planner

To make all this work, we need to add a file called pilz_industrial_motion_planner_planning_planner.yaml.

Here is an example file from MoveIt’s GitHub

This file preprocesses the robot’s motion planning request to make sure it meets certain safety and feasibility criteria, like ensuring the robot’s starting position is safe and within allowed limits before planning its movement.

cd ~/ros2_ws/src/mycobot_ros2/mycobot_moveit_config_manual_setup/config/
gedit pilz_industrial_motion_planner_planning.yaml

Add this code.

planning_plugin: pilz_industrial_motion_planner/CommandPlanner
request_adapters: >-
  default_planner_request_adapters/FixWorkspaceBounds
  default_planner_request_adapters/FixStartStateBounds
  default_planner_request_adapters/FixStartStateCollision
  default_planner_request_adapters/FixStartStatePathConstraints
default_planner_config: PTP
capabilities: >-
    pilz_industrial_motion_planner/MoveGroupSequenceAction
    pilz_industrial_motion_planner/MoveGroupSequenceService

Save the file, and close it.

Add an RViz Configuration File

Let’s create a file that sets the parameters for visualizing all this in RViz.

cd ~/ros2_ws/src/mycobot_ros2/mycobot_moveit_config_manual_setup
mkdir rviz && cd rviz
gedit move_group.rviz

Add this code.

Save the file, and close it.

Create a Launch File

Let’s create a launch file.

Open a terminal window, and type:

cd ~/ros2_ws/src/mycobot_ros2/mycobot_moveit_config_manual_setup

Create a new launch folder.

mkdir launch && cd launch
gedit move_group.launch.py

Add this code.

# Author: Addison Sears-Collins
# Date: July 31, 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/move_group.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')

    # Load the robot configuration
    # 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="pilz_industrial_motion_planner"
        )
        .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()
    )
    
    # 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}},
        ],
    )

    # 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
    ld.add_action(declare_use_sim_time_cmd)
    ld.add_action(declare_use_rviz_cmd)

    # Add any actions
    ld.add_action(start_move_group_node_cmd)
    ld.add_action(start_rviz_node_cmd)
    
    # Clean shutdown of RViz
    ld.add_action(exit_event_handler)

    return ld

Save the file, and close it.

This launch file launches MoveIt and RViz.

Looking at the source code for the moveit_configs_builder helped me to complete this launch file.

Create a Setup Assistant File

The .setup_assistant file is typically created automatically when you use the MoveIt Setup Assistant to configure your robot.

Create a new file named .setup_assistant in the root directory of your MoveIt configuration package (e.g., mycobot_moveit_config_manual_setup).

cd ~/ros2_ws/src/mycobot_ros2/mycobot_moveit_config_manual_setup/
gedit .setup_assistant

Add this code:

moveit_setup_assistant_config:
  URDF:
    package: mycobot_gazebo
    relative_path: urdf/ros2_control/gazebo/mycobot_280.urdf.xacro
    xacro_args: ""
  SRDF:
    relative_path: config/mycobot_280.srdf
  CONFIG:
    author_name: AutomaticAddison
    author_email: addison@todo.com
    generated_timestamp: 1690848000

Save the file, and close it.

Set file permissions.

sudo chmod 644 .setup_assistant

Edit package.xml

Let’s edit the package.xml file with the dependencies.

cd ~/ros2_ws/src/mycobot_ros2/mycobot_moveit_config_manual_setup
gedit package.xml

Add this code.

Save the file, and close it.

Edit CMakeLists.txt

Update CMakeLists.txt with the new folders we have created.

cd ~/ros2_ws/src/mycobot_ros2/mycobot_moveit_config_manual_setup
gedit CMakeLists.txt

Add this code.

cmake_minimum_required(VERSION 3.8)
project(mycobot_moveit_config_manual_setup)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
find_package(moveit_ros_move_group REQUIRED)
find_package(moveit_kinematics REQUIRED)
find_package(moveit_planners_ompl REQUIRED)
find_package(moveit_ros_visualization REQUIRED)
find_package(rviz2 REQUIRED)
find_package(rviz_visual_tools REQUIRED) 

# Copy necessary files to designated locations in the project
install (
  DIRECTORY config launch rviz scripts
  DESTINATION share/${PROJECT_NAME}
)

# Install the .setup_assistant file
install(
  FILES .setup_assistant
  DESTINATION share/${PROJECT_NAME}
)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

Save the file, and close it.

Build the Package

Now let’s build the package.

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

Launch the Demo and Configure the Plugin

Begin by launching the robot in Gazebo along with all the controllers (i.e. arm_controller, grip_controller). Since we are going to launch RViz using the MoveIt launch file, we can set the use_rviz flag to False. 

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

This will:

  • Start Gazebo
  • Spawn the robot
  • Start the robot_state_publisher
  • Load and start the necessary controllers (joint_state_broadcaster, arm_controller, grip_controller)

If you look in the logs, you will see the joint_state_broadcaster, arm_controller, and grip_controller are loaded and activated.

Launch MoveIt and RViz.

ros2 launch mycobot_moveit_config_manual_setup move_group.launch.py 

This will:

  • Start the move_group node
  • Launch RViz with MoveIt configuration
  • Use the simulation time from Gazebo

Rather than launching two launch files one after the other, you can put them both into a bash script like this (call the file some name like “robot.sh”):

#!/bin/bash
# Single script to launch the mycobot with Gazebo, RViz, and MoveIt 2

cleanup() {
    echo "Restarting ROS 2 daemon to cleanup before shutting down all processes..."
    ros2 daemon stop
    sleep 1
    ros2 daemon start
    echo "Terminating all ROS 2-related processes..."
    kill 0
    exit
}

trap 'cleanup' SIGINT;
ros2 launch mycobot_gazebo mycobot_280_arduino_bringup_ros2_control_gazebo.launch.py use_rviz:=false &
sleep 10
ros2 launch mycobot_moveit_config_manual_setup move_group.launch.py

Be sure to set permissions to this bash script before running it.

sudo chmod +x robot.sh

Then run this command to launch everything at once.

bash robot.sh

Here is what your screen should look like when everything is loaded properly. You will notice that on the right-hand side of Gazebo I closed the two panels. You can close them by right-clicking and hitting “Close”.

2-loading-gz-sim-gazebo-moveit2

Let’s check out the active topics:

ros2 topic list
3-ros2-topic-list

The active nodes are:

ros2 node list
4-ros2-node-list

Here are our available action servers:

ros2 action list
5-ros2-action-list

Here are our services. You will see we have a lot of services. This page at the ROS 2 tutorials shows you how we can call services from the command line.

ros2 service list

Let’s check out the parameters for the different nodes my launching a GUI tool called rqt_reconfigure:

sudo apt install ros-$ROS_DISTRO-rqt-reconfigure
rqt --standalone rqt_reconfigure
9-parameters-rqt-reconfigure

Click “Refresh”.

Click on any of the nodes to see the parameters for that node. For example, let’s take a look at the moveit_simple_controller_manager node.

This tool enables you to see if the parameters you set in the config file of your MoveIt package were set correctly. 

Explore MoveIt in RViz

We will now roughly follow this official MoveIt Quickstart in RViz tutorial which walks you through the main features provided by the MoveIt RViz plugins.

First, click on the Displays panel in the upper left.

Under Global Options, make sure the Fixed Frame is set to “base_link”

Scroll down to the MotionPlanning plugin.

Make sure the Robot Description field is set to robot_description.

11-set-to-robot-description

Make sure the Planning Scene Topic field is set to /monitored_planning_scene. 

Make sure the Trajectory Topic under Planned Path is set to /display_planned_path.

12-display-planned-path

In the Planning Request section,  make sure the Planning Group is set to arm. You can also see this in the MotionPlanning panel in the bottom left.

13-planning-group-arm

Play With the Robotic Arm

Rviz has four different visualizations for MoveIt. These visualizations are overlapping.

  1. The robot’s configuration in the /monitored_planning_scene planning environment:
    • Planning Scene -> Scene Robot -> Show Robot Visual checkbox
  2. The planned path for the robot:
    • Motion Planning -> Planned Path -> Show Robot Visual checkbox
  3. Green: The start state for motion planning:
    • MotionPlanning -> Planning Request -> Query Start State checkbox
  4. Orange: The goal state for motion planning:
    • MotionPlanning -> Planning Request -> Query Goal State checkbox

You can toggle each of these visualizations on and off using the checkboxes in the Displays panel.

Interact With the Robotic Arm

Now let’s configure our RViz so that all we have are the scene robot, the starting state, and the goal state.

  1. Check the Motion Planning -> Planned Path -> Show Robot Visual checkbox
  2. Uncheck the Planning Scene -> Scene Robot -> Show Robot Visual checkbox
  3. Uncheck the MotionPlanning -> Planning Request -> Query Start State checkbox
  4. Check the MotionPlanning -> Planning Request -> Query Goal State checkbox

The orange colored robot represents the desired Goal State

14-orange-colored-goal-state

Find the “Joints” tab in the bottom MotionPlanning panel. This lets you control individual joints of the robot.

Try moving the sliders for the joints, and watch how the robot arm changes its pose. 

Uncheck the MotionPlanning -> Planning Request -> Query Goal State checkbox.

Check the MotionPlanning -> Planning Request -> Query Start State checkbox.

The green colored robot is used to set the Start State for motion planning.

15-start-state

Use Motion Planning

Now it’s time to plan a movement for your robotic arm using RViz and MoveIt 2.

In the Displays panel in RViz, do the following…

Uncheck the MotionPlanning -> Planning Request -> Query Goal State checkbox

Check the MotionPlanning -> Planning Request -> Query Start State checkbox.

You can see if any links are in collision by going to the MotionPlanning window at the bottom and moving to the “Status” tab.

Move the green (Start State) to where you want the movement to begin.

Rather than using the interactive marker, I find it easier to go to the “Joints” tab in the Motion Planning window at the bottom. This lets you control individual joints of the robot to get your desired Start State.

16-joints-panel

Uncheck the MotionPlanning -> Planning Request -> Query Start State checkbox.

Check the MotionPlanning -> Planning Request -> Query Goal State checkbox.

Move the orange (Goal State) to where you want the movement to end.

Check the MotionPlanning -> Planning Request -> Query Start State checkbox.

If you have a strong computer, you can check the MotionPlanning -> Planned Path -> Show Trail checkbox. This will show you a trail of the planned path. This step is optional.

17-show-trail

Now let’s plan a motion. Look for the “MotionPlanning” window at the bottom-left part of the screen.

Click the Context tab.

16b-ptp
  • Select CIRC (circular motion) if you want the end-effector to move in a circular arc.
  • Select LIN (linear motion) if you want the end effector to move in a straight line between the start and goal positions
  • Select PTP (point-to-point motion) if you want the quickest joint movement between start and goal configurations.
pilz
Image Source: Picknik Robotics

I will click PTP.

Now click the Planning tab.

18-motion-planning-velocity-acceleration-scaling

Click the Plan button to generate a motion plan between the start and goal state.

The system will calculate a path, and you should see a preview of the planned motion in the 3D visualization window.

Review the planned path carefully to ensure it’s appropriate and collision-free.

If you’re not satisfied with the plan, you can adjust the start and goal states in the 3D view and plan again.

Once you’re happy with the plan, click the Plan & Execute button to make the robot carry out the planned motion.

You should see your simulated robotic arm moving in both Gazebo and RViz.

If you want to speed things up, you can tweak the Velocity Scaling figures on the Planning tab. You can make them all 1.0 for example.

Then click Plan & Execute again.

Keep setting new Goal States and moving your arm to different poses. 

Be careful not to move your robot into a Goal State where the robot appears red. Red links on your robot mean the robot is colliding with itself.

19-red-colliding-links

If you want to open and close the gripper. The steps are the same.

Go to the MotionPlanning window at the bottom-left.

Go to the Planning tab.

Change the Planning Group to gripper.

Go to the Context tab.

Select PTP under the pilz_industrial_motion_planner.

Click the Planning tab again.

Select closed as the Goal State. Remember this is one of the states we configured in the SRDF file.

Click Plan & Execute. No need to click Plan first and then Execute since this is a simple motion.

You should see the gripper close in Gazebo.

20-gripper-closed

You can also make motion plans for the combined arm and gripper group. Have fun with it, and play around to get a feel for MoveIt2 in Rviz. 

Don’t forget that every time you change a planning group, you need to go to the Context tab and select PTP.

You can try other planners other than Pilz if you like. 

I have set up OMPL and STOMP as some alternative motion planning libraries. Just use the dropdown menu on the Context tab.

The two parameters files that are required to use OMPL (ompl_planning.yaml) and STOMP (stomp_planning.yaml) are in the mycobot_moveit_config_manual_setup/config folder.

A good planner to use if you are in doubt of what to use is the ompl/RRTConnectkConfigDefault planner. It works well for a wide variety of use cases.

Introspecting Trajectory Waypoints

Understanding how your robot moves in detail is important for ensuring smooth and safe operations. Let’s explore a tool that allows us to do just that:

Add a new tool to RViz.

  • Click on the “Panels” menu
  • Choose “Trajectory – Trajectory Slider”
  • You’ll see a new slider appear in RViz
21-panels-trajectory-slider

Plan a movement:

  • Set a new goal pose for the robotic arm.
  • Click the “Plan” button

Use the new Trajectory Slider:

  • Move the slider to see each step of the robot’s planned movement
  • Press the “Play” button to watch the whole movement smoothly
22-trajectory-slider

Remember: If you change where you want the robot to go, always click “Plan” again before using the slider or “Play” button. This makes sure you’re looking at the newest plan, not an old one.

This tool helps you understand exactly how your robot will move from start to finish.

Plan Cartesian Motions

Sometimes, you want your robot to move in a straight line. Here’s how to do that.

Go to the MotionPlanning panel at the bottom-right of the screen.

Click the Planning tab.

Click the Use Cartesian Path checkbox.

What does this do?

  • When checked, the robot will try to move its end-effector in a straight line.
  • This is useful for tasks that need precise, direct movements, like drawing a straight line or moving directly towards an object.

Why is this important?

  • Normal planning might move the robot’s arm in a curved or complex path.
  • Cartesian planning ensures a more predictable, straight-line movement.

Try planning some movements with this option on and off to see the difference in how the robot moves.

That’s it! Keep building!