🤖
Introduction to ROS2 and Robotics
  • Introduction
  • ROS2
    • Index
    • IDE and CMake Setup
      • How to add additional include search path
    • ROS2 Building Blocks
      • ROS Workspace and Package Layout
      • Launch File
      • tf2
      • Quality of Service
      • Configurations
        • Rviz Configuration
      • Built-in Types
        • Built-in Message Type
    • ROS Architecture
      • Intra-process Communication
    • Navigation and Planning
      • Navigation Stack and Concepts
      • Navigation2 Implementation Overview
        • 🏗️Cost Map
        • Obstacle Avoidance and DWB Controller
      • DWB Controller
      • Page 5
    • How to launch the Nav2 stack
    • ROS2 Control
      • Online Resources
      • Overview of Codebase
    • 🍳Cookbook
      • Useful Commands
      • How to specify parameters
      • How to build the workspace
      • 🏗️How to publish message to a topic from command line?
      • How to inspect service and make a service call
      • How to properly terminate ROS and Gazebo
      • How to add and remove models in Gazebo simulation dynamically
      • 🚧How to spin nodes
    • 🛒Tutorials
      • Services and Communication between ROS2 and Gazebo
      • Subscription and Message Filters Demo
      • Executor and Spin Explained
      • Lifecycle Node Demo
      • Robotic Arm Demo
      • ⚒️Multiple Robotic Arms Simulation Demo
      • 🚧Introduction to xacro
    • Page
    • 🍺Tech Blog
      • Difference between URDF and SDF and how to convert
  • Gazebo
    • Index
    • Terminology
    • GUI
    • World Frame and Axis
    • Cookbook
    • Page 1
  • Programming in Robotics
    • C++
      • CMake
    • Python
    • Rust
  • Mathematics in Robotics
    • Linear Algebra
    • Matrix Properties
    • Probability
      • Expectation-Maximization Algorithm
    • Multivariable Function and Derivatives
  • Physics in Robotics
  • Control of Dynamic Systems
    • Dynamic Response and Transfer Function
    • Block Diagram
    • PID Controller
  • Robot Modeling and Control
    • Rotation and Homogeneous Transformation
  • Probabilistic Robotics
    • Bayes Filter
    • Kalman Filter
    • Particle Filter
    • Discrete Bayes Filter
    • Motion Model
    • Perception Model
    • Localization
    • SLAM
  • Miscellany
  • Concept Index
    • Quaternions
Powered by GitBook
On this page
  • Introduction
  • Related Readings
  • Step 1: Download the Robotic Arm Model
  • Step 2: Convert SDF to URDF
  • Step 3: Configure ros2 control and Gazebo
  • Step 4: Launch the system
  • Step 5: Send a command to move the robot
  • Demo
  • Download Code
  1. ROS2
  2. Tutorials

Robotic Arm Demo

PreviousLifecycle Node DemoNextMultiple Robotic Arms Simulation Demo

Last updated 1 year ago

Introduction

In this tutorial, we will walk you through setting up a robot arm in Gazebo. First, we will download the SDF model file for the UR10 robot arm and convert it into a URDF file. Next, we will explain how to configure the ros2 control and Gazebo simulation for the robot model. Finally, we demonstrate launching the system and issuing a basic command to move the robotic arm.

Please note, this tutorial focuses on practical steps and does not cover the theoretical aspects of robotic arm control.

Related Readings

Step 1: Download the Robotic Arm Model

The first step is to download a robotic arm model. Many options are available online and in this tutorial, we will use the Universal Robotics UR10 robot arm. You can download the model file at this .

The model files assume all relevant files are store in the ur10 directory, therefore, it's important to ensure the content in the zip file extract to a ur10 directory. Moreover, the parent directory of the ur10 directory needs to be included in the Gazebo resource path. More specifically, suppose we extract the UR10 robot arm model files to ~/demo/models/ directory:

learnros2@7D41:~/demo/models$ tree -L 2
.
└── ur10
    ├── meshes
    ├── model.config
    └── ur10.sdf

We need to ensure that the path ~/demo/models is included in the Gazebo resource path, which can be configured with the environment variable IGN_GAZEBO_RESOURCE_PATH in Gazebo Fortress.

We can now launch the system and verify the setup is correct. In this tutorial, both ROS2 components and Gazebo are launched using the launch file. This ensures a consistent experience with different components and minimize the separation between ROS2 and Gazebo. Through out the tutorial, we consider Gazebo a sub-system of the ROS2 ecosystem.

If you download the code attached to this tutorial, you can launch the system using the following command:

ros2 launch bringup ur10.launch.py 

You can also create your own launch file. The objective is to launch the the Gazebo world with the downloaded robotic arm model. The code below demonstrates how to launch a Gazebo world with a specific world sdf file:

def generate_launch_description():
    bringup_package = get_package_share_directory("bringup")
    export_models(path.join(bringup_package, "models"))

    ros_gz_sim_package = get_package_share_directory('ros_gz_sim')
    gz_node = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(path.join(ros_gz_sim_package, 'launch', 'gz_sim.launch.py')),
        launch_arguments={'gz_args': PathJoinSubstitution([
            bringup_package,
            'worlds',
            "simple_world_with_ur10.sdf",
        ])}.items(),
    )

    return LaunchDescription([
        gz_node
    ])

Make sure the world sdf file (in the above case, it's simple_world_with_ur10.sdf) contains the robot model:

<model name="ur10-original-with-minor-edit">
    <self_collide>true</self_collide>
    <pose>0 1 0 0 0 0</pose>
    <include merge="true">
        <uri>model://models/ur10/ur10.modified.sdf</uri>
    </include>
</model>

If everything works correctly, you should see Gazebo launched successfully and a robot arm lying on the ground:

Step 2: Convert SDF to URDF

If we don't care about simulation, we don't need to deal with the SDF file because URDF is the native format for describing robots in ROS2. However, if we want to integrate ROS2 with a simulator such as Gazebo, additional simulation-specific information is required and it's usually specified in the SDF file.

Both URDF and SDF are used to describe robots and they have overlaps. If we use two formats to describe the same robot, we need to ensure they are in-sync. For example, whenever we modify the geometry in one file, we need to make the same update in another file too.

It's important to notice that many components in the ROS2 and Gazebo ecosystem either require URDF file or is able to consume it. Therefore, a possible solution is to put geometry-related information in an URDF file, group the control-specific and simulation-specific information in a separate URDF file, and use xacro to combine the two files. For example:

<!-- This is the model.xacro.urdf file -->
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="ur10">
    <!-- model definition -->
    
    <xacro:include filename="path-to-additional-info.xacro.urdf" />
</robot>

<!-- This is the additional-info.xacro.urdf file -->
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <ros2_control name="MyRobot" type="system">
        ...
    </ros2_control>
    <gazebo>
        <plugin filename="ign_ros2_control-system" name="ign_ros2_control::IgnitionROS2ControlPlugin">
            ... 
        </plugin>
    </gazebo>
</robot>

In this way, the robot model definition is separated from the control and simulation configurations and we only need to maintain a single source of the model. Another benefit is that it's easy to replace the control and simulation configurations.

ignition_spawn_entity = Node(
    package='ros_gz_sim',
    executable='create',
    output='screen',
    arguments=['-world', 'demo',
                '-string', urdf_content,
                '-name', 'MyRobot',
                '-allow_renaming', 'true',
                "-x", "0", "-y", "2"],
)

One more technical detail. The robot arm will be subject to the force. If we don't fix the base to the ground, it will not behave as expected. To fix the base to the ground in Gazebo, we can add a dummy link and joint in the URDF file. For example:

<link name="world"/>
<joint name="base_to_world" type="fixed">
    <parent link="world"/>
    <child link="base"/>
</joint>

Step 3: Configure ros2 control and Gazebo

In this tutorial, we want to demonstrate how to move a robotic arm. It has two aspects:

  • Use the ros2 control package to control the robot hardware

  • Because we don't have the real robot hardware, we will use Gazebo to simulate one.

As mentioned earlier, the configurations of ros2 control and the gazebo simulation are separate from the robot model. Below is a sample configuration file:

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <ros2_control name="MyRobot" type="system">
        <hardware>
            <plugin>ign_ros2_control/IgnitionSystem</plugin>
        </hardware>

        <joint name="shoulder_pan">
          <command_interface name="position">
            <param name="min">{-2*pi}</param>
            <param name="max">{2*pi}</param>
          </command_interface>
          <state_interface name="position">
            <param name="initial_value">0.0</param>
          </state_interface>
        </joint>

        <joint name="shoulder_lift">
          <command_interface name="position">
            <param name="min">{-2*pi}</param>
            <param name="max">{2*pi}</param>
          </command_interface>
          <state_interface name="position">
            <param name="initial_value">0.0</param>
          </state_interface>
        </joint>

        <joint name="elbow">
          <command_interface name="position">
            <param name="min">{-2*pi}</param>
            <param name="max">{2*pi}</param>
          </command_interface>
          <state_interface name="position">
            <param name="initial_value">0.0</param>
          </state_interface>
        </joint>

        <joint name="forearm_wrist_1">
          <command_interface name="position">
            <param name="min">{-2*pi}</param>
            <param name="max">{2*pi}</param>
          </command_interface>
          <state_interface name="position">
            <param name="initial_value">0.0</param>
          </state_interface>
        </joint>

        <joint name="wrist_1_wrist_2">
          <command_interface name="position">
            <param name="min">{-2*pi}</param>
            <param name="max">{2*pi}</param>
          </command_interface>
          <state_interface name="position">
            <param name="initial_value">0.0</param>
          </state_interface>
        </joint>

        <joint name="writs_2_wrist_3">
          <command_interface name="position">
            <param name="min">{-2*pi}</param>
            <param name="max">{2*pi}</param>
          </command_interface>
          <state_interface name="position">
            <param name="initial_value">0.0</param>
          </state_interface>
        </joint>
    </ros2_control>

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

    <gazebo>
        <static>false</static>
        <self_collide>1</self_collide>
    </gazebo>

</robot>

We also need to configure the controllers, which are specified in a parameter file. In our case, the parameter file is simple: we will use a position controller for all the joints.

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:
      - shoulder_pan
      - shoulder_lift
      - elbow
      - forearm_wrist_1
      - wrist_1_wrist_2
      - writs_2_wrist_3

Step 4: Launch the system

We need to create the following tasks in the launch file:

  • Process the xacro file. Recall that the model definition and the ros2 control/simulation configurations are stored in two separate files. We need to merge them into one single file during the launch. This can be done using the xacro library.

  • Create a robot state publisher. This is almost always needed in any ros2 application.

  • Load the position controller and the joint state broadcaster. These two components are part of the rso2 control package. They are required because they are specified in the parameter file.

  • Launch the Gazebo node. We can specify a world SDF file in this step.

  • Spawn the model. This is the step where we instantiate the robot arm model. When we spawn a model, we need to provide the world name, model name, and the model URDF file with the ros2 control and the simulation configuration. We can also specify the entity position.

Note that the sequence of the task execution is important. We spawn the model first, followed by activating the joint state broadcaster, and activate the position controller at last. The execution order can be enforced by using the RegisterEventHandler utility function. For example, the code blow only execute load_joint_state_broadcaster after the ignition_spawn_entity process exits.

RegisterEventHandler(
    event_handler=OnProcessExit(
        target_action=ignition_spawn_entity,
        on_exit=[load_joint_state_broadcaster],
    )
)

Step 5: Send a command to move the robot

Once everything is up running. We can now send a request to the controller. The request takes an array as inputs as the controller controls multiple joints. To send a request, we publish a message to the /position_controller/commands topic. For example:

ros2 topic pub --once  /position_controller/commands std_msgs/msg/Float64MultiArray "{data: [0,-1.5,0,0,0,0]}"

Demo

Download Code

©2023 - 2024 all rights reserved

Many tools exist to convert SDF files to URDF files. You can also check out the on how to perform the conversion manually. It's a good practice to always verify the generate files. Unfortunately, we cannot simply include the URDF model in the SDf world file. Instead, we can create a node that will run the create executable in the ros_gz_sim package. For example:

The configuration of ros2 control is standard. We specify the joint types, state interfaces, and command interfaces in the URDF file inside the ros2_control element. The simulation part is handled by the package, which is responsible for simulating the movement of the joints. Moreover, it also provides the hardware interface used by the ros2 control.

The code can be downloaded at this .

🛒
How to control Universal Robot by using ROS2
GitHub repo - ros2_control
GitHub repo - ros2_controllers
Examples of ros2_control
Simple Tutorial of gz_ros2_control
gz_ros2_control demo
Ros2 Control Example: 6DoF robot
link
blog
gz_ros2_control
link