Skip to content

Commit cf8c525

Browse files
tahsinkosedavetcoleman
tahsinkose
and
davetcoleman
authored
Add MoveIt+Gazebo Integration tutorial for panda arm (#499)
* Create stub gazebo simulation tutorial * Finish pure-gazebo tutorial for panda arm. * Finish moveit-gazebo tutorial for panda arm. * Add the missing step regarding to auto-generated robot_description parameter speecification. * Update external links for panda_moveit_config package in Gazebo tutorial. Co-authored-by: Dave Coleman <davetcoleman@gmail.com> Co-authored-by: Tahsincan Kose <tahsincankose@gmail.com>
1 parent 2cee639 commit cf8c525

5 files changed

Lines changed: 378 additions & 1 deletion

File tree

‎_static/override.css‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -84,3 +84,11 @@ a:hover {
8484
margin-bottom: 0;
8585
text-align: right;
8686
}
87+
88+
.caption-text {
89+
font-size: 12px;
90+
font-style: italic;
91+
font-weight: normal;
92+
line-height: normal;
93+
color: #404040;
94+
}

‎doc/gazebo_simulation/gazebo_simulation.rst‎

Lines changed: 368 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,368 @@
1+
Gazebo Simulation Integration
2+
=============================
3+
4+
`Gazebo <http://gazebosim.org/>`_ is the most popular robotics simulator in the ROS ecosystem, so is naturally a good fit to integrate with MoveIt.
5+
6+
The `MoveIt Setup Assistant <../setup_assistant/setup_assistant_tutorial.html>`_ helps setup your robot to work with Gazebo, but there are still additional steps required to successfully run MoveIt in Gazebo.
7+
8+
----------------------------
9+
After MoveIt Setup Assistant
10+
----------------------------
11+
This tutorial assumes that the robot is set up with `MoveIt Setup Assistant <../setup_assistant/setup_assistant_tutorial.html>`_,
12+
so it is crucial to follow that document first. To the best of our knowledge, official FRANKA EMIKA Panda repository doesn't particularly consider Gazebo simulation
13+
such that the necessary components to properly simulate the robot in Gazebo are missing with respect to :code:`urdf` and :code:`xacro` files. This is a rare incident, since most other robots
14+
have those components out of the box. *If you have a custom robot, which already works well in Gazebo, you can skip the steps until Step-6.* Fortunately, there is already a good solution offered in `the blog post <https://erdalpekel.de/?p=55>`_ to this problem. For the sake of completion though,
15+
the procedure outlined in there will be repeated (with improvements) in here as well for preparing the robot for Gazebo simulation.
16+
17+
**Note that these steps assume that you have cloned the** `franka_ros repository <https://github.com/frankaemika/franka_ros>`_ **from the source**:
18+
19+
1. Fix the robot to the world coordinate system
20+
2. Add damping to the joint specifications
21+
3. Add inertia matrices and masses to the links
22+
4. Add friction and colorize the links
23+
5. Configure gazebo_ros_control, transmissions and actuators
24+
6. Adjust auto-generated ros_controllers.yaml
25+
7. Adjust auto-generated ros_controllers.launch
26+
27+
28+
1. Fix the robot to the world coordinate system
29+
-----------------------------------------------
30+
Open the :code:`franka_description/robots/panda_arm_hand.urdf.xacro` file and change the line 5 with:
31+
32+
.. code-block:: xml
33+
34+
<xacro:panda_arm xyz="0 0 0" rpy="0 0 0" connected_to="world"/>
35+
36+
It alone doesn't fix the problem, since now we need to provide a link with name :code:`world`. Add the following line to
37+
:code:`franka_description/robots/panda_arm_hand.urdf.xacro`:
38+
39+
.. code-block:: xml
40+
41+
<link name="world" />
42+
43+
at line 10 just before the macro calls. Additionally, we should rename the fixed joint to :code:`virtual_joint` to properly match
44+
the SRDF specification created in previous tutorial. Open :code:`franka_description/robots/panda_arm.xacro` file and replace
45+
:code:`${arm_id}_joint_${connected_to}` with :code:`virtual_joint` at line 5.
46+
47+
2. Add damping to the joint specifications
48+
------------------------------------------
49+
In this step, we need to add:
50+
51+
.. code-block:: xml
52+
53+
<dynamics damping="1.0"/>
54+
55+
to every joint (except :code:`${arm_id}_joint8}` joint) in :code:`franka_description/robots/panda_arm.xacro` file.
56+
57+
58+
3. Add inertia matrices and masses to the links
59+
-----------------------------------------------
60+
Inertia matrices are required for Gazebo physics engine to work properly. We don't require exact numbers for pedagogic reasons,
61+
yet it is perfectly suitable to go after them with MeshLab as explained at `relevant Gazebo documentation page <http://gazebosim.org/tutorials?tut=inertia&cat=build_robot>`_
62+
in great depth. Add following snippet to the links from :code:`${arm_id}_link0` to :code:`${arm_id}_link8` in :code:`franka_description/robots/panda_arm.xacro`:
63+
64+
.. code-block:: xml
65+
66+
<inertial>
67+
<origin xyz="0 0 0" rpy="0 0 0" />
68+
<mass value="3.06" />
69+
<inertia ixx="0.3" ixy="0.0" ixz="0.0" iyy="0.3" iyz="0.0" izz="0.3" />
70+
</inertial>
71+
72+
73+
Similarly add following snippet to the :code:`${ns}_hand` link in :code:`franka_description/robots/hand.xacro` file:
74+
75+
.. code-block:: xml
76+
77+
<inertial>
78+
<origin xyz="0 0 0" rpy="0 0 0"/>
79+
<mass value="0.68"/>
80+
<inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
81+
</inertial>
82+
83+
Finally add following inertia block to :code:`finger` links:
84+
85+
.. code-block:: xml
86+
87+
<inertial>
88+
<origin xyz="0 0 0" rpy="0 0 0"/>
89+
<mass value="0.01"/>
90+
<inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1"/>
91+
</inertial>
92+
93+
As previously mentioned, these values come from the referred blog post. It is explicitly advised to have a look in there to grasp the matter in-depth.
94+
95+
4. Add friction and colorize the links
96+
--------------------------------------
97+
In order to have a nice illustration of the robot in Gazebo simulation we need to colorize the links.
98+
Moreover friction forces are added in order to have realistic dynamics. You can ignore them at all or change their values to experiment with.
99+
Since the focus is MoveIt in this tutorial, we will just use the values from the provided solution.
100+
101+
This step is a bit tedious to do manually, so the ultimate :code:`xacro` file is provided entirely in below:
102+
103+
.. code-block:: xml
104+
105+
<?xml version='1.0' encoding='utf-8'?>
106+
<!-- panda.gazebo.xacro -->
107+
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
108+
<xacro:macro name="panda_gazebo" params="arm_id">
109+
<xacro:macro name="arm_gazebo" params="link">
110+
<gazebo reference="${link}">
111+
<material>Gazebo/White</material>
112+
<mu1>0.2</mu1>
113+
<mu2>0.2</mu2>
114+
</gazebo>
115+
</xacro:macro>
116+
<xacro:macro name="hand_gazebo" params="link">
117+
<gazebo reference="${link}">
118+
<material>Gazebo/Grey</material>
119+
<mu1>0.2</mu1>
120+
<mu2>0.2</mu2>
121+
</gazebo>
122+
</xacro:macro>
123+
<xacro:arm_gazebo link="${arm_id}_link0"/>
124+
<xacro:arm_gazebo link="${arm_id}_link1"/>
125+
<xacro:arm_gazebo link="${arm_id}_link2"/>
126+
<xacro:arm_gazebo link="${arm_id}_link3"/>
127+
<xacro:arm_gazebo link="${arm_id}_link4"/>
128+
<xacro:arm_gazebo link="${arm_id}_link5"/>
129+
<xacro:arm_gazebo link="${arm_id}_link6"/>
130+
<xacro:hand_gazebo link="${arm_id}_link7"/>
131+
<xacro:hand_gazebo link="${arm_id}_link8"/>
132+
<xacro:hand_gazebo link="${arm_id}_hand"/>
133+
<xacro:hand_gazebo link="${arm_id}_rightfinger"/>
134+
<xacro:hand_gazebo link="${arm_id}_leftfinger"/>
135+
</xacro:macro>
136+
</robot>
137+
138+
The filename is specified as an inline comment, but let's be pedantic. It should be named as :code:`panda.gazebo.xacro` and placed next
139+
to the other xacro files.
140+
141+
Then add the following block to the end of :code:`franka_description/robots/panda_arm_hand_urdf.xacro` file:
142+
143+
.. code-block:: xml
144+
145+
<xacro:include filename="$(find franka_description)/robots/panda.gazebo.xacro"/>
146+
<xacro:panda_gazebo arm_id="panda"/>
147+
148+
5. Configure gazebo_ros_control, transmissions and actuators
149+
------------------------------------------------------------
150+
151+
This is necessary for the robot to move in Gazebo. ROS Control is a highly capable robot-agnostic stack, providing interfaces
152+
to control theoretically any type of robot. :code:`gazebo_ros_control` enables the ROS control to be used in Gazebo.
153+
See `its document <http://gazebosim.org/tutorials/?tut=ros_control>`_ for full details.
154+
155+
156+
Along with the transmissions and actuators, which are the crucial components for joints to be able to move in Gazebo,
157+
the plugin specification will be handled in a new file, :code:`panda.control.xacro`. As before, I will provide the full content now:
158+
159+
.. code-block:: xml
160+
161+
<?xml version="1.0"?>
162+
<!-- panda.control.xacro -->
163+
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
164+
<xacro:macro name="panda_control" params="arm_id">
165+
<xacro:macro name="arm_control" params="transmission joint motor">
166+
<transmission name="${transmission}">
167+
<type>transmission_interface/SimpleTransmission</type>
168+
<joint name="${joint}">
169+
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
170+
</joint>
171+
<actuator name="${motor}">
172+
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
173+
<mechanicalReduction>1</mechanicalReduction>
174+
</actuator>
175+
</transmission>
176+
</xacro:macro>
177+
<xacro:arm_control transmission="${arm_id}_tran_1" joint="${arm_id}_joint1" motor="${arm_id}_motor_1"/>
178+
<xacro:arm_control transmission="${arm_id}_tran_2" joint="${arm_id}_joint2" motor="${arm_id}_motor_2"/>
179+
<xacro:arm_control transmission="${arm_id}_tran_3" joint="${arm_id}_joint3" motor="${arm_id}_motor_3"/>
180+
<xacro:arm_control transmission="${arm_id}_tran_4" joint="${arm_id}_joint4" motor="${arm_id}_motor_4"/>
181+
<xacro:arm_control transmission="${arm_id}_tran_5" joint="${arm_id}_joint5" motor="${arm_id}_motor_5"/>
182+
<xacro:arm_control transmission="${arm_id}_tran_6" joint="${arm_id}_joint6" motor="${arm_id}_motor_6"/>
183+
<xacro:arm_control transmission="${arm_id}_tran_7" joint="${arm_id}_joint7" motor="${arm_id}_motor_7"/>
184+
<xacro:arm_control transmission="${arm_id}_leftfinger" joint="${arm_id}_finger_joint1" motor="${arm_id}_finger_joint1"/>
185+
<xacro:arm_control transmission="${arm_id}_rightfinger" joint="${arm_id}_finger_joint2" motor="${arm_id}_finger_joint2"/>
186+
<gazebo>
187+
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so"/>
188+
</gazebo>
189+
</xacro:macro>
190+
</robot>
191+
192+
Again, this file should be placed next to other xacro files in :code:`franka_description` package.
193+
Similarly add the following line to the end of :code:`franka_description/robots/panda_arm_hand_urdf.xacro` file:
194+
195+
.. code-block:: xml
196+
197+
<xacro:include filename="$(find franka_description)/robots/panda.control.xacro"/>
198+
<xacro:panda_control arm_id="panda"/>
199+
200+
6. Adjust auto-generated ros_controllers.yaml
201+
---------------------------------------------
202+
203+
Thankfully the blog post used as the source for this tutorial provides perfectly tuned gains both for hand and arm controllers.
204+
In addition to them, all the necessary control configurations can be grouped in auto-generated :code:`ros_controllers.yaml` file.
205+
Just copy the following snippet and overwrite :code:`panda_moveit_config/config/ros_controllers.yaml` with it:
206+
207+
.. code-block:: xml
208+
209+
# MoveIt-specific simulation settings
210+
moveit_sim_hw_interface:
211+
joint_model_group: controllers_initial_group_
212+
joint_model_group_pose: controllers_initial_pose_
213+
# Settings for ros_control control loop
214+
generic_hw_control_loop:
215+
loop_hz: 300
216+
cycle_time_error_threshold: 0.01
217+
# Settings for ros_control hardware interface
218+
hardware_interface:
219+
joints:
220+
- panda_joint1
221+
- panda_joint2
222+
- panda_joint3
223+
- panda_joint4
224+
- panda_joint5
225+
- panda_joint6
226+
- panda_joint7
227+
- panda_finger_joint1
228+
sim_control_mode: 1 # 0: position, 1: velocity
229+
# Publish all joint states
230+
# Creates the /joint_states topic necessary in ROS
231+
joint_state_controller:
232+
type: joint_state_controller/JointStateController
233+
publish_rate: 50
234+
panda_arm_controller:
235+
type: effort_controllers/JointTrajectoryController
236+
joints:
237+
- panda_joint1
238+
- panda_joint2
239+
- panda_joint3
240+
- panda_joint4
241+
- panda_joint5
242+
- panda_joint6
243+
- panda_joint7
244+
gains:
245+
panda_joint1: { p: 12000, d: 50, i: 0.0, i_clamp: 10000 }
246+
panda_joint2: { p: 30000, d: 100, i: 0.02, i_clamp: 10000 }
247+
panda_joint3: { p: 18000, d: 50, i: 0.01, i_clamp: 1 }
248+
panda_joint4: { p: 18000, d: 70, i: 0.01, i_clamp: 10000 }
249+
panda_joint5: { p: 12000, d: 70, i: 0.01, i_clamp: 1 }
250+
panda_joint6: { p: 7000, d: 50, i: 0.01, i_clamp: 1 }
251+
panda_joint7: { p: 2000, d: 20, i: 0.0, i_clamp: 1 }
252+
253+
constraints:
254+
goal_time: 2.0
255+
state_publish_rate: 25
256+
257+
panda_hand_controller:
258+
type: effort_controllers/JointTrajectoryController
259+
joints:
260+
- panda_finger_joint1
261+
- panda_finger_joint2
262+
263+
gains:
264+
panda_finger_joint1: { p: 5, d: 3.0, i: 0, i_clamp: 1 }
265+
panda_finger_joint2: { p: 5, d: 1.0, i: 0, i_clamp: 1 }
266+
267+
state_publish_rate: 25
268+
269+
controller_list:
270+
- name: panda_arm_controller
271+
action_ns: follow_joint_trajectory
272+
type: FollowJointTrajectory
273+
default: true
274+
joints:
275+
- panda_joint1
276+
- panda_joint2
277+
- panda_joint3
278+
- panda_joint4
279+
- panda_joint5
280+
- panda_joint6
281+
- panda_joint7
282+
- name: panda_hand_controller
283+
action_ns: follow_joint_trajectory
284+
type: FollowJointTrajectory
285+
default: true
286+
joints:
287+
- panda_finger_joint1
288+
- panda_finger_joint2
289+
290+
291+
7. Adjust auto-generated :code:`ros_controllers.launch` in the :code:`panda_moveit_config` package.
292+
---------------------------------------------------------------------------------------------------
293+
294+
Fill the :code:`args` in line 9 with:
295+
296+
.. code-block:: xml
297+
298+
joint_state_controller panda_hand_controller panda_arm_controller
299+
300+
-------------------------------
301+
302+
8. Change the way :code:`robot_description` is loaded to Parameter Server.
303+
--------------------------------------------------------------------------
304+
Open auto-generated :code:`gazebo.launch` in the :code:`panda_moveit_config` package. Find the line starting
305+
with :code:`<param name="robot_description" textfile="$(arg urdf_path)" />` and replace it with:
306+
307+
.. code-block:: xml
308+
309+
<param name="robot_description" command="$(find xacro)/xacro '$(find franka_description)/robots/panda_arm_hand.urdf.xacro'"/>
310+
311+
312+
With this adjustment we are using :code:`xacro` executable that compiles :code:`xacro` files into URDF files.
313+
314+
-------------------------------
315+
316+
At last purely Gazebo way of using the panda robot is ready! In order to be able to control the robot via a simpler
317+
GUI, install `rqt_joint_trajectory_controller <http://wiki.ros.org/rqt_joint_trajectory_controller>`_.
318+
319+
In terminal-1:
320+
321+
.. code-block:: xml
322+
323+
roslaunch panda_moveit_config gazebo.launch
324+
325+
In terminal-2:
326+
327+
.. code-block:: xml
328+
329+
rosrun rqt_joint_trajectory_controller rqt_joint_trajectory_controller
330+
331+
332+
.. figure:: pure-gazebo.gif
333+
:width: 700px
334+
335+
Panda arm control in Gazebo simulation.
336+
337+
If you happen to find all these steps too tedious (you cannot be blamed for that), just clone `the franka_ros fork <https://github.com/tahsinkose/franka_ros>`_, that is created
338+
particularly for this tutorial with the final versions of the files mentioned in the previous steps.
339+
The changes made thus far in auto-generated :code:`panda_moveit_config` package are `in this repository <https://github.com/tahsinkose/panda_moveit_config>`_.
340+
At the end, both repositories will have the updated and directly usable versions.
341+
342+
-------------------------------
343+
344+
Now it is time to integrate MoveIt to this work. Open :code:`panda_moveit_config/launch/demo_gazebo.launch` file
345+
and replace line 61 with:
346+
347+
.. code-block:: xml
348+
349+
<arg name="rviz_config" value="$(find panda_moveit_config)/launch/moveit.rviz"/>
350+
351+
This will allow us to use juicy Motion Planning display of MoveIt in Rviz. There is a final minor issue in `demo_gazebo.launch` file. Remove
352+
line 31 from that file, which contains unused :code:`urdf_path` argument. After that, launch:
353+
354+
.. code-block:: xml
355+
356+
roslaunch panda_moveit_config demo_gazebo.launch
357+
358+
.. figure:: moveit-gazebo.gif
359+
:width: 700px
360+
361+
Panda arm controlled via MoveIt in Gazebo simulation.
362+
363+
364+
We have successfully integrated MoveIt and Gazebo ultimately. MoveIt Setup Assistant already does
365+
many work under the hood, but it still misses some parts to provide a proper Gazebo integration. After following
366+
this tutorial you should be able to reproduce this locally for any robot. In case you don't want to be
367+
bothered with all the details, `franka_ros <https://github.com/tahsinkose/franka_ros>`_ and `panda_moveit_config <https://github.com/tahsinkose/panda_moveit_config>`_
368+
forks provide a ready-made work.

‎doc/gazebo_simulation/moveit-gazebo.gif‎

16.4 MB
Loading

‎doc/gazebo_simulation/pure-gazebo.gif‎

9.68 MB
Loading

‎index.rst‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -54,7 +54,7 @@ Building more complex applications with MoveIt often requires developers to dig
5454

5555
Integration with a New Robot
5656
----------------------------
57-
Before attempting to integrate a new robot with MoveIt, check whether your robot has already been setup (see the `list of robots running MoveIt <http://moveit.ros.org/robots/>`_). Otherwise, follow the tutorials in this section to integrate your robot with MoveIt (and share your results on the MoveIt mailing list)
57+
Before attempting to integrate a new robot with MoveIt, check whether your robot has already been setup (see the `list of robots running MoveIt <http://moveit.ros.org/robots/>`_). Otherwise, follow the tutorials in this section to integrate your robot with MoveIt (and share your results on the MoveIt Discourse Channel)
5858

5959
.. toctree::
6060
:maxdepth: 1
@@ -87,6 +87,7 @@ Miscellaneous
8787
.. toctree::
8888
:maxdepth: 1
8989

90+
doc/gazebo_simulation/gazebo_simulation.rst
9091
doc/joystick_control_teleoperation/joystick_control_teleoperation_tutorial
9192
doc/realtime_servo/realtime_servo_tutorial
9293
doc/benchmarking/benchmarking_tutorial

0 commit comments

Comments
 (0)