How to Detect ArUco Markers Using OpenCV and Python

In this tutorial, I will show you how to detect an ArUco Marker in a real-time video stream (i.e. my webcam) using OpenCV (Python). I will follow this tutorial

By the end of this tutorial, you will be able to generate output like this:

detect-aruco-marker

Prerequisites

Create the Code

Open your favorite code editor, and write the following code. I will name my program detect_aruco_marker.py. This program detects an ArUco marker in a real-time video stream (we’ll use the built-in webcam).

#!/usr/bin/env python
 
'''
Welcome to the ArUco Marker Detector!
 
This program:
  - Detects ArUco markers using OpenCV and Python
'''
 
from __future__ import print_function # Python 2/3 compatibility
import cv2 # Import the OpenCV library
import numpy as np # Import Numpy library

# Project: ArUco Marker Detector
# Date created: 12/18/2021
# Python version: 3.8
# Reference: https://www.pyimagesearch.com/2020/12/21/detecting-aruco-markers-with-opencv-and-python/

desired_aruco_dictionary = "DICT_ARUCO_ORIGINAL"

# The different ArUco dictionaries built into the OpenCV library. 
ARUCO_DICT = {
  "DICT_4X4_50": cv2.aruco.DICT_4X4_50,
  "DICT_4X4_100": cv2.aruco.DICT_4X4_100,
  "DICT_4X4_250": cv2.aruco.DICT_4X4_250,
  "DICT_4X4_1000": cv2.aruco.DICT_4X4_1000,
  "DICT_5X5_50": cv2.aruco.DICT_5X5_50,
  "DICT_5X5_100": cv2.aruco.DICT_5X5_100,
  "DICT_5X5_250": cv2.aruco.DICT_5X5_250,
  "DICT_5X5_1000": cv2.aruco.DICT_5X5_1000,
  "DICT_6X6_50": cv2.aruco.DICT_6X6_50,
  "DICT_6X6_100": cv2.aruco.DICT_6X6_100,
  "DICT_6X6_250": cv2.aruco.DICT_6X6_250,
  "DICT_6X6_1000": cv2.aruco.DICT_6X6_1000,
  "DICT_7X7_50": cv2.aruco.DICT_7X7_50,
  "DICT_7X7_100": cv2.aruco.DICT_7X7_100,
  "DICT_7X7_250": cv2.aruco.DICT_7X7_250,
  "DICT_7X7_1000": cv2.aruco.DICT_7X7_1000,
  "DICT_ARUCO_ORIGINAL": cv2.aruco.DICT_ARUCO_ORIGINAL
}
 
def main():
  """
  Main method of the program.
  """
  # Check that we have a valid ArUco marker
  if ARUCO_DICT.get(desired_aruco_dictionary, None) is None:
    print("[INFO] ArUCo tag of '{}' is not supported".format(
      args["type"]))
    sys.exit(0)
    
  # Load the ArUco dictionary
  print("[INFO] detecting '{}' markers...".format(
	desired_aruco_dictionary))
  this_aruco_dictionary = cv2.aruco.Dictionary_get(ARUCO_DICT[desired_aruco_dictionary])
  this_aruco_parameters = cv2.aruco.DetectorParameters_create()
  
  # Start the video stream
  cap = cv2.VideoCapture(0)
  
  while(True):
 
    # Capture frame-by-frame
    # This method returns True/False as well
    # as the video frame.
    ret, frame = cap.read()  
    
    # Detect ArUco markers in the video frame
    (corners, ids, rejected) = cv2.aruco.detectMarkers(
      frame, this_aruco_dictionary, parameters=this_aruco_parameters)
      
    # Check that at least one ArUco marker was detected
    if len(corners) > 0:
      # Flatten the ArUco IDs list
      ids = ids.flatten()
      
      # Loop over the detected ArUco corners
      for (marker_corner, marker_id) in zip(corners, ids):
      
        # Extract the marker corners
        corners = marker_corner.reshape((4, 2))
        (top_left, top_right, bottom_right, bottom_left) = corners
        
        # Convert the (x,y) coordinate pairs to integers
        top_right = (int(top_right[0]), int(top_right[1]))
        bottom_right = (int(bottom_right[0]), int(bottom_right[1]))
        bottom_left = (int(bottom_left[0]), int(bottom_left[1]))
        top_left = (int(top_left[0]), int(top_left[1]))
        
        # Draw the bounding box of the ArUco detection
        cv2.line(frame, top_left, top_right, (0, 255, 0), 2)
        cv2.line(frame, top_right, bottom_right, (0, 255, 0), 2)
        cv2.line(frame, bottom_right, bottom_left, (0, 255, 0), 2)
        cv2.line(frame, bottom_left, top_left, (0, 255, 0), 2)
        
        # Calculate and draw the center of the ArUco marker
        center_x = int((top_left[0] + bottom_right[0]) / 2.0)
        center_y = int((top_left[1] + bottom_right[1]) / 2.0)
        cv2.circle(frame, (center_x, center_y), 4, (0, 0, 255), -1)
        
        # Draw the ArUco marker ID on the video frame
        # The ID is always located at the top_left of the ArUco marker
        cv2.putText(frame, str(marker_id), 
          (top_left[0], top_left[1] - 15),
          cv2.FONT_HERSHEY_SIMPLEX,
          0.5, (0, 255, 0), 2)
 
    # Display the resulting frame
    cv2.imshow('frame',frame)
         
    # If "q" is pressed on the keyboard, 
    # exit this loop
    if cv2.waitKey(1) & 0xFF == ord('q'):
      break
 
  # Close down the video stream
  cap.release()
  cv2.destroyAllWindows()
  
if __name__ == '__main__':
  print(__doc__)
  main()

Save the file, and close it.

You need to have opencv-contrib-python installed and not opencv-python. Open a terminal window, and type:

pip uninstall opencv-python
pip3 install opencv-contrib-python==4.6.0.66

To run the program in Linux for example, type the following command:

python3 detect_aruco_marker.py
detect_aruco_marker-1

If you want to restore OpenCV to the previous version after you’re finished creating the ArUco markers, type:

pip uninstall opencv-contrib-python
pip install opencv-python

To set the changes, I recommend rebooting your computer.

That’s it. Keep building!

How to Control a Robotic Arm Using ROS 2 Control and Gazebo

In this tutorial, I will show you how to set up and control a robotic arm using ROS 2 Control and Gazebo (the classic version and the newer version). ROS 2 Control is a framework for robots that lets you create programs to control their movement. It provides tools for different parts of the control system to work together, like the sensors, the controller software, and the motors.

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

mycobot-280-ros2-control-gazebo-rviz

Prerequisites

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

Here is the official documentation for ROS 2 control.

Gazebo (classic version)

Install ROS 2 Control 

To begin, open a terminal window, and install the ROS 2 Control packages (the packages might already be installed):

sudo apt-get install ros-${ROS_DISTRO}-ros2-control ros-${ROS_DISTRO}-ros2-controllers ros-${ROS_DISTRO}-gazebo-ros2-control

Create the XACRO/URDF Files

Let’s begin by creating some URDF files. Open a new terminal window, and type:

cd ~/ros2_ws/src/mycobot_ros2/mycobot_gazebo/urdf
mkdir -p ros2_control/classic_gazebo
cd ros2_control/classic_gazebo
gedit mycobot_280.urdf.xacro

Add this code. Then save and close.

If you take a look at the file, you will see we have added a <transmissions> element.

The purpose of the <transmission> element is to define how the joint commands from the controllers are mapped to the actual joint motions of the robot.

Historically, in ROS 1, the <transmission> element was commonly used to specify the relationship between the joint commands and the actuator commands. It provided a way to define the gearing ratio, mechanical reduction, and other properties related to the transmission of motion from the actuators to the joints.

In general, when using the ros2_control framework in ROS 2, the <transmission> element is not strictly necessary in the XACRO files. The joint-actuator mapping and control-related configurations are typically handled through the controller configuration YAML files and the hardware interface layer.

gedit mycobot_280_classic_gazebo.xacro

Add this code. Then save and close.

This XML code defines a Gazebo plugin for a robot named “mycobot_280” using the ROS 2 control framework. It specifies the robot description, robot state publisher node, and the location of the controller configuration YAML file for the mycobot_280 robot.

gedit mycobot_280_ros2_control.xacro

Add this code. Then save and close.

This XML code defines the interface between the robot’s hardware and the ROS 2 Control library for a robot named “mycobot_280”. It specifies the joint names, their command and state interfaces, and the minimum and maximum position limits for each joint, including the gripper joints with mimic properties.

If you take a closer look at the file, you can see that ROS 2 Control provides a structured way to handle the hardware interface for different types of robots through a set of standardized components. These components are primarily divided into command interfaces and state interfaces. Here’s an overview of each:

Command Interfaces (write to hardware)

Command interfaces are used for sending commands to hardware components. These commands could be setpoints, desired positions, velocities, or efforts (torque/force) that the hardware should attempt to achieve. Command interfaces are important for motors and are defined based on the type of control you need to exert on a hardware component. Typical command interfaces include:

  • Position Commands: Used to set a desired position for the motors.
  • Velocity Commands: Used to set a desired velocity for the motors.
  • Effort Commands: Used to control motors based on force or torque (e.g. the force to be applied to grasp an object)

These interfaces allow controllers to interact directly with the hardware abstraction layers, issuing commands that the hardware then tries to follow based on its capabilities and feedback mechanisms.

State Interfaces (read from hardware)

State interfaces are used to read the current state of the hardware components. These are essential for monitoring and feedback loops in control systems. State interfaces provide real-time data on various parameters such as position, velocity, effort, and other specific sensor readings. Common types of state interfaces include:

  • Position State: Provides the current position of actuators or sensors.
  • Velocity State: Gives the current velocity of moving parts.
  • Effort State: Indicates the current force or torque being applied by actuators.

In practice, to use ros2_control, you define these interfaces in a YAML configuration file (see next section) where you specify each joint or hardware component along with its corresponding command and state interfaces. This configuration is then loaded and managed by a controller manager, which handles the life cycle and updates of each controller in real-time.

This framework allows developers to focus more on the higher-level control strategies rather than the specifics of each hardware component, promoting reusability and scalability in robotics applications.

Create a YAML Configuration File for the Controller Manager

In this section, we will create a YAML configuration file that defines the controllers and their properties for our robotic arm. The controller manager in ROS 2 Control uses this file to load and configure the appropriate controllers during runtime.

The YAML file allows us to specify the joint controllers, their types, and associated parameters. By creating a well-structured configuration file, we can easily manage and control the behavior of our robotic arm in the Gazebo simulation environment.

We will walk through the process of creating the YAML file step by step, explaining each part and its significance. By the end of this section, you will have a complete configuration file ready to be used with the controller manager in your ROS 2 Control setup.

cd ~/ros2_ws/src/mycobot_ros2/mycobot_gazebo/config
gedit mycobot_280_controllers.yaml

Add this code, and save it.

# controller_manager provides the necessary infrastructure to manage multiple controllers efficiently and robustly.
controller_manager:
  ros__parameters:
    update_rate: 10 # update_rate specifies how often (in Hz) the controllers should be updated.

    # The JointTrajectoryController allows you to send joint trajectory commands to a group 
    # of joints on a robot. These commands specify the desired positions for each joint.     
    arm_controller:
      type: joint_trajectory_controller/JointTrajectoryController

    grip_controller:
      type: joint_trajectory_controller/JointTrajectoryController

    # Responsible for publishing the current state of the robot's joints to the /joint_states 
    # ROS 2 topic
    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

# Define the parameters for each controller
arm_controller:
  ros__parameters:
    joints:
      - link1_to_link2
      - link2_to_link3
      - link3_to_link4
      - link4_to_link5
      - link5_to_link6
      - link6_to_link6flange

    # The controller will expect position commands as input for each of these joints.
    command_interfaces:
      - position
    
    # Tells the controller that it should expect to receive position data as the state 
    # feedback from the hardware interface,
    state_interfaces:
      - position

    # If true, When set to true, the controller will not use any feedback from the system 
    # (e.g., joint positions, velocities, efforts) to compute the control commands. 
    open_loop_control: true

    # When set to true, it allows the controller to integrate the trajectory goals it receives. 
    # This means that if the goal trajectory only specifies positions, the controller will 
    # numerically integrate the positions to compute the velocities and accelerations required 
    # to follow the trajectory.
    allow_integration_in_goal_trajectories: true

grip_controller:
  ros__parameters:
    joints:
      - gripper_controller
      
    command_interfaces:
      - position

    state_interfaces:
      - position

    open_loop_control: true
    allow_integration_in_goal_trajectories: true

Here’s a brief explanation of the different parts of the YAML file:

1. controller_manager: This section defines the configuration for the controller manager, which is responsible for managing multiple controllers in the system.

2. arm_controller and grip_controller: These sections define the configuration for the arm and grip controllers, respectively. Both controllers are of type joint_trajectory_controller/JointTrajectoryController, which allows sending joint trajectory commands to a group of joints.

3. joint_state_broadcaster: This section defines the configuration for the joint state broadcaster, which is responsible for publishing the current state of the robot’s joints to the /joint_states ROS 2 topic. It is of type joint_state_broadcaster/JointStateBroadcaster.

4. ros__parameters under each controller:

  • joints: Specifies the list of joints that the controller should control. In this case, the arm controller controls joints from `link1_to_link2` to `link6_to_link6flange`, while the grip controller controls the `gripper_controller` joint.
  • command_interfaces: Specifies the type of command interface the controller expects. In this case, both controllers expect position commands.
  • state_interfaces: Specifies the type of state feedback the controller expects from the hardware interface. Here, both controllers expect position feedback.
  • open_loop_control: When set to true, the controller will not use any feedback from the system (e.g., joint positions, velocities, efforts) to compute the control commands. It will solely rely on the provided commands.
  • allow_integration_in_goal_trajectories: When set to true, the controller will integrate the trajectory goals it receives. If the goal trajectory only specifies positions, the controller will numerically integrate the positions to compute the velocities and accelerations required to follow the trajectory.

Create a Launch File

cd ~/ros2_ws/src/mycobot_ros2/mycobot_gazebo/launch
gedit mycobot_280_arduino_bringup_ros2_control_classic_gazebo.launch.py

Add this code, and save.

Now build the package.

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

Launch Everything and Move the Arm

Now launch everything:

ros2 launch mycobot_gazebo mycobot_280_arduino_bringup_ros2_control_classic_gazebo.launch.py
1-launch-the-ros2-control-launch-file

See the active controllers:

ros2 control list_controllers
2-list-controllers

To get more information, type:

ros2 control list_controllers -v

To see a list of all the commands you can use type:

ros2 control 

Then press Tab twice.

List all the available hardware components that are loaded and ready to be used by the ros2_control controller manager.

ros2 control list_hardware_components
3-hardware-components

Now let’s send a command to the robotic arm to move it.

Let’s check out the topics:

ros2 topic list
4-ros2-topic-list

We need this topic: /arm_controller/joint_trajectory

We can publish a trajectory to that topic to move the arm. Let’s get more information about the topic:

ros2 topic info /arm_controller/joint_trajectory

We need we need to publish a message of type:

trajectory_msgs/msg/JointTrajectory

Remember, here were the list of joints that are controlled by the arm_controller:

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

There are 6 joints in total.

Here is a command you can try:

ros2 topic pub --once /arm_controller/joint_trajectory trajectory_msgs/msg/JointTrajectory "{joint_names: ['link1_to_link2', 'link2_to_link3', 'link3_to_link4', 'link4_to_link5', 'link5_to_link6', 'link6_to_link6flange'], points: [{positions: [-1.345, -1.23, 0.264, -0.296, 0.389, -1.5], time_from_start: {sec: 5, nanosec: 0}}]}"

You can also move it back home now:

ros2 topic pub --once /arm_controller/joint_trajectory trajectory_msgs/msg/JointTrajectory "{joint_names: ['link1_to_link2', 'link2_to_link3', 'link3_to_link4', 'link4_to_link5', 'link5_to_link6', 'link6_to_link6flange'], points: [{positions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], time_from_start: {sec: 5, nanosec: 0}}]}"

To close the gripper, you can type:

ros2 topic pub /grip_controller/joint_trajectory trajectory_msgs/msg/JointTrajectory "{joint_names: ['gripper_controller'], points: [{positions: [-0.70], time_from_start: {sec: 5} } ]}" --once

To open the gripper, you can type:

ros2 topic pub /grip_controller/joint_trajectory trajectory_msgs/msg/JointTrajectory "{joint_names: ['gripper_controller'], points: [{positions: [0.0], time_from_start: {sec: 5} } ]}" --once

That’s it for Gazebo Classic.

Gazebo (new version)

Now let’s run through the process for ROS 2 Control using the newer version of Gazebo. 

Install ROS 2 Control

Open a new terminal window, and type

sudo apt update
sudo apt install ros-${ROS_DISTRO}-gz-ros2-control

Check your installation:

ros2 pkg list | grep gz_ros2_control

You can also install the gz_ros2_control_demos package.

sudo apt install ros-${ROS_DISTRO}-gz-ros2-control-demos

To run the gripper example, type:

ros2 launch gz_ros2_control_demos gripper_mimic_joint_example.launch.py
5-gripper-demo-example-ros2-control-gazebo

To move the gripper, type:

ros2 run gz_ros2_control_demos example_gripper

You can try the differential drive mobile robot example as well:

ros2 launch gz_ros2_control_demos diff_drive_example.launch.py
6-diff-drive-example-ros2-control-gazebo

To move the robot, type:

ros2 run gz_ros2_control_demos example_diff_drive 

Create the XACRO/URDF Files

Open a new terminal window, and type:

cd ~/ros2_ws/src/mycobot_ros2/mycobot_gazebo/urdf/ros2_control
mkdir gazebo
cd gazebo
gedit mycobot_280.urdf.xacro

Add this code. Then save and close.

gedit mycobot_280_gazebo.xacro

Add this code below. Then save and close.

<?xml version="1.0" ?>
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="mycobot_280">
    <gazebo>
        <plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
            <parameters>$(find mycobot_gazebo)/config/mycobot_280_controllers.yaml</parameters>
        </plugin>
    </gazebo>
</robot>
gedit mycobot_280_ros2_control.xacro

Add this code below. Then save and close.

<?xml version="1.0" ?>
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="mycobot_280">

    <ros2_control name="RobotSystem" type="system">
        <hardware>
            <plugin>gz_ros2_control/GazeboSimSystem</plugin>
        </hardware>

        <joint name="link1_to_link2">
            <command_interface name="position">
                <param name="min">-2.879793</param>
                <param name="max">2.879793</param>
            </command_interface>
            <state_interface name="position"/>
        </joint>

        <joint name="link2_to_link3">
            <command_interface name="position">
                <param name="min">-2.879793</param>
                <param name="max">2.879793</param>
            </command_interface>
            <state_interface name="position"/>
        </joint>

        <joint name="link3_to_link4">
            <command_interface name="position">
                <param name="min">-2.879793</param>
                <param name="max">2.879793</param>
            </command_interface>
            <state_interface name="position"/>
        </joint>

        <joint name="link4_to_link5">
            <command_interface name="position">
                <param name="min">-2.879793</param>
                <param name="max">2.879793</param>
            </command_interface>
            <state_interface name="position"/>
        </joint>

        <joint name="link5_to_link6">
            <command_interface name="position">
                <param name="min">-2.879793</param>
                <param name="max">2.879793</param>
            </command_interface>
            <state_interface name="position"/>
        </joint>

        <joint name="link6_to_link6flange">
            <command_interface name="position">
                <param name="min">-3.05</param>
                <param name="max">3.05</param>
            </command_interface>
            <state_interface name="position"/>
        </joint>

        <joint name="gripper_controller">
            <command_interface name="position">
                <param name="min">-0.7</param>
                <param name="max">0.15</param>
            </command_interface>
            <state_interface name="position"/>
        </joint>

        <joint name="gripper_base_to_gripper_left2">
            <param name="mimic">gripper_controller</param>
            <param name="multiplier">1.0</param>
            <command_interface name="position">
                <param name="min">-0.8</param>
                <param name="max">0.5</param>
            </command_interface>
            <state_interface name="position"/>
        </joint>

        <joint name="gripper_left3_to_gripper_left1">
            <param name="mimic">gripper_controller</param>
            <param name="multiplier">-1.0</param>
            <command_interface name="position">
                <param name="min">-0.5</param>
                <param name="max">0.5</param>
            </command_interface>
            <state_interface name="position"/>
        </joint>

        <joint name="gripper_base_to_gripper_right3">
            <param name="mimic">gripper_controller</param>
            <param name="multiplier">-1.0</param>
            <command_interface name="position">
                <param name="min">-0.15</param>
                <param name="max">0.7</param>
            </command_interface>
            <state_interface name="position"/>
        </joint>

        <joint name="gripper_base_to_gripper_right2">
            <param name="mimic">gripper_controller</param>
            <param name="multiplier">-1.0</param>
            <command_interface name="position">
                <param name="min">-0.5</param>
                <param name="max">0.8</param>
            </command_interface>
            <state_interface name="position"/>
        </joint>

        <joint name="gripper_right3_to_gripper_right1">
            <param name="mimic">gripper_controller</param>
            <param name="multiplier">1.0</param>
            <command_interface name="position">
                <param name="min">-0.5</param>
                <param name="max">0.5</param>
            </command_interface>
            <state_interface name="position"/>
        </joint>

    </ros2_control>    
    
</robot>

Create a Launch File

cd ~/ros2_ws/src/mycobot_ros2/mycobot_gazebo/launch
gedit mycobot_280_arduino_bringup_ros2_control_gazebo.launch.py

Add this code below, and save.

# Author: Addison Sears-Collins
# Date: April 29, 2024
# Description: Launch a robotic arm in Gazebo 
import os
from launch import LaunchDescription
from launch.actions import AppendEnvironmentVariable, DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription
from launch.actions import RegisterEventHandler
from launch.conditions import IfCondition
from launch.event_handlers import OnProcessExit
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
import xacro

def generate_launch_description():

  # Constants for paths to different files and folders
  package_name_description = 'mycobot_description'
  package_name_gazebo = 'mycobot_gazebo'

  default_robot_name = 'mycobot_280'
  gazebo_launch_file_path = 'launch'
  gazebo_models_path = 'models'
  rviz_config_file_path = 'rviz/mycobot_280_arduino_view_description.rviz'
  urdf_file_path = 'urdf/ros2_control/gazebo/mycobot_280.urdf.xacro'
  world_file_path = 'worlds/empty.world' # e.g. 'world/empty.world', 'world/house.world'

  # Set the path to different files and folders.  
  pkg_ros_gz_sim = FindPackageShare(package='ros_gz_sim').find('ros_gz_sim')  
  pkg_share_description = FindPackageShare(package=package_name_description).find(package_name_description)
  pkg_share_gazebo = FindPackageShare(package=package_name_gazebo).find(package_name_gazebo)

  default_rviz_config_path = os.path.join(pkg_share_description, rviz_config_file_path)  
  default_urdf_model_path = os.path.join(pkg_share_gazebo, urdf_file_path)
  gazebo_launch_file_path = os.path.join(pkg_share_gazebo, gazebo_launch_file_path)   
  gazebo_models_path = os.path.join(pkg_share_gazebo, gazebo_models_path)
  world_path = os.path.join(pkg_share_gazebo, world_file_path)
  
  # Launch configuration variables specific to simulation
  robot_name = LaunchConfiguration('robot_name')
  rviz_config_file = LaunchConfiguration('rviz_config_file')
  use_robot_state_pub = LaunchConfiguration('use_robot_state_pub')
  use_rviz = LaunchConfiguration('use_rviz')
  use_sim_time = LaunchConfiguration('use_sim_time')
  world = LaunchConfiguration('world')
  
  # Set the default pose
  x = LaunchConfiguration('x')
  y = LaunchConfiguration('y')
  z = LaunchConfiguration('z')
  roll = LaunchConfiguration('roll')
  pitch = LaunchConfiguration('pitch')
  yaw = LaunchConfiguration('yaw')
  
  # Declare the launch arguments  
  declare_robot_name_cmd = DeclareLaunchArgument(
    name='robot_name',
    default_value=default_robot_name,
    description='The name for the robot')

  declare_rviz_config_file_cmd = DeclareLaunchArgument(
    name='rviz_config_file',
    default_value=default_rviz_config_path,
    description='Full path to the RVIZ config file to use')

  declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
    name='use_robot_state_pub',
    default_value='True',
    description='Whether to start the robot state publisher')

  declare_use_rviz_cmd = DeclareLaunchArgument(
    name='use_rviz',
    default_value='True',
    description='Whether to start RVIZ')
    
  declare_use_sim_time_cmd = DeclareLaunchArgument(
    name='use_sim_time',
    default_value='true',
    description='Use simulation (Gazebo) clock if true')

  declare_world_cmd = DeclareLaunchArgument(
    name='world',
    default_value=world_path,
    description='Full path to the world model file to load')

  declare_x_cmd = DeclareLaunchArgument(
    name='x',
    default_value='0.0',
    description='x component of initial position, meters')

  declare_y_cmd = DeclareLaunchArgument(
    name='y',
    default_value='0.0',
    description='y component of initial position, meters')
    
  declare_z_cmd = DeclareLaunchArgument(
    name='z',
    default_value='0.05',
    description='z component of initial position, meters')
    
  declare_roll_cmd = DeclareLaunchArgument(
    name='roll',
    default_value='0.0',
    description='roll angle of initial orientation, radians')

  declare_pitch_cmd = DeclareLaunchArgument(
    name='pitch',
    default_value='0.0',
    description='pitch angle of initial orientation, radians')

  declare_yaw_cmd = DeclareLaunchArgument(
    name='yaw',
    default_value='0.0',
    description='yaw angle of initial orientation, radians')
    
  # Specify the actions

  set_env_vars_resources = AppendEnvironmentVariable(
    'GZ_SIM_RESOURCE_PATH',
    gazebo_models_path)

  # Start arm controller
  start_arm_controller_cmd = ExecuteProcess(
    cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
        'arm_controller'],
        output='screen')

  # Start Gazebo environment
  start_gazebo_cmd = IncludeLaunchDescription(
    PythonLaunchDescriptionSource(
      os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')),
    launch_arguments=[('gz_args', [' -r -v 4 ', world])])

  # Start gripper controller
  start_gripper_controller_cmd =  ExecuteProcess(
    cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
        'grip_controller'],
        output='screen')
  
  # Launch joint state broadcaster
  start_joint_state_broadcaster_cmd = ExecuteProcess(
    cmd=['ros2', 'control', 'load_controller', '--set-state', 'active',
        'joint_state_broadcaster'],
        output='screen')
    
  # Subscribe to the joint states of the robot, and publish the 3D pose of each link.
  doc = xacro.parse(open(default_urdf_model_path))
  xacro.process_doc(doc)
  start_robot_state_publisher_cmd = Node(
    condition=IfCondition(use_robot_state_pub),
    package='robot_state_publisher',
    executable='robot_state_publisher',
    name='robot_state_publisher',
    output='screen',
    parameters=[{
      'use_sim_time': use_sim_time, 
      'robot_description': doc.toxml()}])

  # Launch RViz
  start_rviz_cmd = Node(
    condition=IfCondition(use_rviz),
    package='rviz2',
    executable='rviz2',
    name='rviz2',
    output='screen',
    arguments=['-d', rviz_config_file])  
    
  # Spawn the robot
  start_gazebo_ros_spawner_cmd = Node(
    package='ros_gz_sim',
    executable='create',
    arguments=[
      '-string', doc.toxml(),
      '-name', robot_name,
      '-allow_renaming', 'true',
      '-x', x,
      '-y', y,
      '-z', z,
      '-R', roll,
      '-P', pitch,
      '-Y', yaw
      ],
    output='screen')

  # Launch the joint state broadcaster after spawning the robot
  load_joint_state_broadcaster_cmd = RegisterEventHandler(
     event_handler=OnProcessExit(
     target_action=start_gazebo_ros_spawner_cmd ,
     on_exit=[start_joint_state_broadcaster_cmd],))

  # Launch the arm controller after launching the joint state broadcaster
  load_arm_controller_cmd = RegisterEventHandler(
    event_handler=OnProcessExit(
    target_action=start_joint_state_broadcaster_cmd,
    on_exit=[start_arm_controller_cmd],))

  # Launch the gripper controller after launching the arm controller
  load_gripper_controller_cmd = RegisterEventHandler(
    event_handler=OnProcessExit(
    target_action=start_arm_controller_cmd,
    on_exit=[start_gripper_controller_cmd],))

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

  # Declare the launch options
  ld.add_action(declare_robot_name_cmd)
  ld.add_action(declare_rviz_config_file_cmd)
  ld.add_action(declare_use_robot_state_pub_cmd)  
  ld.add_action(declare_use_rviz_cmd) 
  ld.add_action(declare_use_sim_time_cmd)
  ld.add_action(declare_world_cmd)

  ld.add_action(declare_x_cmd)
  ld.add_action(declare_y_cmd)
  ld.add_action(declare_z_cmd)
  ld.add_action(declare_roll_cmd)
  ld.add_action(declare_pitch_cmd)
  ld.add_action(declare_yaw_cmd)  

  # Add any actions
  ld.add_action(set_env_vars_resources)
  ld.add_action(start_gazebo_cmd)
  ld.add_action(start_robot_state_publisher_cmd)
  ld.add_action(start_rviz_cmd)
  
  ld.add_action(start_gazebo_ros_spawner_cmd)

  ld.add_action(load_joint_state_broadcaster_cmd)
  ld.add_action(load_arm_controller_cmd) 
  ld.add_action(load_gripper_controller_cmd) 

  return ld

Now build the package.

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

Launch Everything and Move the Arm

Now launch everything:

ros2 launch mycobot_gazebo mycobot_280_arduino_bringup_ros2_control_gazebo.launch.py

See the active controllers:

ros2 control list_controllers

To get more information, type:

ros2 control list_controllers -v

To see a list of all the commands you can use type:

ros2 control 

Then press Tab twice.

List all the available hardware components that are loaded and ready to be used by the ros2_control controller manager.

ros2 control list_hardware_components

Now let’s send a command to the robotic arm to move it.

Let’s check out the topics:

ros2 topic list

We need this topic: /arm_controller/joint_trajectory

We can publish a trajectory to that topic to move the arm. Let’s get more information about the topic:

ros2 topic info /arm_controller/joint_trajectory

We need we need to publish a message of type:

trajectory_msgs/msg/JointTrajectory

Remember, here were the list of joints that are controlled by the arm_controller:

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

There are 6 joints in total.

Here is a command you can try:

ros2 topic pub --once /arm_controller/joint_trajectory trajectory_msgs/msg/JointTrajectory "{joint_names: ['link1_to_link2', 'link2_to_link3', 'link3_to_link4', 'link4_to_link5', 'link5_to_link6', 'link6_to_link6flange'], points: [{positions: [-1.345, -1.23, 0.264, -0.296, 0.389, -1.5], time_from_start: {sec: 3, nanosec: 0}}]}"

You can also move it back home now:

ros2 topic pub --once /arm_controller/joint_trajectory trajectory_msgs/msg/JointTrajectory "{joint_names: ['link1_to_link2', 'link2_to_link3', 'link3_to_link4', 'link4_to_link5', 'link5_to_link6', 'link6_to_link6flange'], points: [{positions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], time_from_start: {sec: 3, nanosec: 0}}]}"

To close the gripper, you can type:

ros2 topic pub /grip_controller/joint_trajectory trajectory_msgs/msg/JointTrajectory "{joint_names: ['gripper_controller'], points: [{positions: [-0.70], time_from_start: {sec: 3.0} } ]}" --once

To open the gripper, you can type:

ros2 topic pub /grip_controller/joint_trajectory trajectory_msgs/msg/JointTrajectory "{joint_names: ['gripper_controller'], points: [{positions: [0.0], time_from_start: {sec: 3.0} } ]}" --once

Move the Robotic Arm Using Code

Let’s create a ROS 2 node that can loop through a list of trajectories to simulate the arm moving from the home position to a goal location and then back to home.

Python

Let’s start by creating a script using Python.

cd ~/ros2_ws/src/mycobot_ros2/mycobot_gazebo/scripts
gedit example_joint_trajectory_publisher.py

Add this code and save the file.

#! /usr/bin/env python3

"""
Description:
  ROS 2: Executes a sample trajectory for a robotic arm in Gazebo
-------
Publishing Topics (ROS 2):
  Desired goal pose of the robotic arm
    /arm_controller/joint_trajectory - trajectory_msgs/JointTrajectory
  
  Desired goal pose of the gripper
    /grip_controller/joint_trajectory - trajectory_msgs/JointTrajectory
-------
Author: Addison Sears-Collins
Date: April 29, 2024
"""

import rclpy # Python client library for ROS 2
from rclpy.node import Node # Handles the creation of nodes
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from builtin_interfaces.msg import Duration
from std_msgs.msg import Header  # Import the Header message

# Define constants
arm_joints = [ 'link1_to_link2', 
               'link2_to_link3', 
               'link3_to_link4', 
               'link4_to_link5', 
               'link5_to_link6', 
               'link6_to_link6flange']

gripper_joints = ['gripper_controller']

class ExampleJointTrajectoryPublisherPy(Node):
    """This class executes a sample trajectory for a robotic arm
    
    """      
    def __init__(self):
        """ Constructor.
      
        """
        # Initialize the class using the constructor
        super().__init__('example_joint_trajectory_publisher_py')    
 
        # Create the publisher of the desired arm and gripper goal poses
        self.arm_pose_publisher = self.create_publisher(JointTrajectory, '/arm_controller/joint_trajectory', 1)
        self.gripper_pose_publisher = self.create_publisher(JointTrajectory, '/grip_controller/joint_trajectory', 1)

        self.timer_period = 5.0  # seconds 5.0
        self.timer = self.create_timer(self.timer_period, self.timer_callback)

        self.frame_id = "base_link"
       
        # Desired time from the trajectory start to arrive at the trajectory point.
        # Needs to be less than or equal to the self.timer_period above to allow
        # the robotic arm to smoothly transition between points.
        self.duration_sec = 2 
        self.duration_nanosec = 0.5 * 1e9 # (seconds * 1e9)

        # Set the desired goal poses for the robotic arm.
        self.arm_positions = []
        self.arm_positions.append([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) # Home location
        self.arm_positions.append([-1.345, -1.23, 0.264, -0.296, 0.389, -1.5]) # Goal location
        self.arm_positions.append([-1.345, -1.23, 0.264, -0.296, 0.389, -1.5]) 
        self.arm_positions.append([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]) # Home location

        self.gripper_positions = []
        self.gripper_positions.append([0.0]) # Open gripper
        self.gripper_positions.append([0.0])
        self.gripper_positions.append([-0.70]) # Close gripper
        self.gripper_positions.append([-0.70]) 

        # Keep track of the current trajectory we are executing
        self.index = 0

    def timer_callback(self):
        """Set the goal pose for the robotic arm.
    
        """
        # Create new JointTrajectory messages
        msg_arm = JointTrajectory()
        msg_arm.header = Header()  
        msg_arm.header.frame_id = self.frame_id  
        msg_arm.joint_names = arm_joints

        msg_gripper = JointTrajectory()
        msg_gripper.header = Header()  
        msg_gripper.header.frame_id = self.frame_id  
        msg_gripper.joint_names = gripper_joints

        # Create JointTrajectoryPoints
        point_arm = JointTrajectoryPoint()
        point_arm.positions = self.arm_positions[self.index]
        point_arm.time_from_start = Duration(sec=int(self.duration_sec), nanosec=int(self.duration_nanosec))  # Time to next position
        msg_arm.points.append(point_arm)
        self.arm_pose_publisher.publish(msg_arm)

        point_gripper = JointTrajectoryPoint()
        point_gripper.positions = self.gripper_positions[self.index]
        point_gripper.time_from_start = Duration(sec=int(self.duration_sec), nanosec=int(self.duration_nanosec))  
        msg_gripper.points.append(point_gripper)
        self.gripper_pose_publisher.publish(msg_gripper)

        # Reset the index
        if self.index == len(self.arm_positions) - 1:
            self.index = 0
        else:
            self.index = self.index + 1
    
def main(args=None):
  
    # Initialize the rclpy library
    rclpy.init(args=args)
  
    # Create the node
    example_joint_trajectory_publisher_py = ExampleJointTrajectoryPublisherPy()
  
    # Spin the node so the callback function is called.
    rclpy.spin(example_joint_trajectory_publisher_py)
    
    # Destroy the node
    example_joint_trajectory_publisher_py.destroy_node()
  
    # Shutdown the ROS client library for Python
    rclpy.shutdown()
  
if __name__ == '__main__':
  main()

Add the code to your CMakeLists.txt file.

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

Launch your arm in Gazebo:

ros2 launch mycobot_gazebo mycobot_280_arduino_bringup_ros2_control_gazebo.launch.py

Now run your node:

ros2 run mycobot_gazebo  example_joint_trajectory_publisher.py

C++

Now let’s create the same node in C++.

mkdir ~/ros2_ws/src/mycobot_ros2/mycobot_gazebo/src
mkdir ~/ros2_ws/src/mycobot_ros2/mycobot_gazebo/include
cd src
gedit example_joint_trajectory_publisher.cpp
/**
 * @file example_joint_trajectory_publisher.cpp
 * @brief ROS 2: Executes a sample trajectory for a robotic arm in Gazebo
 * 
 * Publishing Topics (ROS 2):
 *   Desired goal pose of the robotic arm
 *     /arm_controller/joint_trajectory - trajectory_msgs::msg::JointTrajectory
 *   
 *   Desired goal pose of the gripper
 *     /grip_controller/joint_trajectory - trajectory_msgs::msg::JointTrajectory
 * 
 * @author Addison Sears-Collins
 * @date April 29, 2024
 */

#include <chrono>
#include <functional>
#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include "trajectory_msgs/msg/joint_trajectory.hpp"
#include "std_msgs/msg/header.hpp"

using namespace std::chrono_literals;

// Define constants
const std::vector<std::string> arm_joints = {
    "link1_to_link2",
    "link2_to_link3",
    "link3_to_link4",
    "link4_to_link5",
    "link5_to_link6",
    "link6_to_link6flange"
};

const std::vector<std::string> gripper_joints = {
    "gripper_controller"
};

class ExampleJointTrajectoryPublisherCpp : public rclcpp::Node
{
public:
    ExampleJointTrajectoryPublisherCpp()
        : Node("example_joint_trajectory_publisher_cpp")
    {
        // Create the publisher of the desired arm and gripper goal poses
        arm_pose_publisher_ = create_publisher<trajectory_msgs::msg::JointTrajectory>("/arm_controller/joint_trajectory", 1);
        gripper_pose_publisher_ = create_publisher<trajectory_msgs::msg::JointTrajectory>("/grip_controller/joint_trajectory", 1);

        // Create a timer to periodically call the timerCallback function
        timer_ = create_wall_timer(5s, std::bind(&ExampleJointTrajectoryPublisherCpp::timerCallback, this));

        frame_id_ = "base_link";

        // Desired time from the trajectory start to arrive at the trajectory point.
        // Needs to be less than or equal to the timer period above to allow
        // the robotic arm to smoothly transition between points.
        duration_sec_ = 2;
        duration_nanosec_ = 0.5 * 1e9;  // (seconds * 1e9)

        // Set the desired goal poses for the robotic arm.
        arm_positions_ = {
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0},  // Home location
            {-1.345, -1.23, 0.264, -0.296, 0.389, -1.5},  // Goal location
            {-1.345, -1.23, 0.264, -0.296, 0.389, -1.5},
            {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}  // Home location
        };

        gripper_positions_ = {
            {0.0},  // Open gripper
            {0.0},
            {-0.70},  // Close gripper
            {-0.70}
        };

        // Keep track of the current trajectory we are executing
        index_ = 0;
    }

private:
    void timerCallback()
    {
        // Create new JointTrajectory messages for arm and gripper
        auto msg_arm = trajectory_msgs::msg::JointTrajectory();
        msg_arm.header.frame_id = frame_id_;
        msg_arm.joint_names = arm_joints;

        auto msg_gripper = trajectory_msgs::msg::JointTrajectory();
        msg_gripper.header.frame_id = frame_id_;
        msg_gripper.joint_names = gripper_joints;

        // Create JointTrajectoryPoints for arm and gripper
        auto point_arm = trajectory_msgs::msg::JointTrajectoryPoint();
        point_arm.positions = arm_positions_[index_];
        point_arm.time_from_start = rclcpp::Duration(duration_sec_, duration_nanosec_);
        msg_arm.points.push_back(point_arm);
        arm_pose_publisher_->publish(msg_arm);

        auto point_gripper = trajectory_msgs::msg::JointTrajectoryPoint();
        point_gripper.positions = gripper_positions_[index_];
        point_gripper.time_from_start = rclcpp::Duration(duration_sec_, duration_nanosec_);
        msg_gripper.points.push_back(point_gripper);
        gripper_pose_publisher_->publish(msg_gripper);

        // Reset the index 
        if (index_ == arm_positions_.size() - 1) {
            index_ = 0;
        } else {
            index_++;
        }
    }

    // Publishers for arm and gripper joint trajectories
    rclcpp::Publisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr arm_pose_publisher_;
    rclcpp::Publisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr gripper_pose_publisher_;

    // Timer for periodic callback
    rclcpp::TimerBase::SharedPtr timer_;

    // Frame ID for the joint trajectories
    std::string frame_id_;

    // Duration for each trajectory point
    int duration_sec_;
    int duration_nanosec_;

    // Desired goal poses for the robotic arm and gripper
    std::vector<std::vector<double>> arm_positions_;
    std::vector<std::vector<double>> gripper_positions_;

    // Index to keep track of the current trajectory point
    size_t index_;
};

int main(int argc, char * argv[])
{
    // Initialize the ROS 2 client library
    rclcpp::init(argc, argv);

    // Create an instance of the ExampleJointTrajectoryPublisherCpp node
    auto node = std::make_shared<ExampleJointTrajectoryPublisherCpp>();

    // Spin the node to execute the callbacks
    rclcpp::spin(node);

    // Shutdown the ROS 2 client library
    rclcpp::shutdown();

    return 0;
}

Update your CMakeLists.txt file and package.xml file.

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

Launch your arm in Gazebo:

ros2 launch mycobot_gazebo mycobot_280_arduino_bringup_ros2_control_gazebo.launch.py

Now run your node:

ros2 run mycobot_gazebo  example_joint_trajectory_publisher_cpp

Your robotic arm will go repeatedly from the home position to the target position.

7-mycobot-280-new-gazebo

Congratulations! You made it to the end. That’s it! Keep building!

How to Simulate a Robotic Arm in Gazebo – ROS 2

In this tutorial, I will guide you through the process of simulating and performing basic control of a robotic arm in Gazebo. By the end of this tutorial, you will be able to build this:

moving-mycobot-280-gazebo-classic

Gazebo is a robotics simulator that enables the testing and development of robots in a virtual environment. It supports a wide range of robots and integrates seamlessly with ROS 2, facilitating the transition from simulation to real-world application. This makes Gazebo an essential tool for roboticists aiming to prototype and refine algorithms efficiently.

Before we begin, I should advise you that Gazebo has been going through a lot of changes over the last several years. These changes are discussed here. They have changed the name several times and have not fully ported all the ROS plugins from the old classic Gazebo to the new Gazebo simulation engine (some folks still call the new Gazebo, “Ignition”, although it is no longer called Ignition).

For this reason, I have split this tutorial into two sections: Gazebo (new version) and Gazebo Classic (old version). I will show you how to launch and perform basic control of a robotic arm using both Gazebo versions.

Prerequisites

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

Gazebo (new version)

Install Gazebo, ros_gz, and numpy

The first thing we need to do is to install the package that handles the integration between ROS 2 and Gazebo. The name of this package is ros_gz. Here is the GitHub repository, and here are the official installation instructions. Let’s walk through the steps together.

I am using ROS 2 Iron, but regardless of the ROS 2 version you are using, you will need to open a terminal window, and type this command to install the package:

sudo apt-get install ros-${ROS_DISTRO}-ros-gz

Type Y and press Enter to install the package.

The command above installs ros_gz and the correct version of Gazebo for your ROS 2 version.

Now install Numpy, a scientific computing library for Python.

sudo apt install python3 python3-pip
pip3 install numpy

Test Your Installation

To test your installation, we will run the example from this part of the repo.

ros2 launch ros_gz_sim gz_sim.launch.py gz_args:="shapes.sdf"

Here is what you should see:

1-shapes-sdf

Feel free to run the other demos which you can find here.

When you run each demo, you can explore the topics by typing:

ros2 topic list

Create World Files

The first thing we need to do is to create an environment for your simulated robot.

We are going to create a new package named mycobot_gazebo.

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

Create a worlds folder:

mkdir worlds

Also, create other folders we will need later:

mkdir launch
mkdir models

Now let’s create our world. This first world we will create is an empty world.

cd worlds
gedit empty.world

Add this code.

<?xml version="1.0" ?>

<sdf version="1.6">
  <world name="default">
    <!-- Plugin for simulating physics -->
    <plugin
      filename="gz-sim-physics-system"
      name="gz::sim::systems::Physics">
    </plugin>
    
    <!-- Plugin for handling user commands -->
    <plugin
      filename="gz-sim-user-commands-system"
      name="gz::sim::systems::UserCommands">
    </plugin>
    
    <!-- Plugin for broadcasting scene updates -->
    <plugin
      filename="gz-sim-scene-broadcaster-system"
      name="gz::sim::systems::SceneBroadcaster">
    </plugin>

    <!-- To add realistic gravity, do: 0.0 0.0 -9.8, otherwise do 0.0 0.0 0.0 -->
    <gravity>0.0 0.0 -9.8</gravity>
    
    <!-- Include a model of the Sun from an external URI -->
    <include>
      <uri>
        https://fuel.gazebosim.org/1.0/OpenRobotics/models/Sun
      </uri>
    </include>

    <!-- Include a model of the Ground Plane from an external URI -->
    <include>
      <uri>
        https://fuel.gazebosim.org/1.0/OpenRobotics/models/Ground Plane
      </uri>
    </include>
    
    <!-- Define scene properties -->
    <scene>
      <shadows>false</shadows>
    </scene>
    
  </world>
</sdf>

Save the file, and close it.

Now let’s create a world called house.world.

gedit house.world

Add this code.

Save the file, and close it.

Create a models folder. These models are physical objects that will exist inside your house world.

cd ..
mkdir models
cd models

Make sure you put these models inside your models folder.

2a-house-world
Rendering of house.world in Gazebo

Create a URDF File

Now let’s create our URDF file. A URDF (Unified Robot Description Format) file is an XML format file used to describe the physical configuration and properties of a robot in a structured and standard way.

cd ~/ros2_ws/src/mycobot_ros2/mycobot_gazebo/
mkdir urdf
cd urdf
gedit mycobot_280_gazebo.urdf.xacro

Add this code. Then save and close.

You will notice I added plugins at the end of the URDF File. Here is a list of plugins that can be added to our URDF file to create extra functionality. 

The two plugins I added are the JointStatePublisher and the JointPositionController.

The JointStatePublisher publishes the state of all joints in a robot, including positions (radians or meters), velocities (radians per second or meters per second), and efforts (Nm or N) as a sensor_msgs/JointState message.

The JointPositionController subscribes to target joint angles (i.e. positions) as a std_msgs/Float64 message (i.e. a floating-point number like 0.36).

Create the Parameters File

To enable Gazebo to communicate with to ROS 2 topics and vice versa, you need to set up a parameter file that bridges the topics between the two platforms. You can see tutorials of how this works here on GitHub (for ROS 2 Iron and older)

Open a new terminal window, and type:

cd ~/ros2_ws/src/mycobot_ros2/mycobot_gazebo/
mkdir config
cd config
gedit ros_gz_bridge.yaml

Add this code, and then save the file.

Create the Launch File

Let’s create a launch file to spawn both our world and our robotic arm.

cd ~/ros2_ws/src/mycobot_ros2/mycobot_gazebo/
mkdir launch
cd launch
gedit mycobot_280_arduino_bringup_gazebo.launch.py

Add this code.

# Author: Addison Sears-Collins
# Date: April 14, 2024
# Description: Launch a robotic arm in Gazebo 
import os
from launch import LaunchDescription
from launch.actions import AppendEnvironmentVariable, DeclareLaunchArgument, IncludeLaunchDescription
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import Command, LaunchConfiguration, PythonExpression
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue
from launch_ros.substitutions import FindPackageShare

def generate_launch_description():

  # Constants for paths to different files and folders
  package_name_description = 'mycobot_description'
  package_name_gazebo = 'mycobot_gazebo'

  default_robot_name = 'mycobot_280'
  gazebo_launch_file_path = 'launch'
  gazebo_models_path = 'models'
  ros_gz_bridge_config_file_path = 'config/ros_gz_bridge.yaml'
  rviz_config_file_path = 'rviz/mycobot_280_arduino_view_description.rviz'
  urdf_file_path = 'urdf/mycobot_280_gazebo.urdf.xacro'
  world_file_path = 'worlds/empty.world' # e.g. 'world/empty.world', 'world/house.world'

  # Set the path to different files and folders.  
  pkg_ros_gz_sim = FindPackageShare(package='ros_gz_sim').find('ros_gz_sim')  
  pkg_share_description = FindPackageShare(package=package_name_description).find(package_name_description)
  pkg_share_gazebo = FindPackageShare(package=package_name_gazebo).find(package_name_gazebo)

  default_ros_gz_bridge_config_file_path = os.path.join(pkg_share_gazebo, ros_gz_bridge_config_file_path)
  default_rviz_config_path = os.path.join(pkg_share_description, rviz_config_file_path)  
  default_urdf_model_path = os.path.join(pkg_share_gazebo, urdf_file_path)
  gazebo_launch_file_path = os.path.join(pkg_share_gazebo, gazebo_launch_file_path)   
  gazebo_models_path = os.path.join(pkg_share_gazebo, gazebo_models_path)
  world_path = os.path.join(pkg_share_gazebo, world_file_path)
  
  # Launch configuration variables specific to simulation
  headless = LaunchConfiguration('headless')
  robot_name = LaunchConfiguration('robot_name')
  rviz_config_file = LaunchConfiguration('rviz_config_file')
  urdf_model = LaunchConfiguration('urdf_model')
  use_robot_state_pub = LaunchConfiguration('use_robot_state_pub')
  use_rviz = LaunchConfiguration('use_rviz')
  use_sim_time = LaunchConfiguration('use_sim_time')
  use_simulator = LaunchConfiguration('use_simulator')
  world = LaunchConfiguration('world')
  
  # Set the default pose
  x = LaunchConfiguration('x')
  y = LaunchConfiguration('y')
  z = LaunchConfiguration('z')
  roll = LaunchConfiguration('roll')
  pitch = LaunchConfiguration('pitch')
  yaw = LaunchConfiguration('yaw')
  
  # Declare the launch arguments  
  declare_robot_name_cmd = DeclareLaunchArgument(
    name='robot_name',
    default_value=default_robot_name,
    description='The name for the robot')

  declare_rviz_config_file_cmd = DeclareLaunchArgument(
    name='rviz_config_file',
    default_value=default_rviz_config_path,
    description='Full path to the RVIZ config file to use')

  declare_simulator_cmd = DeclareLaunchArgument(
    name='headless',
    default_value='False',
    description='Display the Gazebo GUI if False, otherwise run in headless mode')

  declare_urdf_model_path_cmd = DeclareLaunchArgument(
    name='urdf_model', 
    default_value=default_urdf_model_path, 
    description='Absolute path to robot urdf file')
    
  declare_use_robot_state_pub_cmd = DeclareLaunchArgument(
    name='use_robot_state_pub',
    default_value='True',
    description='Whether to start the robot state publisher')

  declare_use_rviz_cmd = DeclareLaunchArgument(
    name='use_rviz',
    default_value='True',
    description='Whether to start RVIZ')
    
  declare_use_sim_time_cmd = DeclareLaunchArgument(
    name='use_sim_time',
    default_value='true',
    description='Use simulation (Gazebo) clock if true')

  declare_use_simulator_cmd = DeclareLaunchArgument(
    name='use_simulator',
    default_value='True',
    description='Whether to start Gazebo')

  declare_world_cmd = DeclareLaunchArgument(
    name='world',
    default_value=world_path,
    description='Full path to the world model file to load')

  declare_x_cmd = DeclareLaunchArgument(
    name='x',
    default_value='0.0',
    description='x component of initial position, meters')

  declare_y_cmd = DeclareLaunchArgument(
    name='y',
    default_value='0.0',
    description='y component of initial position, meters')
    
  declare_z_cmd = DeclareLaunchArgument(
    name='z',
    default_value='0.05',
    description='z component of initial position, meters')
    
  declare_roll_cmd = DeclareLaunchArgument(
    name='roll',
    default_value='0.0',
    description='roll angle of initial orientation, radians')

  declare_pitch_cmd = DeclareLaunchArgument(
    name='pitch',
    default_value='0.0',
    description='pitch angle of initial orientation, radians')

  declare_yaw_cmd = DeclareLaunchArgument(
    name='yaw',
    default_value='0.0',
    description='yaw angle of initial orientation, radians')
    
  # Specify the actions

  set_env_vars_resources = AppendEnvironmentVariable(
    'GZ_SIM_RESOURCE_PATH',
    gazebo_models_path)
  
  # Start Gazebo server
  start_gazebo_server_cmd = IncludeLaunchDescription(
    PythonLaunchDescriptionSource(
      os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')),
    condition=IfCondition(use_simulator),
    launch_arguments={'gz_args': ['-r -s -v4 ', world], 'on_exit_shutdown': 'true'}.items())

  # Start Gazebo client    
  start_gazebo_client_cmd = IncludeLaunchDescription(
    PythonLaunchDescriptionSource(
      os.path.join(pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py')),
    launch_arguments={'gz_args': '-g -v4 '}.items(),
    condition=IfCondition(PythonExpression([use_simulator, ' and not ', headless])))
    
  # Subscribe to the joint states of the robot, and publish the 3D pose of each link.
  robot_description_content = ParameterValue(Command(['xacro ', urdf_model]), value_type=str)
  start_robot_state_publisher_cmd = Node(
    condition=IfCondition(use_robot_state_pub),
    package='robot_state_publisher',
    executable='robot_state_publisher',
    name='robot_state_publisher',
    output='screen',
    parameters=[{
      'use_sim_time': use_sim_time, 
      'robot_description': robot_description_content}])

  # Launch RViz
  start_rviz_cmd = Node(
    condition=IfCondition(use_rviz),
    package='rviz2',
    executable='rviz2',
    name='rviz2',
    output='screen',
    arguments=['-d', rviz_config_file])  
    
  # Spawn the robot
  start_gazebo_ros_spawner_cmd = Node(
    package='ros_gz_sim',
    executable='create',
    arguments=[
      '-name', robot_name,
      '-topic', "robot_description"
      '-x', x,
      '-y', y,
      '-z', z,
      '-R', roll,
      '-P', pitch,
      '-Y', yaw
      ],
    output='screen')  
    
  # Bridge ROS topics and Gazebo messages for establishing communication
  start_gazebo_ros_bridge_cmd = Node(
    package='ros_gz_bridge',
    executable='parameter_bridge',
    parameters=[{
      'config_file': default_ros_gz_bridge_config_file_path,
    }],
    output='screen'
  )  
    
  # Create the launch description and populate
  ld = LaunchDescription()

  # Declare the launch options
  ld.add_action(declare_robot_name_cmd)
  ld.add_action(declare_rviz_config_file_cmd)
  ld.add_action(declare_simulator_cmd)
  ld.add_action(declare_urdf_model_path_cmd)
  ld.add_action(declare_use_robot_state_pub_cmd)  
  ld.add_action(declare_use_rviz_cmd) 
  ld.add_action(declare_use_sim_time_cmd)
  ld.add_action(declare_use_simulator_cmd)
  ld.add_action(declare_world_cmd)

  ld.add_action(declare_x_cmd)
  ld.add_action(declare_y_cmd)
  ld.add_action(declare_z_cmd)
  ld.add_action(declare_roll_cmd)
  ld.add_action(declare_pitch_cmd)
  ld.add_action(declare_yaw_cmd)  

  # Add any actions
  ld.add_action(set_env_vars_resources)
  ld.add_action(start_gazebo_server_cmd)
  ld.add_action(start_gazebo_client_cmd)
  ld.add_action(start_robot_state_publisher_cmd)
  ld.add_action(start_rviz_cmd)
  
  ld.add_action(start_gazebo_ros_spawner_cmd)
  ld.add_action(start_gazebo_ros_bridge_cmd)

  return ld

Save the file, and close it.

Edit package.xml

Now let’s modify our package.xml file to update the package dependencies.

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

Add this code. Save the file, and then close it.

Update CMakeLists.txt

Go to the CMakeLists.txt, and make sure it looks like this:

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

You can comment out or remove any folders or scripts in CMakeLists.txt that we have not yet created in this tutorial (e.g. test_set_joint_position_publisher.py). Be sure to do this before running “colcon build” below.

Save the file, and close it.

cd ~/ros2_ws/

Make sure all dependencies are installed.

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

Build the package.

colcon build
source ~/.bashrc

Launch the World and URDF Files Together

Run the launch file using the following command:

ros2 launch mycobot_gazebo mycobot_280_arduino_bringup_gazebo.launch.py

Here is the output using the empty.world file:

2-empty-world-new-gazebo-robotic-arm

Here is the output using the house.world file. You might see a pop up asking if you would like to Force Quit Gazebo…just wait and be patient for everything to load (house.world is full of SDF models):

3-robotic-arm-house-world-new-gazebo

To see a list of active topics, type:

ign topic -l

To see the options, you can type:

ign topic -help

Move the Robotic Arm Manually

I could not get the JointTrajectoryController to work in this new Gazebo. As I mentioned earlier, things are moving fast over at Gazebo, and not all the plugins have been transitioned over. However, I was able to get the JointPositionController to work and make the arm move.

To send joint angles to the robot using Gazebo, type this in a terminal window (in future versions of Gazebo, you will replace “ign” with “gz” below:

ign topic -t /model/mycobot_280/joint/link1_to_link2/0/cmd_pos -m ignition.msgs.Double -p "data: -2.0"

To send joint angles to the robot using ROS 2, type this in a terminal window:

ros2 topic pub -1 /model/mycobot_280/joint/link1_to_link2/cmd_pos std_msgs/msg/Float64 '{data: -2.0}'

Move the Robotic Arm Using a Script

Let’s create a ROS 2 node that can loop through a list of joint positions to simulate the robotic arm moving from the home position to a goal location and then back to home, repeatedly.

cd ~/ros2_ws/src/mycobot_ros2/mycobot_gazebo/
mkdir mycobot_gazebo
cd mycobot_gazebo
gedit __init__.py

Add this code, and then close it.

cd ..
mkdir scripts
cd scripts
gedit test_set_joint_position_publisher.py

Add this code:

#! /usr/bin/env python3

"""
Description:
  ROS 2: Executes a sample trajectory for a robotic arm in Gazebo
-------
Publishing Topics (ROS 2):
  Desired goal positions for joints on a robotic arm
    /model/mycobot_280/joint/link1_to_link2/cmd_pos - std_msgs/Float64
    /model/mycobot_280/joint/link2_to_link3/cmd_pos - std_msgs/Float64
    /model/mycobot_280/joint/link3_to_link4/cmd_pos - std_msgs/Float64
    /model/mycobot_280/joint/link4_to_link5/cmd_pos - std_msgs/Float64
    /model/mycobot_280/joint/link5_to_link6/cmd_pos - std_msgs/Float64
    /model/mycobot_280/joint/link6_to_link6flange/cmd_pos - std_msgs/Float64
    /model/mycobot_280/joint/gripper_controller/cmd_pos - std_msgs/Float64
    /model/mycobot_280/joint/gripper_base_to_gripper_left2/cmd_pos - std_msgs/Float64
    /model/mycobot_280/joint/gripper_left3_to_gripper_left1/cmd_pos - std_msgs/Float64
    /model/mycobot_280/joint/gripper_base_to_gripper_right3/cmd_pos - std_msgs/Float64
    /model/mycobot_280/joint/gripper_base_to_gripper_right2/cmd_pos - std_msgs/Float64
    /model/mycobot_280/joint/gripper_right3_to_gripper_right1/cmd_pos - std_msgs/Float64
-------
Author: Addison Sears-Collins
Date: April 18, 2024
"""
import numpy as np
import rclpy # Python client library for ROS 2
from rclpy.node import Node # Handles the creation of nodes
from std_msgs.msg import Float64  # Import the Float64 message


# Define constants
names_of_joints = [ 'link1_to_link2', 
                    'link2_to_link3', 
                    'link3_to_link4', 
                    'link4_to_link5', 
                    'link5_to_link6', 
                    'link6_to_link6flange', 
                    'gripper_controller',
                    'gripper_base_to_gripper_left2',
                    'gripper_left3_to_gripper_left1',
                    'gripper_base_to_gripper_right3',
                    'gripper_base_to_gripper_right2',
                    'gripper_right3_to_gripper_right1']

class BasicJointPositionPublisher(Node):
    """This class executes a sample trajectory for a robotic arm
    
    """      
    def __init__(self):
        """ Constructor.
      
        """
        # Initialize the class using the constructor
        super().__init__('basic_joint_position_publisher')    
 
        # Create a publisher for each joint
        self.position_publishers = [
            self.create_publisher(Float64, f'/model/mycobot_280/joint/{name}/cmd_pos', 1)
            for name in names_of_joints
        ]
        self.timer_period = 0.05 # seconds
        self.timer = self.create_timer(self.timer_period, self.timer_callback)

        # Starting position and goal position for the robotic arm joints
        self.start_position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
        self.end_position = [-1.345, -1.23, 0.264, -0.296, 0.389, -1.5, -0.7, -0.7, 0.7, 0.7, 0.7, -0.7]   

        # Number of steps to interpolate between positions
        self.num_steps = 50 
        
        # Set the desired goal poses for the robotic arm.
        self.positions = self.generate_positions(self.start_position, self.end_position)  
    
        # Keep track of the current trajectory we are executing
        self.index = 0

        # Indicate the direction of movement in the list of goal positions.
        self.forward = True

    def generate_positions(self, start_position, end_position):
        """
        Generates positions along a path from start to end positions.
        
        Args:
            start_position (list): The starting position of the robotic arm.
            end_position (list): The ending position of the robotic arm.
        
        Returns:
            list: A complete list of positions including all intermediate steps.
        """
        # Example path including start and end, could be expanded to more waypoints
        path_positions = [start_position, end_position]  
        all_positions = []
        for i in range(len(path_positions) - 1):
            interpolated = self.interpolate_positions(path_positions[i], path_positions[i + 1])
            all_positions.extend(interpolated[:-1])  # Exclude the last to avoid duplicates
        all_positions.append(path_positions[-1])  # Ensure the last position is included
        return all_positions

    def interpolate_positions(self, start, end):
        """
        Linearly interpolates between two positions.
        
        Args:
            start (list): The starting position for interpolation.
            end (list): The ending position for interpolation.
        
        Returns:
            list: A list of positions including the start, interpolated, and end positions.
        """
        interpolated_positions = [start]  # Initialize with the start position
        step_vector = (np.array(end) - np.array(start)) / (self.num_steps + 1)  # Calculate step vector
        for step in range(1, self.num_steps + 1):
            interpolated_position = np.array(start) + step * step_vector  # Compute each interpolated position
            interpolated_positions.append(interpolated_position.tolist())  # Append to the list
        interpolated_positions.append(end)  # Append the end position
        return interpolated_positions

    def timer_callback(self):
        """Set the goal pose for the robotic arm.
    
        """
        # Publish the current position for each joint
        for pub, pos in zip(self.position_publishers, self.positions[self.index]):
            msg = Float64()
            msg.data = pos
            pub.publish(msg)

        # Update the trajectory index
        if self.forward:
            if self.index < len(self.positions) - 1:
                self.index =  self.index + 1
            else:
                self.forward = False
        else:
            if self.index > 0:
                self.index = self.index - 1
            else:
                self.forward = True
    
def main(args=None):
  
    # Initialize the rclpy library
    rclpy.init(args=args)
  
    # Create the node
    basic_joint_position_publisher = BasicJointPositionPublisher()
  
    # Spin the node so the callback function is called.
    rclpy.spin(basic_joint_position_publisher)
    
    # Destroy the node
    basic_joint_position_publisher.destroy_node()
  
    # Shutdown the ROS client library for Python
    rclpy.shutdown()
  
if __name__ == '__main__':
  main()

Update your CMakeLists.txt to include this new script.

Now launch the robot. Wait for everything to come up, including RViz.

ros2 launch mycobot_gazebo mycobot_280_arduino_bringup_gazebo.launch.py

Run the basic joint position publisher to simulate movement for your robotic arm:

ros2 run mycobot_gazebo test_set_joint_position_publisher.py

Your arm will move from a home position to a goal position over and over again.

Gazebo (classic version)

Now let’s take a look at how to simulate and move a robotic arm using the classic version of Gazebo (which will reach end of life in 2025).

Install gazebo_ros_pkgs

Open a new terminal window, and install the packages that will enable you to use ROS 2 to interface with Gazebo Classic.

sudo apt install ros-$ROS_DISTRO-gazebo-ros-pkgs

Create a URDF File

cd ~/ros2_ws/src/mycobot_ros2/mycobot_gazebo/urdf
gedit mycobot_280_classic_gazebo.urdf.xacro

Add this code. Then save and close.

Create the Launch File

Let’s create a launch file to spawn both our world and our robotic arm.

cd ~/ros2_ws/src/mycobot_ros2/mycobot_gazebo/launch
gedit mycobot_280_arduino_bringup_classic_gazebo.launch.py

Add this code.

Save the file, and close it.

Create World Files

Create an environment for your simulated robot.

cd ~/ros2_ws/src/mycobot_ros2/mycobot_gazebo/worlds
gedit empty_classic.world

Add this code

If you want gravity turned ON, the gravity line should look like this: 

<gravity>0.0 0.0 -9.8</gravity>

If you want gravity turned OFF, the gravity line should look like this: 

<gravity>0.0 0.0 0.0</gravity>

Save the file, and close it.

Now let’s create a world called house_classic.world.

gedit house_classic.world

Add this code

Save the file, and close it.

Launch the World and URDF Files Together

Build the Package.

cd ~/ros2_ws/
rosdep install --from-paths src --ignore-src -r -y
colcon build
source ~/.bashrc

Run the launch file using the following command:

ros2 launch mycobot_gazebo mycobot_280_arduino_bringup_classic_gazebo.launch.py

Here is the output using the empty_classic.world file:

4-robot-arm-classic-gazebo-empty-world

Here is the output using the house_classic.world file:

5-robot-arm-classic-gazebo-house-world

Move the Robotic Arm Manually

To make the robot move, you use this command:

ros2 topic pub -1 /set_joint_trajectory trajectory_msgs/msg/JointTrajectory  "{header: {frame_id: base_link}, joint_names: [link1_to_link2, link2_to_link3, link3_to_link4, link4_to_link5, link5_to_link6, link6_to_link6flange, gripper_controller, gripper_base_to_gripper_left2, gripper_left3_to_gripper_left1, gripper_base_to_gripper_right3, gripper_base_to_gripper_right2, gripper_right3_to_gripper_right1], points: [{positions: [0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0]}]}"

You can change the values in the positions array to move the robotic arm to different poses. 

If you want to close the gripper, the gripper_controller position value needs to be -0.7.

ros2 topic pub -1 /set_joint_trajectory trajectory_msgs/msg/JointTrajectory  "{header: {frame_id: base_link}, joint_names: [link1_to_link2, link2_to_link3, link3_to_link4, link4_to_link5, link5_to_link6, link6_to_link6flange, gripper_controller, gripper_base_to_gripper_left2, gripper_left3_to_gripper_left1, gripper_base_to_gripper_right3, gripper_base_to_gripper_right2, gripper_right3_to_gripper_right1], points: [{positions: [-1.345,-1.230,0.264,-0.296,0.389,-1.50,-0.7, -0.7, 0.7, 0.7, 0.7, -0.7]}]}"

The two Gazebo plugins defined in the xacro file that make all this work are libgazebo_ros_joint_state_publisher.so (publishes the joint states from Gazebo to ROS 2) and libgazebo_ros_joint_pose_trajectory.so (subscribes to the desired goal poses for the arm that are sent from ROS 2 to Gazebo). That is all you need for basic arm control.

To close Gazebo, press CTRL + C on your keyboard.

Move the Robotic Arm Using a Script

Let’s create a ROS 2 node that can loop through a list of trajectories to simulate the arm moving from the home position to a goal location and then back to home.

cd ~/ros2_ws/src/mycobot_ros2/mycobot_gazebo/scripts
gedit test_set_joint_trajectory_publisher.py

Add this code:

#! /usr/bin/env python3

"""
Description:
  ROS 2: Executes a sample trajectory for a robotic arm in Gazebo Classic
-------
Publishing Topics (ROS 2):
  Desired goal pose of a robotic arm
  /set_joint_trajectory - trajectory_msgs/JointTrajectory
-------
Author: Addison Sears-Collins
Date: April 18, 2024
"""

import rclpy # Python client library for ROS 2
from rclpy.node import Node # Handles the creation of nodes
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from builtin_interfaces.msg import Duration
from std_msgs.msg import Header  # Import the Header message

# Define constants
names_of_joints = [ 'link1_to_link2', 
                    'link2_to_link3', 
                    'link3_to_link4', 
                    'link4_to_link5', 
                    'link5_to_link6', 
                    'link6_to_link6flange', 
                    'gripper_controller',
                    'gripper_base_to_gripper_left2',
                    'gripper_left3_to_gripper_left1',
                    'gripper_base_to_gripper_right3',
                    'gripper_base_to_gripper_right2',
                    'gripper_right3_to_gripper_right1']

class BasicJointTrajectoryPublisher(Node):
    """This class executes a sample trajectory for a robotic arm
    
    """      
    def __init__(self):
        """ Constructor.
      
        """
        # Initialize the class using the constructor
        super().__init__('basic_joint_trajectory_publisher')    
 
        # Create the publisher of the desired arm goal poses
        self.pose_publisher = self.create_publisher(JointTrajectory, '/set_joint_trajectory', 1)
        self.timer_period = 0.05  # seconds
        self.timer = self.create_timer(self.timer_period, self.timer_callback)

        self.frame_id = "base_link"

        # Set the desired goal poses for the robotic arm.
        # To make the code cleaner, I could have imported these positions from a yaml file.
        self.positions = [
            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-0.0269, -0.0246, 0.00528, -0.00592, 0.00778, -0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-0.0538, -0.0492, 0.01056, -0.01184, 0.01556, -0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-0.0807, -0.0738, 0.01584, -0.01776, 0.02334, -0.09, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-0.1076, -0.0984, 0.02112, -0.02368, 0.03112, -0.12, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-0.1345, -0.123, 0.0264, -0.0296, 0.0389, -0.15, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-0.1614, -0.1476, 0.03168, -0.03552, 0.04668, -0.18, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-0.1883, -0.1722, 0.03696, -0.04144, 0.05446, -0.21, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-0.2152, -0.1968, 0.04224, -0.04736, 0.06224, -0.24, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-0.2421, -0.2214, 0.04752, -0.05328, 0.07002, -0.27, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-0.269, -0.246, 0.0528, -0.0592, 0.0778, -0.3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-0.2959, -0.2706, 0.05808, -0.06512, 0.08558, -0.33, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-0.3228, -0.2952, 0.06336, -0.07104, 0.09336, -0.36, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-0.3497, -0.3198, 0.06864, -0.07696, 0.10114, -0.39, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-0.3766, -0.3444, 0.07392, -0.08288, 0.10892, -0.42, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-0.4035, -0.369, 0.0792, -0.0888, 0.1167, -0.45, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-0.4304, -0.3936, 0.08448, -0.09472, 0.12448, -0.48, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-0.4573, -0.4182, 0.08976, -0.10064, 0.13226, -0.51, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-0.4842, -0.4428, 0.09504, -0.10656, 0.14004, -0.54, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-0.5111, -0.4674, 0.10032, -0.11248, 0.14782, -0.57, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-0.538, -0.492, 0.1056, -0.1184, 0.1556, -0.6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-0.5649, -0.5166, 0.11088, -0.12432, 0.16338, -0.63, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-0.5918, -0.5412, 0.11616, -0.13024, 0.17116, -0.66, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-0.6187, -0.5658, 0.12144, -0.13616, 0.17894, -0.69, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-0.6456, -0.5904, 0.12672, -0.14208, 0.18672, -0.72, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-0.6725, -0.615, 0.132, -0.148, 0.1945, -0.75, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-0.6994, -0.6396, 0.13728, -0.15392, 0.20228, -0.78, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-0.7263, -0.6642, 0.14256, -0.15984, 0.21006, -0.81, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-0.7532, -0.6888, 0.14784, -0.16576, 0.21784, -0.84, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-0.7801, -0.7134, 0.15312, -0.17168, 0.22562, -0.87, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-0.807, -0.738, 0.1584, -0.1776, 0.2334, -0.9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-0.8339, -0.7626, 0.16368, -0.18352, 0.24118, -0.93, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-0.8608, -0.7872, 0.16896, -0.18944, 0.24896, -0.96, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-0.8877, -0.8118, 0.17424, -0.19536, 0.25674, -0.99, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-0.9146, -0.8364, 0.17952, -0.20128, 0.26452, -1.02, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-0.9415, -0.861, 0.1848, -0.2072, 0.2723, -1.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-0.9684, -0.8856, 0.19008, -0.21312, 0.28008, -1.08, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-0.9953, -0.9102, 0.19536, -0.21904, 0.28786, -1.11, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-1.0222, -0.9348, 0.20064, -0.22496, 0.29564, -1.14, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-1.0491, -0.9594, 0.20592, -0.23088, 0.30342, -1.17, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-1.076, -0.984, 0.2112, -0.2368, 0.3112, -1.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-1.1029, -1.0086, 0.21648, -0.24272, 0.31898, -1.23, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-1.1298, -1.0332, 0.22176, -0.24864, 0.32676, -1.26, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-1.1567, -1.0578, 0.22704, -0.25456, 0.33454, -1.29, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-1.1836, -1.0824, 0.23232, -0.26048, 0.34232, -1.32, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-1.2105, -1.107, 0.2376, -0.2664, 0.3501, -1.35, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-1.2374, -1.1316, 0.24288, -0.27232, 0.35788, -1.38, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-1.2643, -1.1562, 0.24816, -0.27824, 0.36566, -1.41, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-1.2912, -1.1808, 0.25344, -0.28416, 0.37344, -1.44, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-1.3181, -1.2054, 0.25872, -0.29008, 0.38122, -1.47, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-1.345, -1.23, 0.264, -0.296, 0.389, -1.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
            [-1.345, -1.23, 0.264, -0.296, 0.389, -1.5, -0.07, -0.07, 0.07, 0.07, 0.07, -0.07],
            [-1.345, -1.23, 0.264, -0.296, 0.389, -1.5, -0.14, -0.14, 0.14, 0.14, 0.14, -0.14],
            [-1.345, -1.23, 0.264, -0.296, 0.389, -1.5, -0.21, -0.21, 0.21, 0.21, 0.21, -0.21], 
            [-1.345, -1.23, 0.264, -0.296, 0.389, -1.5, -0.28, -0.28, 0.28, 0.28, 0.28, -0.28],
            [-1.345, -1.23, 0.264, -0.296, 0.389, -1.5, -0.35, -0.35, 0.35, 0.35, 0.35, -0.35],
            [-1.345, -1.23, 0.264, -0.296, 0.389, -1.5, -0.42, -0.42, 0.42, 0.42, 0.42, -0.42],
            [-1.345, -1.23, 0.264, -0.296, 0.389, -1.5, -0.49, -0.49, 0.49, 0.49, 0.49, -0.49],
            [-1.345, -1.23, 0.264, -0.296, 0.389, -1.5, -0.56, -0.56, 0.56, 0.56, 0.56, -0.56],
            [-1.345, -1.23, 0.264, -0.296, 0.389, -1.5, -0.63, -0.63, 0.63, 0.63, 0.63, -0.63],
            [-1.345, -1.23, 0.264, -0.296, 0.389, -1.5, -0.7, -0.7, 0.7, 0.7, 0.7, -0.7],
            [-1.345, -1.23, 0.264, -0.296, 0.389, -1.5, -0.7, -0.7, 0.7, 0.7, 0.7, -0.7],
            [-1.345, -1.23, 0.264, -0.296, 0.389, -1.5, -0.7, -0.7, 0.7, 0.7, 0.7, -0.7],
            [-1.345, -1.23, 0.264, -0.296, 0.389, -1.5, -0.7, -0.7, 0.7, 0.7, 0.7, -0.7],
            [-1.345, -1.23, 0.264, -0.296, 0.389, -1.5, -0.7, -0.7, 0.7, 0.7, 0.7, -0.7],
            [-1.345, -1.23, 0.264, -0.296, 0.389, -1.5, -0.7, -0.7, 0.7, 0.7, 0.7, -0.7],
            [-1.345, -1.23, 0.264, -0.296, 0.389, -1.5, -0.7, -0.7, 0.7, 0.7, 0.7, -0.7],
            [-1.345, -1.23, 0.264, -0.296, 0.389, -1.5, -0.7, -0.7, 0.7, 0.7, 0.7, -0.7],
            [-1.345, -1.23, 0.264, -0.296, 0.389, -1.5, -0.7, -0.7, 0.7, 0.7, 0.7, -0.7],
            [-1.345, -1.23, 0.264, -0.296, 0.389, -1.5, -0.7, -0.7, 0.7, 0.7, 0.7, -0.7],
        ]

        # Keep track of the current trajectory we are executing
        self.index = 0

        # Indicate the direction of movement in the list of goal positions.
        self.forward = True

    def timer_callback(self):
        """Set the goal pose for the robotic arm.
    
        """
        # Create a new JointTrajectory message
        msg = JointTrajectory()
        msg.header = Header()  # Initialize the header
        msg.header.frame_id = self.frame_id  
        msg.joint_names = names_of_joints

        # Create a JointTrajectoryPoint
        point = JointTrajectoryPoint()
        point.positions = self.positions[self.index]
        point.time_from_start = Duration(sec=0, nanosec=int(self.timer_period * 1e9))  # Time to next position
        msg.points.append(point)
        self.pose_publisher.publish(msg)

        # Move index forward or backward
        if self.forward:
            if self.index < len(self.positions) - 1:
                self.index += 1
            else:
                self.forward = False
        else:
            if self.index > 0:
                self.index -= 1
            else:
                self.forward = True
    
def main(args=None):
  
    # Initialize the rclpy library
    rclpy.init(args=args)
  
    # Create the node
    basic_joint_trajectory_publisher = BasicJointTrajectoryPublisher()
  
    # Spin the node so the callback function is called.
    rclpy.spin(basic_joint_trajectory_publisher)
    
    # Destroy the node
    basic_joint_trajectory_publisher.destroy_node()
  
    # Shutdown the ROS client library for Python
    rclpy.shutdown()
  
if __name__ == '__main__':
  main()

Make sure to update your CMakeLists.txt file.

Build your package.

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

Now launch the robot. Wait for everything to come up, including RViz.

ros2 launch mycobot_gazebo mycobot_280_arduino_bringup_classic_gazebo.launch.py

Run the basic joint trajectory publisher to simulate movement for your robotic arm:

ros2 run mycobot_gazebo test_set_joint_trajectory_publisher.py

The output should look like the animated image at the beginning of this blog post.

Note that even though we have the “mimic” tag in the URDF file for the gripper, we still have to explicitly define all non-fixed joints in the URDF Gazebo plugins section in order for everything to work properly in Gazebo.

That’s it! Keep building!