{"id":55,"date":"2019-01-14T20:08:50","date_gmt":"2019-01-14T20:08:50","guid":{"rendered":"http:\/\/erdalpekel.de\/?p=55"},"modified":"2019-03-12T12:38:25","modified_gmt":"2019-03-12T12:38:25","slug":"integrating-franka-emika-panda-robot-into-gazebo","status":"publish","type":"post","link":"https:\/\/erdalpekel.de\/?p=55","title":{"rendered":"Integrating FRANKA EMIKA Panda robot into Gazebo"},"content":{"rendered":"\n

Introduction<\/h2>\n\n\n\n

I have recently integrated the FRANKA EMIKA Panda robot into a Gazebo<\/a><\/em> simulation and wanted to share my experience. The Panda robot is currently the flagship robot used in the MoveIt! tutorials and has both low- and high level open source libraries for integration with ROS.<\/p>\n\n\n\n

The low-level library is called libfranka<\/a><\/em> and can be used for fine-grained information retrieval from the robot and realtime manipulation with custom controllers.<\/p>\n\n\n\n

The high-level library is called franka_ros<\/a><\/em> and integrates tightly with the MoveIt! motion planning framework. It allows a more practical and abstract approach to controlling the robot for customers who are already deeply involved in the ROS<\/em> + MoveIt!<\/em> ecosystem and want to migrate or start out with minimal effort.<\/p>\n\n\n\n

Gazebo<\/em> is an open source dynamics simulation environment with support for various modules and packages that are required to interface with ROS<\/em> for robot manipulation.<\/p>\n\n\n\n

The configuration of the robot for the high-level motion-planning library is called panda_moveit_config<\/em><\/a> and was recently moved from the franka_ros<\/em> package to ros<\/em>-planning.<\/a><\/p>\n\n\n\n

In the following I will outline a step-by-step procedure to simulate the Panda robot in Gazebo<\/em> and connect it to MoveIt!<\/em> for motion planning.<\/p>\n\n\n\n

The procedure<\/h2>\n\n\n\n

For integration into Gazebo<\/em> we need to <\/p>\n\n\n\n

  1. Fix the robot to the world coordinate system<\/li>
  2. Add damping to the joint specifications<\/li>
  3. Add inertia matrices to the links<\/li>
  4. Add mass to the links<\/li>
  5. Add friction to the links<\/li>
  6. Colorize the links<\/li>
  7. Configure the gazebo control plugin gazebo_ros_control<\/em><\/li>
  8. Set initial controller gains<\/li>
  9. Add changes to parent xacro file<\/li><\/ol>\n\n\n\n

    For MoveIt!<\/em> integration we need to<\/p>\n\n\n\n

    1. Add the Gazebo controller configuration to the MoveIt! configuration files<\/li><\/ol>\n\n\n\n

      ROS integration<\/p>\n\n\n\n

      1. Load the robot model onto the ROS parameter server<\/li>
      2. Launch Gazebo and spawn the robot model<\/li>
      3. Load the controller configurations and spawn the controllers with controller_manager<\/li>
      4. Run a robot state publisher to take the joint values and publish them to tf<\/em><\/li>
      5. Launch the move_group files for MoveIt! path planning<\/li>
      6. Remap joint_states to joint_states_desired<\/li><\/ol>\n\n\n\n

        Gazebo integration<\/h2>\n\n\n\n

        Looking at the robot model declaration in the franka_description module inside franka_ros<\/em> we realize that the mass, friction and inertia declarations are missing. But there is more to the integration than adding those values. We will find out in the following.<\/p>\n\n\n\n

        1. Fix the robot to the world coordinate system<\/h3>\n\n\n\n

        To fix the robot to the Gazebo<\/em> world coordinate system we need to enter the following lines into src\/franka_ros\/franka_description\/robots\/panda_arm.xacro<\/strong><\/p>\n\n\n\n

        <link name=\"world\" \/>\n\n<joint name=\"robot_to_world\" type=\"fixed\">\n  <parent link=\"world\" \/>\n  <child link=\"${arm_id}_link0\" \/>\n  <origin xyz=\"0.0 0.0 0.0\" rpy=\"0.0 0.0 0.0\" \/>\n<\/joint><\/code><\/pre>\n\n\n\n
        2. Add damping to the joint specifications<\/h3>\n\n\n\n
        In both the hand and arms xacro file, first add the damping variable definition and then add the <dynamics><\/em> tag with the damping variable to every joint(except for joint8) as demonstrated here for the above added robot_to_world joint:<\/p>\n\n\n\n
        <xacro:property name=\"joint_damping\" value=\"1.0\"\/>\n\n<joint name=\"robot_to_world\" type=\"fixed\">\n  <parent link=\"world\" \/>\n  <child link=\"${arm_id}_link0\" \/>\n  <origin xyz=\"0.0 0.0 0.0\" rpy=\"0.0 0.0 0.0\" \/>\n  <dynamics damping=\"${joint_damping}\"\/>\n<\/joint><\/code><\/pre>\n\n\n\n
        3. Add inertia matrices to the links<\/h3>\n\n\n\n
        Gazebo requires inertia matrices for each link for exact dynamic simulation. Unfortunately FRANKA EMIKA also doesn’t specify anything here. You could load the stl<\/em> model files in an appropriate application that can compute inertia from polygon models but we will stick with a working guess. We will expand all links in the franka_description<\/em> package as follows:<\/p>\n\n\n\n
        <link name=\"${arm_id}_link0\">\n   <visual>\n      <geometry>\n         <mesh filename=\"package:\/\/${description_pkg}\/meshes\/visual\/link0.dae\" \/>\n      <\/geometry>\n   <\/visual>\n   <collision>\n      <geometry>\n         <mesh filename=\"package:\/\/${description_pkg}\/meshes\/collision\/link0.stl\" \/>\n      <\/geometry>\n   <\/collision>\n   <inertial>\n      <origin xyz=\"0 0 0\" rpy=\"0 0 0\" \/>\n      <inertia ixx=\"0.3\" ixy=\"0.0\" ixz=\"0.0\" iyy=\"0.3\" iyz=\"0.0\" izz=\"0.3\" \/>\n   <\/inertial>\n<\/link><\/code><\/pre>\n\n\n\n
        4. Add mass to the links<\/h3>\n\n\n\n
        This is a tricky one, as FRANKA EMIKA does not specify the masses in the specification sheet. Before we outline the calculation let’s first add the mass to the link from above by expanding its inertia definition:<\/p>\n\n\n\n
        <inertial>\n    <origin xyz=\"0 0 0\" rpy=\"0 0 0\" \/>\n    <mass value=\"3.06\" \/>\n    <inertia ixx=\"0.3\" ixy=\"0.0\" ixz=\"0.0\" iyy=\"0.3\" iyz=\"0.0\" izz=\"0.3\" \/>\n<\/inertial><\/code><\/pre>\n\n\n\n
        My mass calculations are based on the volumes of the links which I acquired by loading the models into meshlab<\/em><\/a> which calculates the volume of the given link. When the visual model gave an error I loaded the collision model. Together with the mass of the complete arm(18 kg) and hand(0,7 kg) from the panda specification, I obtained these results: <\/p>\n\n\n\n
        Link<\/td>Model<\/td>Volume<\/td>Mass<\/strong><\/td><\/tr>
        0<\/td>collision<\/td>0,002996<\/td>3,06357<\/strong><\/td><\/tr>
        1<\/td>visual<\/td>0,002293<\/td>2,34471<\/strong><\/td><\/tr>
        2<\/td>visual<\/td>0,002312<\/td>2,36414<\/strong><\/td><\/tr>
        3<\/td>collision<\/td>0,002328<\/td>2,38050<\/strong><\/td><\/tr>
        4<\/td>collision<\/td>0,002374
        <\/td>        		2,42754<\/strong><\/td><\/tr>
        5<\/td>collision<\/td>0,003419<\/td>3,49611<\/strong><\/td><\/tr>
        6<\/td>collision<\/td>0,001435<\/td>1,46736<\/strong><\/td><\/tr>
        7<\/td>collision<\/td>0,000446<\/td>0,45606<\/strong><\/td><\/tr>
        <\/td><\/td>0,017603<\/td>18<\/td><\/tr>
        <\/td><\/td><\/td><\/td><\/tr>
        hand<\/td>collision<\/td>0,000709<\/td>0,67893<\/strong><\/td><\/tr>
        finger1<\/td>visual<\/td>0,000011<\/td>0,01053<\/strong><\/td><\/tr>
        finger2<\/td>visual<\/td>0,000011<\/td>0,01053<\/strong><\/td><\/tr>
        <\/td><\/td>0,000731<\/td>0,7<\/td><\/tr><\/tbody><\/table>\n\n\n\n
        5. Add friction to the links<\/h3>\n\n\n\n
        The Gazebo documentation which explains the friction parameter links to an online resource<\/a> from which I chose the value for Aluminium mild<\/em> : 0,61.
        For the friction we will create a separate file in the same directory as the other xacro files: src\/franka_ros\/franka_description\/robots\/panda.gazebo.xacro<\/strong>
        In this file we define a <gazebo><\/em> tag for each link:<\/p>\n\n\n\n
        <gazebo reference=\"${robot_name}_link0\">\n   <mu1>0.61<\/mu1>\n   <mu2>0.61<\/mu2>\n<\/gazebo><\/code><\/pre>\n\n\n\n
        6. Colorize the links<\/h3>\n\n\n\n
        We will define white color for the links. This can be useful for computer vision tasks in the future.<\/p>\n\n\n\n
        <gazebo reference=\"${robot_name}_link1\">\n   <material>Gazebo\/White<\/material>\n   <mu1>0.2<\/mu1>\n   <mu2>0.2<\/mu2>\n<\/gazebo><\/code><\/pre>\n\n\n\n
        7. Configure the gazebo control plugin gazebo_ros_control<\/em><\/h3>\n\n\n\n
        Now we add the Gazebo controller to the description in order to activate the controller in the simulation. For this we create a new file called panda.transmission.xacro<\/strong> in which we put all the information gazebo needs to simulate the control behavior. More specific information on all tags is provided in the documentation<\/a>. We load the gazebo_ros_control<\/em> plugin and have the option to specify the namespace it is running in(commented out as not used in our case):<\/p>\n\n\n\n
        <gazebo>\n   <plugin name=\"gazebo_ros_control\" filename=\"libgazebo_ros_control.so\">\n     <!-- <robotNamespace>\/${robot_name}<\/robotNamespace> -->\n   <\/plugin>\n<\/gazebo><\/code><\/pre>\n\n\n\n
        Subsequently, we need to define the controller and transmission type for each joint together with the actuator itself. The transmission links the actuator to the joint. Here is an example for joint1 which links link0 and link1:<\/p>\n\n\n\n
        <transmission name=\"${robot_name}_tran_1\">\n   <type>transmission_interface\/SimpleTransmission<\/type>\n   <joint name=\"${robot_name}_joint1\">\n      <hardwareInterface>hardware_interface\/EffortJointInterface<\/hardwareInterface>\n   <\/joint>\n   <actuator name=\"${robot_name}_motor_1\">\n      <hardwareInterface>hardware_interface\/EffortJointInterface<\/hardwareInterface>\n      <mechanicalReduction>1<\/mechanicalReduction>\n   <\/actuator>\n<\/transmission><\/code><\/pre>\n\n\n\n
        8. Set initial controller gains<\/h3>\n\n\n\n
        Instead of guessing the initial controller gains I will share the perfect gains with you so that you don’t have to undergo the time-consuming parameter tuning step like I did. We will create a file called panda_control.yaml<\/strong> for this purpose:<\/p>\n\n\n\n
        #panda:                    #useful if you use a namespace for the robot\n    # Publish joint states\n    joint_state_controller:\n        type: joint_state_controller\/JointStateController\n        publish_rate: 50\n\n    panda_arm_controller:\n        type: effort_controllers\/JointTrajectoryController\n        joints:\n            - panda_joint1\n            - panda_joint2\n            - panda_joint3\n            - panda_joint4\n            - panda_joint5\n            - panda_joint6\n            - panda_joint7\n\n        gains:\n            panda_joint1: { p: 12000, d: 50, i: 0.0, i_clamp: 10000 }\n            panda_joint2: { p: 30000, d: 100, i: 0.02, i_clamp: 10000 }\n            panda_joint3: { p: 18000, d: 50, i: 0.01, i_clamp: 1 }\n            panda_joint4: { p: 18000, d: 70, i: 0.01, i_clamp: 10000 }\n            panda_joint5: { p: 12000, d: 70, i: 0.01, i_clamp: 1 }\n            panda_joint6: { p: 7000, d: 50, i: 0.01, i_clamp: 1 }\n            panda_joint7: { p: 2000, d: 20, i: 0.0, i_clamp: 1 }\n\n        constraints:\n            goal_time: 2.0\n\n        state_publish_rate: 25\n\n    panda_hand_controller:\n        type: effort_controllers\/JointTrajectoryController\n        joints:\n            - panda_finger_joint1\n            - panda_finger_joint2\n\n        gains:\n            panda_finger_joint1: { p: 5, d: 3.0, i: 0, i_clamp: 1 }\n            panda_finger_joint2: { p: 5, d: 1.0, i: 0, i_clamp: 1 }\n\n        state_publish_rate: 25<\/code><\/pre>\n\n\n\n