diff --git a/doc/gazebo_simulation/gazebo_simulation.rst b/doc/gazebo_simulation/gazebo_simulation.rst
index 1ce098075..a6006462b 100644
--- a/doc/gazebo_simulation/gazebo_simulation.rst
+++ b/doc/gazebo_simulation/gazebo_simulation.rst
@@ -1,65 +1,56 @@
Gazebo Simulation Integration
=============================
-`Gazebo `_ is the most popular robotics simulator in the ROS ecosystem, so is naturally a good fit to integrate with MoveIt.
+`Gazebo `_ is the most popular robotics simulator in the ROS ecosystem, so it is naturally a good fit to integrate with MoveIt.
-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.
+The `MoveIt Setup Assistant <../setup_assistant/setup_assistant_tutorial.html>`_ helps setup your robot to work with Gazebo, but there are still some additional steps required to successfully run MoveIt with Gazebo.
----------------------------
After MoveIt Setup Assistant
----------------------------
-This tutorial assumes that the robot is set up with `MoveIt Setup Assistant <../setup_assistant/setup_assistant_tutorial.html>`_,
-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
-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
-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 `_ to this problem. For the sake of completion though,
-the procedure outlined in there will be repeated (with improvements) in here as well for preparing the robot for Gazebo simulation.
+This tutorial assumes that the robot was set up with `MoveIt Setup Assistant <../setup_assistant/setup_assistant_tutorial.html>`_,
+so it is crucial to follow that tutorial first. To prepare your robot for Gazebo simulation a few configuration settings are required as indicated by the following list:
-**Note that these steps assume that you have cloned the** `franka_ros repository `_ **from the source**:
+1. :ref:`Fix the robot to the world coordinate system`
+2. :ref:`Add dynamics parameters to the joint specifications`
+3. :ref:`Add inertia matrices and masses to the links`
+4. :ref:`Add material properties to links like friction and color`
+5. :ref:`Configure gazebo_ros_control, transmissions and actuators`
+6. :ref:`Configure ros_control controllers`
-1. Fix the robot to the world coordinate system
-2. Add damping to the joint specifications
-3. Add inertia matrices and masses to the links
-4. Add friction and colorize the links
-5. Configure gazebo_ros_control, transmissions and actuators
-6. Adjust auto-generated ros_controllers.yaml
-7. Adjust auto-generated ros_controllers.launch
+.. _fix to world frame:
1. Fix the robot to the world coordinate system
-----------------------------------------------
-Open the :code:`franka_description/robots/panda/panda.urdf.xacro` file and change the line 5 with:
-
-.. code-block:: xml
-
-
-
-It alone doesn't fix the problem, since now we need to provide a link with name :code:`world`. Add the following line to
-:code:`franka_description/robots/panda/panda.urdf.xacro`:
+In order to fixate your robot to Gazebo's world frame, you need to add a fixed joint between a link called :code:`world` and your robot's base link:
.. code-block:: xml
-
+
+
+
+
+
+
-at line 10 just before the macro calls. Additionally, we should rename the fixed joint to :code:`virtual_joint` to properly match
-the SRDF specification created in previous tutorial. Open :code:`franka_description/robots/panda_arm.xacro` file and replace
-:code:`${arm_id}_joint_${connected_to}` with :code:`virtual_joint` at line 5.
+.. _dynamics params for joints:
-2. Add damping to the joint specifications
-------------------------------------------
-In this step, we need to add:
+2. Add dynamics parameters to the joint specifications
+------------------------------------------------------
+For correct physical simulation of your joints, we need to add dynamics parameters (damping and friction) to every movable joint definition:
.. code-block:: xml
-
-
-to every joint (except :code:`${arm_id}_joint8}` joint) in :code:`franka_description/robots/panda_arm.xacro` file.
+
+.. _inertia params for links:
3. Add inertia matrices and masses to the links
-----------------------------------------------
-Inertia matrices are required for Gazebo physics engine to work properly. We don't require exact numbers for pedagogic reasons,
-yet it is perfectly suitable to go after them with MeshLab as explained at `relevant Gazebo documentation page `_
-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`:
+For correct physical simulation of your links, we need to define inertia properties for every link.
+If your robot manufacturer doesn't provide the corresponding values, you can estimate them via :code:`MeshLab` as explained in the relevant `Gazebo documentation page `_.
+Add the following XML snippet to your URDF link definitions:
.. code-block:: xml
@@ -69,168 +60,79 @@ in great depth. Add following snippet to the links from :code:`${arm_id}_link0`
+.. _material props for links:
-Similarly add following snippet to the :code:`${ns}_hand` link in :code:`franka_description/robots/hand.xacro` file:
+4. Add material properties to links like friction and color
+-----------------------------------------------------------
+In order to have a nice colorization of the robot in Gazebo's visualization we need to define material properties.
+Besides colors, these also include friction parameters µ1 and µ2, which should be chosen identical usually.
+They define the Coloumb friction coefficients for two orthogonal directions in the contact plane.
+For each link add the following XML snippet at the top level of your URDF document:
.. code-block:: xml
-
-
-
-
-
-
-Finally add following inertia block to :code:`finger` links:
-
-.. code-block:: xml
-
-
-
-
-
-
+
+ Gazebo/White
+ 0.2
+ 0.2
+
-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.
+.. _transmissions and actuators:
-4. Add friction and colorize the links
---------------------------------------
-In order to have a nice illustration of the robot in Gazebo simulation we need to colorize the links.
-Moreover friction forces are added in order to have realistic dynamics. You can ignore them at all or change their values to experiment with.
-Since the focus is MoveIt in this tutorial, we will just use the values from the provided solution.
-
-This step is a bit tedious to do manually, so the ultimate :code:`xacro` file is provided entirely in below:
+5. Configure gazebo_ros_control, transmissions and actuators
+------------------------------------------------------------
-.. code-block:: xml
+To enable the actuation of your robot in Gazebo, we need to configure ros_control.
+ROS Control is a highly capable robot-agnostic stack, providing interfaces to control theoretically any type of robot. :code:`gazebo_ros_control` enables ROS control to be used in Gazebo.
+See `its document `_ for full details.
-
-
-
-
-
-
- Gazebo/White
- 0.2
- 0.2
-
-
-
-
- Gazebo/Grey
- 0.2
- 0.2
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-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
-to the other xacro files.
-
-Then add the following block to the end of :code:`franka_description/robots/panda_arm_hand_urdf.xacro` file:
+To define transmissions and actuators for every joint, we define a reusable xacro macro first:
.. code-block:: xml
-
-
+
+
+ transmission_interface/SimpleTransmission
+
+
+ hardware_interface/EffortJointInterface
+
+
+ hardware_interface/EffortJointInterface
+ 1
+
+
+
-5. Configure gazebo_ros_control, transmissions and actuators
-------------------------------------------------------------
+Subsequently, we can use the macro to instantiate transmissions and actuators for every joint:
-This is necessary for the robot to move in Gazebo. ROS Control is a highly capable robot-agnostic stack, providing interfaces
-to control theoretically any type of robot. :code:`gazebo_ros_control` enables the ROS control to be used in Gazebo.
-See `its document `_ for full details.
+.. code-block:: xml
+
-Along with the transmissions and actuators, which are the crucial components for joints to be able to move in Gazebo,
-the plugin specification will be handled in a new file, :code:`panda.control.xacro`. As before, I will provide the full content now:
+Further, we need to configure the :code:`gazebo_ros_control` plugin:
.. code-block:: xml
-
-
-
-
-
-
- transmission_interface/SimpleTransmission
-
- hardware_interface/EffortJointInterface
-
-
- hardware_interface/EffortJointInterface
- 1
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-Again, this file should be placed next to other xacro files in :code:`franka_description` package.
-Similarly add the following line to the end of :code:`franka_description/robots/panda_arm_hand_urdf.xacro` file:
-
-.. code-block:: xml
+
+
+
-
-
+.. _controller config:
-6. Adjust auto-generated ros_controllers.yaml
----------------------------------------------
+6. Configure ros_control controllers
+------------------------------------
-Thankfully the blog post used as the source for this tutorial provides perfectly tuned gains both for hand and arm controllers.
-In addition to them, all the necessary control configurations can be grouped in auto-generated :code:`ros_controllers.yaml` file.
-Just copy the following snippet and overwrite :code:`panda_moveit_config/config/ros_controllers.yaml` with it:
+Next, we need to configure and tune the controllers for the robot. The following gain parameters are just coarse examples and should be tuned for your specific robot. See the `ros_control documentation `_ for more details. Edit your MoveIt config's :code:`ros_controllers.yaml` file along these lines (this is an example for the Panda robot):
-.. code-block:: xml
+.. code-block:: yaml
- # MoveIt-specific simulation settings
- moveit_sim_hw_interface:
- joint_model_group: controllers_initial_group_
- joint_model_group_pose: controllers_initial_pose_
- # Settings for ros_control control loop
- generic_hw_control_loop:
- loop_hz: 300
- cycle_time_error_threshold: 0.01
- # Settings for ros_control hardware interface
- hardware_interface:
- joints:
- - panda_joint1
- - panda_joint2
- - panda_joint3
- - panda_joint4
- - panda_joint5
- - panda_joint6
- - panda_joint7
- - panda_finger_joint1
- sim_control_mode: 1 # 0: position, 1: velocity
- # Publish all joint states
- # Creates the /joint_states topic necessary in ROS
+ # Publish joint states
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
+
+ # Configure effort-based trajectory controller for the Panda arm
panda_arm_controller:
type: effort_controllers/JointTrajectoryController
joints:
@@ -242,18 +144,19 @@ Just copy the following snippet and overwrite :code:`panda_moveit_config/config/
- panda_joint6
- panda_joint7
gains:
- panda_joint1: { p: 12000, d: 50, i: 0.0, i_clamp: 10000 }
- panda_joint2: { p: 30000, d: 100, i: 0.02, i_clamp: 10000 }
- panda_joint3: { p: 18000, d: 50, i: 0.01, i_clamp: 1 }
- panda_joint4: { p: 18000, d: 70, i: 0.01, i_clamp: 10000 }
- panda_joint5: { p: 12000, d: 70, i: 0.01, i_clamp: 1 }
- panda_joint6: { p: 7000, d: 50, i: 0.01, i_clamp: 1 }
- panda_joint7: { p: 2000, d: 20, i: 0.0, i_clamp: 1 }
+ panda_joint1: { p: 15000, d: 50, i: 0.01 }
+ panda_joint2: { p: 15000, d: 50, i: 0.01 }
+ panda_joint3: { p: 15000, d: 50, i: 0.01 }
+ panda_joint4: { p: 15000, d: 50, i: 0.01 }
+ panda_joint5: { p: 10000, d: 50, i: 0.01 }
+ panda_joint6: { p: 5000, d: 50, i: 0.01 }
+ panda_joint7: { p: 2000, d: 50, i: 0.01 }
+ state_publish_rate: 25
constraints:
goal_time: 2.0
- state_publish_rate: 25
+ # Configure effort-based trajectory controller for the Panda hand
panda_hand_controller:
type: effort_controllers/JointTrajectoryController
joints:
@@ -261,11 +164,12 @@ Just copy the following snippet and overwrite :code:`panda_moveit_config/config/
- panda_finger_joint2
gains:
- panda_finger_joint1: { p: 5, d: 3.0, i: 0, i_clamp: 1 }
- panda_finger_joint2: { p: 5, d: 1.0, i: 0, i_clamp: 1 }
+ panda_finger_joint1: { p: 5, d: 1.0, i: 0 }
+ panda_finger_joint2: { p: 5, d: 1.0, i: 0 }
state_publish_rate: 25
+ # Declare available controllers for MoveIt
controller_list:
- name: panda_arm_controller
action_ns: follow_joint_trajectory
@@ -279,6 +183,7 @@ Just copy the following snippet and overwrite :code:`panda_moveit_config/config/
- panda_joint5
- panda_joint6
- panda_joint7
+
- name: panda_hand_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
@@ -288,33 +193,10 @@ Just copy the following snippet and overwrite :code:`panda_moveit_config/config/
- panda_finger_joint2
-7. Adjust auto-generated :code:`ros_controllers.launch` in the :code:`panda_moveit_config` package.
----------------------------------------------------------------------------------------------------
-
-Fill the :code:`args` in line 9 with:
-
-.. code-block:: xml
-
- joint_state_controller panda_hand_controller panda_arm_controller
-
-------------------------------
-8. Change the way :code:`robot_description` is loaded to Parameter Server.
---------------------------------------------------------------------------
-Open auto-generated :code:`gazebo.launch` in the :code:`panda_moveit_config` package. Find the line starting
-with :code:`` and replace it with:
-
-.. code-block:: xml
-
-
-
-
-With this adjustment we are using :code:`xacro` executable that compiles :code:`xacro` files into URDF files.
-
--------------------------------
-
-At last purely Gazebo way of using the panda robot is ready! In order to be able to control the robot via a simpler
-GUI, install `rqt_joint_trajectory_controller `_.
+Now we can control our robot in Gazebo via ROS and MoveIt.
+For a simple GUI interface with sliders to move your joints, install `rqt_joint_trajectory_controller `_.
In terminal-1:
@@ -334,22 +216,9 @@ In terminal-2:
Panda arm control in Gazebo simulation.
-If you happen to find all these steps too tedious (you cannot be blamed for that), just clone `the franka_ros fork `_, that is created
-particularly for this tutorial with the final versions of the files mentioned in the previous steps.
-The changes made thus far in auto-generated :code:`panda_moveit_config` package are `in this repository `_.
-At the end, both repositories will have the updated and directly usable versions.
-
-------------------------------
-Now it is time to integrate MoveIt to this work. Open :code:`panda_moveit_config/launch/demo_gazebo.launch` file
-and replace line 61 with:
-
-.. code-block:: xml
-
-
-
-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
-line 31 from that file, which contains unused :code:`urdf_path` argument. After that, launch:
+Now it is time to use MoveIt and Gazebo together. Just launch:
.. code-block:: xml
@@ -359,10 +228,3 @@ line 31 from that file, which contains unused :code:`urdf_path` argument. After
:width: 700px
Panda arm controlled via MoveIt in Gazebo simulation.
-
-
-We have successfully integrated MoveIt and Gazebo ultimately. MoveIt Setup Assistant already does
-many work under the hood, but it still misses some parts to provide a proper Gazebo integration. After following
-this tutorial you should be able to reproduce this locally for any robot. In case you don't want to be
-bothered with all the details, `franka_ros `_ and `panda_moveit_config `_
-forks provide a ready-made work.