Adding Simulation Properties
Open the file /urdf/diff_drive.urdf.xacro
and take a brief look at the contents. Notice that links have <interial>
and <collision>
tags - this makes for quite a lengthy looking file. The interia matrix can be found online for simple geometries (sphere, box, cylinder), or if you have a mesh file you can find the inertia matrix from CAD software or software like Meshlab or Cloudcompare. These are critical for the Gazebo physics engine to handle things properly.
This is also the case for real robots! Manipulators for example often need an accurate estimate of the centre of mass and sometimes interia matrix to handle an end-effector or payload correctly. These were touched upon when looking at URDF files in general, this tutorial will focus on additions needed for simulation.
Note
Using xacro allows for variables to be used (and reused) in urdf files, however, the check_urdf
helper does not support xacro macros. To get around this, use the xacro package to convert to a pure .urdf format:
source <WORKSPACE>/install/setup.bash
cd <WORKSPACE>/src/<PACKAGE>/<URDF_DIR>/
check_urdf <(ros2 run xacro xacro <FILENAME>.urdf.xacro)
The xacro xacro
command performs all the substitutions, the output is piped into check_urdf
all in one go. As xacro needs some ROS functionality it is necessary to source the workspace prior to running xacro xacro
.
Xacro properties
Xacro properties is one of the key feature of xacro files. It allows definition of a named variable to be reused in multiple places. In the /urdf/diff_drive.urdf.xacro
we can see the proprties defined as:
18<!-- Values -->
19<xacro:property name="base_link_x" value="0.5" />
20<xacro:property name="base_link_y" value="0.4" />
21<xacro:property name="base_link_z" value="0.3" />
22<xacro:property name="base_link_origin_x" value="0.10" />
23<xacro:property name="base_link_origin_z" value="0.10" />
24
25<xacro:property name="wheel_length" value="0.06" />
26<xacro:property name="wheel_radius" value="0.10" />
27<xacro:property name="wheel_mass" value="1.2" />
28
29<xacro:property name="lidar_mass" value="0.12" />
30<xacro:property name="lidar_width" value="0.10" />
31<xacro:property name="lidar_height" value="0.08" />
32
33<xacro:property name="castor_radius" value="${wheel_radius - (0.5*base_link_z - base_link_origin_z)}"/>
34<xacro:property name="castor_mass" value="0.025" />
Then they are used throughout the reminder of the file, for example in the code below ‘’castor_radius’’ proprety is used (in the line 156 of below snipped).
153<link name="back_castor_link">
154 <visual>
155 <geometry>
156 <sphere radius="${castor_radius}"/>
157 </geometry>
158 <material name="uom_grey"/>
159 </visual>
Spend some time understanding the syntax and modifying the values to see what effect they will have on the robot.
Adding Gazebo Information
At the bottom of /urdf/diff_drive.urdf.xacro
, on line 213 we include another file called /urdf/example_gazebo.xacro
. To make things easier to read, this will contain all the necessary components for simulation.
213<xacro:include filename="$(find example_urdf_robot)/urdf/example_gazebo.xacro" />
Open the example_urdf_robot/urdf/example_gazebo.xacro
file in your text editor of choice. We will walk through each portion individually.
Joint State Publisher
The joint state publisher provides regular publishing of the joint information from the simulation. In this case, it would be the angular velocity of the left and right wheels.
1<?xml version="1.0"?>
2<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
3
4 <!-- GAZEBO PLUGINS -->
5
6
7 <!-- List all movable joints -->
8 <!-- https://gazebosim.org/api/gazebo/6/classignition_1_1gazebo_1_1systems_1_1JointStatePublisher.html -->
9 <gazebo>
10 <plugin filename="ignition-gazebo-joint-state-publisher-system" name="ignition::gazebo::systems::JointStatePublisher">
11 <update_rate>100</update_rate>
12 <joint_name>base_wheel_left_joint</joint_name>
13 <joint_name>base_wheel_right_joint</joint_name>
14
15 <!-- <topic>/world/<world_name>/model/<model_name>/joint_state</topic> THIS IS DEFAULT -->
16 <topic>/model/diff_drive_example/joint_state</topic> <!-- remove world leading prefixes -->
17 </plugin>
18 </gazebo>
By default, all (moveable) joint states are published by Gazebo when we include the JointStatePublisher plugin. If we remove these two lines defining the joints we are interested in, it will still work. However, for completeness specifying the joints has been included. Defining joints manually is good practice to avoid unnecessary use of bandwidth.
12 <joint_name>base_wheel_left_joint</joint_name>
13 <joint_name>base_wheel_right_joint</joint_name>
Differential Drive Controller
Gazebo offers a plugin for a differential drive controller. It takes in arguments regarding wheel radius and wheel separation, and provides the necessary wheel angular velocities - how helpful!
19 <!-- Differential Drive -->
20 <!-- https://gazebosim.org/api/gazebo/6/classignition_1_1gazebo_1_1systems_1_1DiffDrive.html -->
21 <gazebo>
22 <plugin filename="ignition-gazebo-diff-drive-system" name="ignition::gazebo::systems::DiffDrive">
23 <!-- Wheel Joints -->
24 <left_joint>base_wheel_left_joint</left_joint>
25 <right_joint>base_wheel_right_joint</right_joint>
26
27 <!-- Kinematics -->
28 <wheel_separation>${base_link_y + wheel_length}</wheel_separation>
29 <wheel_radius>${wheel_radius}</wheel_radius>
30
31 <odom_publish_frequency>50</odom_publish_frequency>
32
33 <!-- TF Frames -->
34 <frame_id>/odom</frame_id>
35 <child_frame_id>/base_footprint</child_frame_id>
36
37 <!-- topics -->
38 <!-- <topic>/model/diff_drive_example/cmd_vel</topic> THIS IS DEFAULT -->
39 <!-- <odom_topic>/model/diff_drive_example/odometry</odom_topic> THIS IS DEFAULT-->
40 <!-- <tf_topic>/model/diff_drive_example/tf</tf_topic> THIS IS DEFAULT -->
41 <!-- <tf_topic></tf_topic> Leave blank if you plan to use a filter + imu (e.g. EKF) -->
42 </plugin>
43 </gazebo>
Firstly, we tell Gazebo which moveable joints are part of the differential drive system - in this case the left and right wheels.
24 <!-- Wheel Joints -->
25 <left_joint>base_wheel_left_joint</left_joint>
26 <right_joint>base_wheel_right_joint</right_joint>
Secondly, we give the kinematics to calculate motor velocities correctly. Notice that as a xacro, it is possible to use variables from the main .urdf.xacro file (e.g. ${wheel_radius}
) - super helpful!.
28 <!-- Kinematics -->
29 <wheel_separation>${base_link_y + wheel_length}</wheel_separation>
30 <wheel_radius>${wheel_radius}</wheel_radius>
Thirdly, we setup information regarding transformations and estimates of robot position. The odom information is the estimate the robot has of its pose using dead reckoning.
32 <odom_publish_frequency>50</odom_publish_frequency>
33
34 <!-- TF Frames -->
35 <frame_id>/odom</frame_id>
36 <child_frame_id>/base_footprint</child_frame_id>
The odom_publish_frequency
is simply how often we want the differential drive controller to provide the dead reckoning odometry estimate. The <frame_id>
and <child_frame_id>
are simply the frame names for where the robot starts and where to connect the transform to.
Warning
Remember, despite being a simulation, issues such as wheel slip will mean that the odometry estimate will accumulate errors. There are methods to extract the exact pose by different means in Gazebo if you require a ground truth pose estimate.
Finally, we can specify topic names explictly as listed in DiffDrive documentation.
38 <!-- topics -->
39 <!-- <topic>/model/diff_drive_example/cmd_vel</topic> THIS IS DEFAULT -->
40 <!-- <odom_topic>/model/diff_drive_example/odometry</odom_topic> THIS IS DEFAULT-->
41 <!-- <tf_topic>/model/diff_drive_example/tf</tf_topic> THIS IS DEFAULT -->
42 <!-- <tf_topic></tf_topic> Leave blank if you plan to use a filter + imu (e.g. EKF) -->
Note
In this example, we have omitted parameters such as:
min/max jerk (linear and angular)
min/max acceleration (linear and angular)
min/max velocity (linear and angular)
All these can be found in the DiffDrive documentation.
As a “best practice” approach, it is usually sensible to add acceleration and velocity limits. This applies to real robots too, to avoid “jerky” motions.
Sensors
Gazebo offers a range of sensors which can be included in simulation. Note that not all sensors have been implemented yet (see comparison chart), but most are available. Furthermore, it is possible to write your own plugins for sensors or noise models, but that is beyond the scope of this tutorial.
Sensors must be attached to a coordinate frame using the <frame_id>
. For sensors such as cameras or lidars, usually there exists an link defined in the URDF. However, for sensors such as an IMU these are arbitrarily attached to the robot chassis without the need for a specific link. Sensors also need an <update_rate>
tag to state how often they will publish data.
Lidar
48 <gazebo reference="lidar_link">
49 <sensor type="gpu_lidar" name="generic_lidar_sensor">
50
51 <topic>/model/diff_drive_example/scan</topic>
52 <frame_id>lidar_link</frame_id>
53 <ignition_frame_id>lidar_link</ignition_frame_id>
54
55 <update_rate>15.0</update_rate>
56 <ray>
57 <scan>
58 <horizontal>
59 <samples>270</samples>
60 <resolution>1</resolution>
61 <min_angle>-${pi*0.75}</min_angle>
62 <max_angle>${pi*0.75}</max_angle>
63 </horizontal>
64 </scan>
65 <range>
66 <min>0.10</min>
67 <max>8.0</max>
68 <resolution>0.06</resolution>
69 </range>
70 <noise>
71 <type>gaussian</type>
72 <mean>0.0</mean>
73 <stddev>0.02</stddev>
74 </noise>
75 </ray>
76 <always_on>1</always_on>
77 <visualize>true</visualize>
78 </sensor>
79 </gazebo>
80 <gazebo>
81 <plugin filename="libignition-gazebo-sensors-system.so" name="ignition::gazebo::systems::Sensors">
82 <render_engine>ogre2</render_engine>
83 </plugin>
84 </gazebo>
The individual lines will not be discussed in detail, but this offers you an example of how a Lidar sensor can be implemented. It also demonstrates how you can define properties such as the min/max range of the sensor or how many beams it has.
Note
This sensor has been based on the SICK TiM550 series of 2D lidar. The link shows the technical information of the sensor.
IMU
Inertial measurement units are ubiquitous, therefore, it is expected that nearly all mobile robots will have one (less so for manipulators).
87 <gazebo reference="base_link">
88 <sensor name="imu_sensor" type="imu">
89 <topic>/model/diff_drive_example/imu</topic>
90 <frame_id>base_link</frame_id>
91 <ignition_frame_id>base_link</ignition_frame_id>
92 <plugin filename="libignition-gazebo-imu-system.so" name="ignition::gazebo::systems::Imu">
93 <initial_orientation_as_reference>false</initial_orientation_as_reference>
94 </plugin>
95 <always_on>true</always_on>
96 <update_rate>100</update_rate>
97 <visualize>true</visualize>
98 <imu>
99 <angular_velocity>
100 <x>
101 <noise type="gaussian">
102 <mean>0.0</mean>
103 <stddev>2e-4</stddev>
104 <bias_mean>0.0000075</bias_mean>
105 <bias_stddev>0.0000008</bias_stddev>
106 </noise>
107 </x>
108 <y>
109 <noise type="gaussian">
110 <mean>0.0</mean>
111 <stddev>2e-4</stddev>
112 <bias_mean>0.0000075</bias_mean>
113 <bias_stddev>0.0000008</bias_stddev>
114 </noise>
115 </y>
116 <z>
117 <noise type="gaussian">
118 <mean>0.0</mean>
119 <stddev>2e-4</stddev>
120 <bias_mean>0.0000075</bias_mean>
121 <bias_stddev>0.0000008</bias_stddev>
122 </noise>
123 </z>
124 </angular_velocity>
125 <linear_acceleration>
126 <x>
127 <noise type="gaussian">
128 <mean>0.0</mean>
129 <stddev>1.7e-2</stddev>
130 <bias_mean>0.1</bias_mean>
131 <bias_stddev>0.001</bias_stddev>
132 </noise>
133 </x>
134 <y>
135 <noise type="gaussian">
136 <mean>0.0</mean>
137 <stddev>1.7e-2</stddev>
138 <bias_mean>0.1</bias_mean>
139 <bias_stddev>0.001</bias_stddev>
140 </noise>
141 </y>
142 <z>
143 <noise type="gaussian">
144 <mean>0.0</mean>
145 <stddev>1.7e-2</stddev>
146 <bias_mean>0.1</bias_mean>
147 <bias_stddev>0.001</bias_stddev>
148 </noise>
149 </z>
150 </linear_acceleration>
151 </imu>
152 </sensor>
153 </gazebo>
Notice that for each axis of the gyro and accelometer (six in total), there is a <noise>
tag. Once again, characterisation of real sensor hardware can tell us what these values should be in our simulation.
General physics
For our robot to have the wheels stick to the floor, but the castors slide, the coefficients of friction need to modified.
155 <!-- FRICTION AND PHYSICS -->
156 <gazebo reference="back_castor_link">
157 <mu1 value="0.00001"/>
158 <mu2 value="0.00001"/>
159 </gazebo>
160
161 <gazebo reference="front_castor_link">
162 <mu1 value="0.00001"/>
163 <mu2 value="0.00001"/>
164 </gazebo>
165
166 <gazebo reference="wheel_right">
167 <mu1 value="1.0"/>
168 <mu2 value="1.0"/>
169 </gazebo>
170
171 <gazebo reference="wheel_left">
172 <mu1 value="1.0"/>
173 <mu2 value="1.0"/>
174 </gazebo>
175
176
177</robot>
The front and rear castors have their friction values set to a very small value (1e-5), whereas the wheels have a large coefficient value. The actual definitions of <mu>
and <mu2>
are not important for this example, but more details can be found here on GitHub, here on Gazebo Classic docs and here on a third party forum. As a general rule, the two values should be identical.
Now we have examined the gazebo plugins. The next section will run the simulation and explore how to link it into ROS more closely.