Gazebo Simulation Integration

Gazebo is the most popular robotics simulator in the ROS ecosystem, so is naturally a good fit to integrate with MoveIt.

The MoveIt Setup Assistant helps setup your robot to work with Gazebo, but there are still additional steps required to successfully run MoveIt in Gazebo.

After MoveIt Setup Assistant

This tutorial assumes that the robot is set up with MoveIt Setup Assistant, 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 urdf and 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.

Note that these steps assume that you have cloned the franka_ros repository from the source:

  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

1. Fix the robot to the world coordinate system

Open the franka_description/robots/panda_arm_hand.urdf.xacro file and change the line 5 with:

<xacro:panda_arm xyz="0 0 0" rpy="0 0 0" connected_to="world"/>

It alone doesn’t fix the problem, since now we need to provide a link with name world. Add the following line to franka_description/robots/panda_arm_hand.urdf.xacro:

<link name="world" />

at line 10 just before the macro calls. Additionally, we should rename the fixed joint to virtual_joint to properly match the SRDF specification created in previous tutorial. Open franka_description/robots/panda_arm.xacro file and replace ${arm_id}_joint_${connected_to} with virtual_joint at line 5.

2. Add damping to the joint specifications

In this step, we need to add:

<dynamics damping="1.0"/>

to every joint (except ${arm_id}_joint8} joint) in franka_description/robots/panda_arm.xacro file.

5. Configure gazebo_ros_control, transmissions and actuators

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. gazebo_ros_control enables the ROS control to be used in Gazebo. See its document for full details.

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, panda.control.xacro. As before, I will provide the full content now:

<?xml version="1.0"?>
<!-- panda.control.xacro -->
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    <xacro:macro name="panda_control" params="arm_id">
        <xacro:macro name="arm_control" params="transmission joint motor">
            <transmission name="${transmission}">
                <type>transmission_interface/SimpleTransmission</type>
                <joint name="${joint}">
                    <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
                </joint>
                <actuator name="${motor}">
                    <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
                    <mechanicalReduction>1</mechanicalReduction>
                </actuator>
            </transmission>
        </xacro:macro>
        <xacro:arm_control transmission="${arm_id}_tran_1" joint="${arm_id}_joint1" motor="${arm_id}_motor_1"/>
        <xacro:arm_control transmission="${arm_id}_tran_2" joint="${arm_id}_joint2" motor="${arm_id}_motor_2"/>
        <xacro:arm_control transmission="${arm_id}_tran_3" joint="${arm_id}_joint3" motor="${arm_id}_motor_3"/>
        <xacro:arm_control transmission="${arm_id}_tran_4" joint="${arm_id}_joint4" motor="${arm_id}_motor_4"/>
        <xacro:arm_control transmission="${arm_id}_tran_5" joint="${arm_id}_joint5" motor="${arm_id}_motor_5"/>
        <xacro:arm_control transmission="${arm_id}_tran_6" joint="${arm_id}_joint6" motor="${arm_id}_motor_6"/>
        <xacro:arm_control transmission="${arm_id}_tran_7" joint="${arm_id}_joint7" motor="${arm_id}_motor_7"/>
        <xacro:arm_control transmission="${arm_id}_leftfinger" joint="${arm_id}_finger_joint1" motor="${arm_id}_finger_joint1"/>
        <xacro:arm_control transmission="${arm_id}_rightfinger" joint="${arm_id}_finger_joint2" motor="${arm_id}_finger_joint2"/>
        <gazebo>
            <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so"/>
        </gazebo>
    </xacro:macro>
</robot>

Again, this file should be placed next to other xacro files in franka_description package. Similarly add the following line to the end of franka_description/robots/panda_arm_hand_urdf.xacro file:

<xacro:include filename="$(find franka_description)/robots/panda.control.xacro"/>
<xacro:panda_control arm_id="panda"/>

6. Adjust auto-generated ros_controllers.yaml

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 ros_controllers.yaml file. Just copy the following snippet and overwrite panda_moveit_config/config/ros_controllers.yaml with it:

# 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
joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50
panda_arm_controller:
    type: effort_controllers/JointTrajectoryController
    joints:
        - panda_joint1
        - panda_joint2
        - panda_joint3
        - panda_joint4
        - panda_joint5
        - 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 }

    constraints:
        goal_time: 2.0
    state_publish_rate: 25

panda_hand_controller:
    type: effort_controllers/JointTrajectoryController
    joints:
        - panda_finger_joint1
        - 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 }

    state_publish_rate: 25

controller_list:
    - name: panda_arm_controller
      action_ns: follow_joint_trajectory
      type: FollowJointTrajectory
      default: true
      joints:
        - panda_joint1
        - panda_joint2
        - panda_joint3
        - panda_joint4
        - panda_joint5
        - panda_joint6
        - panda_joint7
    - name: panda_hand_controller
      action_ns: follow_joint_trajectory
      type: FollowJointTrajectory
      default: true
      joints:
        - panda_finger_joint1
        - panda_finger_joint2

7. Adjust auto-generated ros_controllers.launch in the panda_moveit_config package.

Fill the args in line 9 with:

joint_state_controller panda_hand_controller panda_arm_controller

8. Change the way robot_description is loaded to Parameter Server.

Open auto-generated gazebo.launch in the panda_moveit_config package. Find the line starting with <param name="robot_description" textfile="$(arg urdf_path)" /> and replace it with:

<param name="robot_description" command="$(find xacro)/xacro '$(find franka_description)/robots/panda_arm_hand.urdf.xacro'"/>

With this adjustment we are using xacro executable that compiles 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.

In terminal-1:

roslaunch panda_moveit_config gazebo.launch

In terminal-2:

rosrun rqt_joint_trajectory_controller rqt_joint_trajectory_controller
../../_images/pure-gazebo.gif

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 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 panda_moveit_config/launch/demo_gazebo.launch file and replace line 61 with:

<arg name="rviz_config" value="$(find panda_moveit_config)/launch/moveit.rviz"/>

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 urdf_path argument. After that, launch:

roslaunch panda_moveit_config demo_gazebo.launch
../../_images/moveit-gazebo.gif

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.

Open Source Feedback

See something that needs improvement? Please open a pull request on this GitHub page