⚒️Multiple Robotic Arms Simulation Demo

Introduction

In this article, we will demonstrate how to set up multiple robotics arms in gazebo. In the article Robotic Arm Simulation Demo, we presented the steps to set up one robotics arms in gazebo, focusing on the interaction between ROS2 and Gazebo. Controlling multiple robotics arms presents new challenges. One of the reasons why it is difficult to manage multiple robots in ROS2 is that there is no standard on "encapsulation". Here we borrow the encapsulation from software engineer, which roughly means a clear boundary of an entity. Ros2 provides two mechanisms to define scope: namespace and node names, but the usages of these two mechanisms are not consistent in the ecosystem.

This article is organize as follows. We first clarify the namespace concept and explain how to leverage this feature to support multiple robots. Next, we demonstrate how to introduce the namespace to the robot model file and why it's needed. Finally, we present the step to launch multiple robots in the Gazebo simulation, which requires additional steps due to some "limits" in the gazebo ros2 control library.

Namespace and Multiple Robots

Namespace is arguably the only way to define multiple robots in ROS2. Drawing a comparison between the concepts of namespace/node name and websites can enhance our understanding: the namespace serves a role similar to that of a domain name, while the node name parallels the specific page URL within a website. Just as a website comprises numerous pages, each designed for various purposes, a robot is equipped with multiple nodes, each tasked with distinct functions.

A simple way of using namespace to support multiple robots is to allocate a unique namespace for each robot. For exmpale, in this article, we will set up two robotic arms: Bob and Toby. All Bob's nodes should use "Bob" as their namespace and respectively all Toby's nodes should use the "Toby" namespace.

It's important to note that the tf2 framework does not have the namespace concept. The frame ID is just a plain string. This means to define scopes in tf2, we need to establish a convention on frame ID first. For example, we could say frame IDs that share the same prefix can considered in the same group. Consequently, frames of a robot named Toby should have IDs such as Toby_frame_A, toby_frame_B, etc.

Frame ID convention is a major source of confusion because different libraries have different behavior. Suppose we have the following robot model definition:

<robot name="Toby">
    <link name="arm">
    ...
    </link>
</robot>

Some libraries add the robot name as the frame ID prefix automatically so they will publish data for the frame Toby_arm or Toby.arm and they tend to be multi-robot friendly. For those libraries that do not add prefix, it's clear that they cannot support multiple robots out of the box because Bob and Toby both publish data for the same frame arm and the data is overwritten constantly. The solution is simple but can be tedious to set up. We can simply change the model file to

<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="robot">
    <xacro:arg name="robot_name" default="UNKNOWN"/>
    <link name="$(arg robot_name)_arm">
    ...
    </link>
</robot>

and then pass the value to the xacro tool:

doc = xacro.parse(open(self.xacro_file))
xacro.process_doc(doc, mappings={
    'robot_name': self.robot_name,
    'control_parameter_file': RobotModelConfigManager.CONTROL_PARAMETER_FILE})
robot_description = doc.toxml()

Edit Robot Model File and Support Multiple Robots

It turns out that ign_ros2_control::IgnitionROS2ControlPlugin does not add robot name as the prefix of the frame ID and as discussed in the previous section, we need to manually add the robot name to all the link and joint names. Alternatively, we can parse the xml file and add the prefix programmatically.

Links and joints specification is only part of the Gazebo ros2 control configuration. Recall that the plugin requires additional settings. For example:

<gazebo>
    <plugin filename="ign_ros2_control-system" name="ign_ros2_control::IgnitionROS2ControlPlugin">
        <parameters>$(find bringup)/configs/ur10-controller.yaml</parameters>
        <robot_param>robot_description</robot_param>
        <robot_param_node>robot_state_publisher</robot_param_node>
    </plugin>
</gazebo>

We need to add namespace to these parameters as well:

<gazebo>
    <plugin filename="ign_ros2_control-system" name="ign_ros2_control::IgnitionROS2ControlPlugin">
        <parameters>$(find bringup)/configs/$(arg robot_name)_control_parameters.yaml</parameters>
        <robot_param>/$(arg robot_name)/robot_description</robot_param>
        <robot_param_node>/$(arg robot_name)/robot_state_publisher</robot_param_node>
        <ros>
            <namespace>$(arg robot_name)</namespace>
        </ros>
    </plugin>
</gazebo>

The added <ros> tag is used to instruct plugin to create nodes with the provided namespace. Unfortunately, the above does not work due to a limitation in the IgnitionROS2ControlPlugin. At least for Gazebo Ign Fortress, the control parameters file is somehow shared among the plugins. Therefore, if we launch robot Bob, followed by robot Toby, both Bob's plugin and Toby's plugin will use Toby's parameter file. This implies that we cannot have separate control parameters for different robots. Instead, we need too have one single control parameters, which contains parameters for multiple namespace. For example:

Toby:
  controller_manager:
    ros__parameters:
      update_rate: 100  # Hz

      joint_state_broadcaster:
        type: joint_state_broadcaster/JointStateBroadcaster

      position_controller:
        type: position_controllers/JointGroupPositionController

  position_controller:
    ros__parameters:
      joints:
        - Toby_shoulder_pan
        - Toby_shoulder_lift
        - Toby_elbow
        - Toby_forearm_wrist_1
        - Toby_wrist_1_wrist_2
        - Toby_writs_2_wrist_3

Bob:
  controller_manager:
    ros__parameters:
      update_rate: 100  # Hz

      joint_state_broadcaster:
        type: joint_state_broadcaster/JointStateBroadcaster

      position_controller:
        type: position_controllers/JointGroupPositionController


  position_controller:
    ros__parameters:
      joints:
        - Bob_shoulder_pan
        - Bob_shoulder_lift
        - Bob_elbow
        - Bob_forearm_wrist_1
        - Bob_wrist_1_wrist_2
        - Bob_writs_2_wrist_3

And the plugin configuration becomes:

<gazebo>
    <plugin filename="ign_ros2_control-system" name="ign_ros2_control::IgnitionROS2ControlPlugin">
        <parameters>$(find bringup)/configs/shared_control_parameters.yaml</parameters>
        <robot_param>/$(arg robot_name)/robot_description</robot_param>
        <robot_param_node>/$(arg robot_name)/robot_state_publisher</robot_param_node>
        <ros>
            <namespace>$(arg robot_name)</namespace>
        </ros>
    </plugin>
</gazebo>

Launch the Stack

Let's first take a look at the composition of the stack:

• Rviz

• Gazebo

• robots

  • robot sate publisher

  • robot instance in Gazebo

  • joint state broadcaster

  • position controller

To launch the stack, we first launch Rviz, Gazebo and robot state publishers. The sequence of these components does not matter. Next, for each robot, we spawn an entity in Gazebo, load the state broadcaster and the position controller. The order of these robot specific components matters and they need to be done in the mentioned order.

It's reasonable to assume that the launch process of each robot is independent and they can be done in parallel. Unfortunately, that's not the case. With Gazebo Ignirition Fortress version, a race condition may be triggered if multiple robots load the position controller at the same time. To get around of the issue, we need to launch robot stack one by one.

There is another issue. Recall that to fix the robot arm on the ground, we introduced a world link and a fixed joint between the world link and the robot base link. By default, the robot base link is set at the origin of the world. When we spawn the robot entities in Gazebo, they are at different places. We can specify the entity position with the ros_gz_sim create command. However, it seems that the IgnitionROS2ControlPlugin only takes care of the position in Gazebo and it does not update the position in ROS2. Therefore, even if we specify different positions for different robots in Gazebo, from ROS2's perspective, they all stay at the origin of the world.

To fix this issue, we need to manually update the position in ROS2 by publishing a static transform between the world link and the robot base link. This can be achieved by the following commands:

ros2 run tf2_ros static_transform_publisher 0 2 0 0 0 0 world Toby_base
ros2 run tf2_ros static_transform_publisher 0 4 0 0 0 0 world Bob_base

Here comes anther limitation. The static_transform_publisher is a node and it doesn't exit after it publishes the data. A manual exit is required. That's why this step is not included in the launch file in the attached code.

Troubleshooting: Rviz cannot find material files

In URDF, we can specify either the relative path of the absolute path. The relative path is defined by

<mesh filename="package://models/ur10/meshes/base.dae" />

and the absolute path is defined by

<mesh filename="file://<absolute-path-base.dae>" />

I cannot find detailed documentation on how ROS2 and Gazebo resolve the search path and most likely they are inconsistent. So far, what seems to work is as follows:

• In the URDF/xacro robot model file, use package://. This notation indicates that the provided path is relative in both ROS2 and Gazebo.

• Process the xacro file with appropriate mapping (i.e. specifying the arguments) and pass the result string to Gazebo nodes or processes. Gazebo model search path can be configured via the IGN_GAZEBO_RESOURCE_PATH environment variable.

• Replace the package:// with file://<absolute-path> in the result string in the previous step, and pass it to ROS2 nodes or processes such as Rviz.

Download Code

The code can be downloaded at this link.

©2023 - 2024 all rights reserved