Article directory
1. Introduction
1.1 Motivation
Recently, I was watching Bruno Siciliano's Robotics Modeling, Planning and Control , and wanted to implement the kinematics-related algorithms. First, there was a simulation environment, because I had used ROS before, so I used ROS-Gazebo to simulate the UR5 robotic arm ( Because UR5 is also used in the laboratory).
The Gazebo simulation of the manipulator found on the Internet is generally controlled by MoveIt . MoveIt is a third-party toolkit for ROS. It has powerful functions and can realize functions such as motion control, motion planning, and obstacle avoidance. However, the MoveIt package is relatively abstract, and the underlying algorithm cannot be seen when using it. What I think is that the interface controlled by the robotic arm is just a 1 × 6 1\times61×6 ’s joint angle vector, the upper forward and reverse kinematics, motion planning and other algorithms are implemented by themselves. So I plan to implement a ROS-Gazebo UR5 simulation package by myself.
1.2 Overview
Let me talk about what is probably needed to implement this package:
- The urdf file ROS package of the UR5 robotic arm
ur5_description. This package can be used directly with the official UR-ROS package, mainly to generate the UR5 model. - The ROS package related to Gazebo is
ur5_gazebomainly to create the Gazebo world, generate the manipulator model, and control the interface.
Because I just started to get in touch with ROS, there are many things that are confusing, and I didn’t mention it after reading various tutorials. It took me a while to understand, so this article will share some of my opinions (personal understanding, please correct me if there are mistakes), compare For beginners (such as me). Familiar students may find it long-winded, you can choose the part you want to read according to the title.
1.3 Prerequisite skills
It is recommended to read the tutorials of Gazebo x ROS first. In fact, this article is a summary of the knowledge taught in these tutorials.
- Tutorial: Using roslaunch to start Gazebo, world files and URDF models
- Tutorial: Using a URDF in Gazebo
- Tutorial: ROS Control
It is recommended that you read more tutorials on the ROS wiki. After you have studied them all, you can search and read these posts if you have specific questions.
2. Robot model description
2.1 URDF
In ROS, URDF (Unified Robot Description Format) is generally used to describe the robot model. URDF is essentially a xmlformat file, which looks like this. Many files in ROS, for example xacro, yamlare actually based on xmlthe format.
<robot name="robot">
<link> ... </link>
<link> ... </link>
<joint> ... </joint>
</robot>
<!-- 注释 -->
For example, in the above code, <robot>the tag contains the definition of the robot body, including the connecting rod <link>and joints <joint>.
For the specific syntax and definition, you can refer to the XML specifications . I won’t go into details here, and <joint>the definition will be used later.
2.2 xacro
xacro (xml macro) is actually still xml, but with added macro definitions. For example, the urdf file of a robot <link> <joint>is almost the same. You can first set up a <link> <joint>macro definition, and then write it later to call this macro definition, which saves a lot of effort. xacro It is mainly for the purpose of code reuse ( lazy ). For details, please refer to the official xacro ROS wiki . The following passage is their introduction to xacro.
We also developed a macro language called xacro to make it easier to maintain the robot description files, increase their readability, and to avoid duplication in the robot description files.
The xacro code is long like this, you can define constants, etc.
<!-- xacro 声明 -->
<?xml version="1.0"?>
<robot name="mrobot" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- xacro 定义常量 -->
<xacro::property name="PI" value="3.14159"/>
<!-- xacro 调用常量、数学公式 -->
<origin xyz="0 0 0" rpy="${PI/2} 0 0"/>
From the user's point of view, xacro is much more convenient to write than urdf. In fact, ROS still uses urdf files to build robots. When using them, the program is called to xacroparse xacro into urdf files and upload them to the ROS parameter server.
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find mrobot_description)/urdf/mrobot.urdf.xacro'"/>
2.3 UR model
The ROS package of UR (Universal Robot) can be downloaded directly from the Universal Robot repo on GitHub .
For Ubuntu 16 and previous students, you can directly apt install. But for Ubuntu 18 and above, you need to compile it from the source code. See Universal Robot for the specific process .
~/catkin_ws/src/universal_robot$ tree -L 1
.
├── README.md
├── universal_robot
├── universal_robots
├── ur5_e_moveit_config
├── ur5_moveit_config
├── ur_bringup
├── ur_description
├── ur_driver
├── ur_e_description
├── ur_e_gazebo
├── ur_gazebo
├── ur_kinematics
└── ur_msgs
After the installation, the directory looks like this (I deleted some unnecessary packages, such as UR3 and UR10). ur_descriptionIt is the description file of the UR robotic arm.
~/catkin_ws/src/universal_robot/ur_description$ tree -L 2
.
├── cfg
│ └── view_robot.rviz
├── CHANGELOG.rst
├── CMakeLists.txt
├── launch
│ ├── ur10_upload.launch
│ ├── ur3_upload.launch
│ ├── ur5_upload.launch
│ ├── view_ur10.launch
│ ├── view_ur3.launch
│ └── view_ur5.launch
├── meshes # 机器人三维模型
│ ├── ur10
│ ├── ur3
│ └── ur5
├── model.pdf
├── package.xml
└── urdf # 机器人urdf文件
├── common.gazebo.xacro
├── ur10_joint_limited_robot.urdf.xacro
├── ur10_robot.urdf.xacro
├── ur10.urdf.xacro
├── ur3_joint_limited_robot.urdf.xacro
├── ur3_robot.urdf.xacro
├── ur3.urdf.xacro
├── ur5_joint_limited_robot.urdf.xacro
├── ur5_robot.urdf.xacro
├── ur5.urdf.xacro
├── ur.gazebo.xacro
└── ur.transmission.xacro
You can open it ur5.urdf.xacroto see how the link and joint of the ur robotic arm are defined.
Next, we extract the UR5 description file from the official package and create a ur5_descriptionpackage by ourselves.
In fact, this step is not necessary, and the official package can also be used directly.
I mainly don't like to have redundant things, and there will be changes to the urdf file later, so I created a new simplified version of the ROS package.
2.4 Create a streamlined ur5_description
First create a ROS package
~/catkin_ws/src/ur_sim$ catkin_create_pkg ur5_description
urdfThen copy the folder of UR5 to the newly created package ur5_description. You can delete the files related to UR3 and UR10. meshesCopy it and delete it ur3 ur10. The package directory you created should look like this:
~/catkin_ws/src/ur_sim/ur5_description$ tree -L 1
.
├── CMakeLists.txt
├── launch
├── meshes
└── ur5
├── collision
└── visual
├── package.xml
└── urdf
├── common.gazebo.xacro
├── ur5_joint_limited_robot.urdf.xacro
├── ur5_robot.urdf.xacro
├── ur5.urdf.xacro
├── ur.gazebo.xacro
└── ur.transmission.xacro
Remember to modify the package name inur5_robot.urdf.xacro and .ur5.urdf.xacro
<!-- ur5_robot.urdf.xacro -->
<!-- common stuff -->
<!-- 记得修改package名 -->
<xacro:include filename="$(find ur5_description)/urdf/common.gazebo.xacro" />
<!-- ur5 -->
<xacro:include filename="$(find ur5_description)/urdf/ur5.urdf.xacro" />
<!-- ur5.urdf.xacro -->
<link name="${prefix}base_link" >
<visual>
<geometry>
<!-- 记得修改package名 -->
<mesh filename="package://ur5_description/meshes/ur5/visual/base.dae" />
</geometry>
<material name="LightGrey">
<color rgba="0.7 0.7 0.7 1.0"/>
</material>
</visual>
<collision>
<geometry>
<mesh filename="package://ur5_description/meshes/ur5/collision/base.stl" />
</geometry>
</collision>
<xacro:cylinder_inertial radius="0.06" length="0.05" mass="${base_mass}">
<origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
</xacro:cylinder_inertial>
</link>
ur5_descriptionBelow we will load the Gazebo simulation environment we just created .
3. Gazebo loads the robot model
3.1 Create ur5_gazebo ROS package
Create ur5_gazeboa ROS package.
~/catkin_ws/src/ur_sim$ catkin_creake_pkg ur5_gazebo
And create some folders. After creation, the directory of this package looks like this:
~/catkin_ws/src/ur_sim/ur5_gazebo$ tree -L 2
.
├── CMakeLists.txt
├── controller
│ └── ur_gazebo_controller.yaml
├── launch
│ ├── ur_controller.launch # "controller_spawner" load controllers
│ ├── ur_gazebo_control.launch # include other two launch files
│ └── ur_gazebo.launch # load gazebo world and spawn robot model
├── package.xml
└── src
3.2 launch file load simulation environment
We write the above here ur_gazebo.launch, which is responsible for loading Gazebo world and robot models. Loading xx in Gazebo is generally called spawnxx. For example, loading a robot model will start a spawn_modelnode called xx, and loading the controller later will start a controller_spawnernode called xx. So everyone sees spawnthis word Just know what to load.
<!-- ur_gazebo.launch 加载Gazebo 世界和机器人模型 -->
<?xml version="1.0"?>
<launch>
<!-- Gazebo world -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" default="worlds/empty.world"/>
</include>
<!-- parse xacro file into urdf and upload urdf to parameter server -->
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find ur5_description)/urdf/ur5_robot.urdf.xacro'" />
<!-- spawn robot in gazebo -->
<node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot" respawn="false" output="screen" />
</launch>
One of the problems I encountered at the beginning was that the launch file can be read clearly, but I don’t know how to write it. I actually took it from other packages, deleted, edited and rewritten it. If you don’t understand the launch file, you can read it The official Tag reference of ROS launch XML format
. If you have any questions, please refer to the ROS wiki. Other online tutorials are fragmented, and many questions can be answered by reading the ROS wiki.
After that, you can start gazebo to see the robot model
$ roslaunch ur5_gazebo ur_gazebo.launch
It is found that UR is lying on the ground, and the end also interferes with the ground, which is why the parameters passed in `spawn_model` in the official `ur5.launch` must have `-z 0.1`, because the UR base needs to be raised 0.1m, otherwise it will interfere with the ground.
<!-- UR原本包的ur5_gazebo.launch中的一行 -->
<node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot -z 0.1" respawn="false" output="screen" />
Generally, the initial pose of UR is upright. Let’s modify the description file of UR to change the initial pose of UR to vertical.
3.3 Modify the initial pose of the robot
shoulder_lift_jointLooking at the model of UR, what we have to do is to rotate the initial joint angle of the second joint − π / 2 -\pi/2− π / 2 . We open the description file of URur5.urdf.xacroand find the second joint. Here we refer to the ROS wiki, modify the Euler angles of the two axes.
(optional: defaults to identity if not specified)
This is the transform from the parent link to the child link. The joint is located at the origin of the child link, as shown in the figure above.
xyz (optional: defaults to zero vector): Represents the offset. All positions are specified in meters.
rpy (optional: defaults 'to zero vector 'if not specified): Represents the rotation around fixed axis: first roll around x, then pitch around y and finally yaw around z. All angles are specified in radians.
<joint name="${prefix}shoulder_lift_joint" type="revolute">
<parent link="${prefix}shoulder_link" />
<child link = "${prefix}upper_arm_link" />
<origin xyz="0.0 ${shoulder_offset} 0.0" rpy="0.0 0.0 0.0" />
<origin>This line was originally rpy="0 ${pi/2.0} 0", the original two axes yaw has a 90° rotation at the beginning, we changed it to three 0s.
After restarting, you can find that the initial pose of UR is vertical. But the robot arm will shake during operation, and I don’t know why yet.
4. ROS Control - Let the robotic arm move in Gazebo
The figure above shows how gazebo_ros_control simulates the real hardware interface for simulation, which is what we use to make the robotic arm move. This is the tutorial ROS Control tutorial of ROS Control .
4.1 and 4.2 are just to explain to everyone, there is no need to operate anything.
4.1 transmission tag
Next, we will use the ROS control package to set up the simulation controller to drive the joints of the robot in Gazebo, so that the mechanical arm can move. To control the robot with ROS control, the joints in the urdf file must have labels <transmission>.
We ur5.urdf.xacrosee such a line
<xacro:include filename="$(find ur_description)/urdf/ur.transmission.xacro" />
This is to ur.transmission.xacroinclude this file, which contains <transmission>the information of each joint, for example, the following is the information of one axis.
<transmission name="${prefix}shoulder_pan_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${prefix}shoulder_pan_joint">
<hardwareInterface>${hw_interface}</hardwareInterface>
</joint>
<actuator name="${prefix}shoulder_pan_motor">
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
4.2 gazebo_ros_control plugin
In the URDF file, in addition to the tags, a gazebo_ros_control plugin needs to be added to parse the tags and load the appropriate interfaces and controllers.
ur5_robot.urdf.xacroIncluded incommon.gazebo.xacro
<xacro:include filename="$(find ur5_description)/urdf/common.gazebo.xacro" />
common.gazebo.xacroThe ros_control plug-in is loaded again.
<plugin name="ros_control" filename="libgazebo_ros_control.so">
<!--robotNamespace>/</robotNamespace-->
<!--robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType-->
</plugin>
4.3 ROS control package
Below we write the virtual controller fileur_gazebo_controller.yaml
# Publish all joint states -----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
# Position Controllers ---------------------------------------
joint1_position_controller:
type: position_controllers/JointPositionController
joint: shoulder_pan_joint
pid: {
p: 100.0, i: 0.01, d: 10.0}
joint2_position_controller:
type: position_controllers/JointPositionController
joint: shoulder_lift_joint
pid: {
p: 100.0, i: 0.01, d: 10.0}
joint3_position_controller:
type: position_controllers/JointPositionController
joint: elbow_joint
pid: {
p: 100.0, i: 0.01, d: 10.0}
joint4_position_controller:
type: position_controllers/JointPositionController
joint: wrist_1_joint
pid: {
p: 100.0, i: 0.01, d: 10.0}
joint5_position_controller:
type: position_controllers/JointPositionController
joint: wrist_2_joint
pid: {
p: 100.0, i: 0.01, d: 10.0}
joint6_position_controller:
type: position_controllers/JointPositionController
joint: wrist_3_joint
pid: {
p: 100.0, i: 0.01, d: 10.0}
If you ask why you write this way, I can only say that you should read the ROS control tutorial.
Then write a launch file ur_controller.launchto start the controller.
<?xml version="1.0"?>
<launch>
<rosparam file="$(find ur5_gazebo)/controller/ur_gazebo_controller.yaml" command="load"/>
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" ns="/"
args="joint_state_controller
joint1_position_controller
joint2_position_controller
joint3_position_controller
joint4_position_controller
joint5_position_controller
joint6_position_controller"/>
<node name="ur_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="false" output="screen">
<!-- <remap from="/joint_states" to="/arm/joint_states"/> -->
</node>
</launch>
After loading ur_gazebo.launchthe UR5 model in Gazebo, start the launch file of this controller, and then you can control each joint angle.
[INFO] [1619959151.260079, 0.000000]: Loading controller: joint_state_controller
[INFO] [1619959195.207835, 5.160000]: Loading controller: joint1_position_controller
[INFO] [1619959195.245979, 5.196000]: Loading controller: joint2_position_controller
[INFO] [1619959195.270927, 5.218000]: Loading controller: joint3_position_controller
[INFO] [1619959195.298858, 5.240000]: Loading controller: joint4_position_controller
[INFO] [1619959195.332093, 5.268000]: Loading controller: joint5_position_controller
[INFO] [1619959195.348003, 5.283000]: Loading controller: joint6_position_controller
[INFO] [1619959195.368850, 5.300000]: Controller Spawner: Loaded controllers: joint_state_controller, joint1_position_controller, joint2_position_controller, joint3_position_controller, joint4_position_controller, joint5_position_controller, joint6_position_controller
# 能够看到控制器加载成功
[INFO] [1619959195.387982, 5.316000]: Started controllers: joint_state_controller, joint1_position_controller, joint2_position_controller, joint3_position_controller, joint4_position_controller, joint5_position_controller, joint6_position_controller
At this time, look at the topic, you can see that there are 1 to 6 axes /joint1_position_controller/command, and you can control the movement of the robot arm by directly sending the joint angle to these topics.
~/catkin_ws$ rostopic list
/clock
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/set_link_state
/gazebo/set_model_state
/joint1_position_controller/command
/joint2_position_controller/command
/joint3_position_controller/command
/joint4_position_controller/command
/joint5_position_controller/command
/joint6_position_controller/command
/joint_states
/rosout
/rosout_agg
/tf
/tf_static
For a little more effort, you can write another launch file to include the above two launch includes.
<?xml version="1.0"?>
<launch>
<include file="$(find ur5_gazebo)/launch/ur_gazebo.launch"/>
<include file="$(find ur5_gazebo)/launch/ur_controller.launch"/>
</launch>
Reference
Hu Chunxu. "ROS Robot Development Practice"
Tutorial: Using roslaunch to start Gazebo, world files and URDF models