<?xml version="1.0" encoding="utf-8"?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from tiago_dual.urdf.xacro          | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<!--
  Copyright (c) 2019, PAL Robotics, S.L.
  All rights reserved.

  This work is licensed under the Creative Commons Attribution-NonCommercial-NoDerivs 3.0 Unported License.
  To view a copy of this license, visit http://creativecommons.org/licenses/by-nc-nd/3.0/ or send a letter to
  Creative Commons, 444 Castro Street, Suite 900, Mountain View, California, 94041, USA.
-->
<robot name="tiago_dual">
  <material name="Hey5DarkGrey">
    <color rgba="0.1 0.1 0.1 1.0"/>
  </material>
  <!--TODO: Check-->
  <material name="Blue">
    <color rgba="0.0 0.0 0.8 1.0"/>
  </material>
  <material name="Green">
    <color rgba="0.0 0.8 0.0 1.0"/>
  </material>
  <material name="Grey">
    <color rgba="0.7 0.7 0.7 1.0"/>
  </material>
  <material name="LightGrey">
    <color rgba="0.9 0.9 0.9 1.0"/>
  </material>
  <material name="DarkGrey">
    <color rgba="0.1 0.1 0.1 1.0"/>
  </material>
  <material name="Red">
    <color rgba="0.8 0.0 0.0 1.0"/>
  </material>
  <material name="White">
    <color rgba="1.0 1.0 1.0 1.0"/>
  </material>
  <material name="Yellow">
    <color rgba="0.96 0.88 0.14 1.0"/>
  </material>
  <material name="Orange">
    <color rgba="1.0 0.487 0 1.0"/>
  </material>
  <material name="Black">
    <color rgba="0.0 0.0 0.0 1.0"/>
  </material>
  <material name="FlatBlack">
    <color rgba="0.09 0.09 0.09 1.0"/>
  </material>
  <gazebo>
    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
      <ns/>
      <robotSimType>pal_hardware_gazebo/PalHardwareGazebo</robotSimType>
      <robotNamespace/>
      <controlPeriod>0.001</controlPeriod>
    </plugin>
    <plugin filename="libgazebo_ros_p3d.so" name="gazebo_ros_p3d">
      <bodyName>base_footprint</bodyName>
      <topicName>ground_truth_odom</topicName>
      <alwaysOn>true</alwaysOn>
      <updateRate>100.0</updateRate>
    </plugin>
  </gazebo>
  <gazebo>
    <plugin filename="libgazebo_world_odometry.so" name="gazebo_ros_odometry">
      <frameName>base_footprint</frameName>
      <topicName>floating_base_pose_simulated</topicName>
    </plugin>
  </gazebo>
  <gazebo reference="base_imu_link">
    <!-- this is expected to be reparented to pelvis with appropriate offset
         when imu_link is reduced by fixed joint reduction -->
    <!-- todo: this is working with gazebo 1.4, need to write a unit test -->
    <sensor name="imu_sensor" type="imu">
      <always_on>1</always_on>
      <update_rate>1000.0</update_rate>
      <imu>
        <noise>
          <type>gaussian</type>
          <!-- Noise parameters from Boston Dynamics
               (http://gazebosim.org/wiki/Sensor_noise):
                 rates (rad/s): mean=0, stddev=2e-4
                 accels (m/s/s): mean=0, stddev=1.7e-2
                 rate bias (rad/s): 5e-6 - 1e-5
                 accel bias (m/s/s): 1e-1
               Experimentally, simulation provide rates with noise of
               about 1e-3 rad/s and accels with noise of about 1e-1 m/s/s.
               So we don't expect to see the noise unless number of inner iterations
               are increased.

               We will add bias.  In this model, bias is sampled once for rates
               and once for accels at startup; the sign (negative or positive)
               of each bias is then switched with equal probability.  Thereafter,
               the biases are fixed additive offsets.  We choose
               bias means and stddevs to produce biases close to the provided
               data. -->
          <rate>
            <mean>0.0</mean>
            <stddev>2e-4</stddev>
            <bias_mean>0.0000075</bias_mean>
            <bias_stddev>0.0000008</bias_stddev>
          </rate>
          <accel>
            <mean>0.0</mean>
            <stddev>1.7e-2</stddev>
            <bias_mean>0.1</bias_mean>
            <bias_stddev>0.001</bias_stddev>
          </accel>
        </noise>
      </imu>
    </sensor>
  </gazebo>
  <link name="base_laser_link" type="laser">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.02559000000 -0.00056000000 -0.05732000000"/>
      <mass value="0.28922000000"/>
      <inertia ixx="0.00002628919" ixy="0.00000024298" ixz="-0.00000368129" iyy="0.00003374542" iyz="-0.00000001330" izz="0.00005832599"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://pmb2_description/meshes/sensors/sick_tim551.stl"/>
      </geometry>
      <material name="DarkGrey"/>
    </visual>
  </link>
  <joint name="base_laser_joint" type="fixed">
    <axis xyz="0 1 0"/>
    <origin rpy="0 0 0" xyz="0.202 0.0 -0.004"/>
    <parent link="base_link"/>
    <child link="base_laser_link"/>
  </joint>
  <gazebo reference="base_laser_link">
    <sensor name="base_laser" type="ray">
      <pose>0 0 0 0 0 0</pose>
      <update_rate>10</update_rate>
      <visualize>true</visualize>
      <ray>
        <scan>
          <horizontal>
            <!-- 818 (270/0.33) steps in 270deg fov -->
            <samples>666.518518519</samples>
            <resolution>1</resolution>
            <!-- not the sensor resolution; just 1 -->
            <min_angle>-1.91986217719</min_angle>
            <max_angle>1.91986217719</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.05</min>
          <max>25.0</max>
          <resolution>0.001</resolution>
        </range>
      </ray>
      <plugin filename="libgazebo_ros_laser.so" name="gazebo_ros_sick_laser">
        <gaussianNoise>0.06</gaussianNoise>
        <alwaysOn>true</alwaysOn>
        <updateRate>10</updateRate>
        <topicName>scan_raw</topicName>
        <frameName>base_laser_link</frameName>
      </plugin>
    </sensor>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <link name="base_sonar_03_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.001"/>
      <inertia ixx="0.000001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.000001"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://pmb2_description/meshes/sensors/srf05.stl"/>
      </geometry>
      <material name="DarkGrey"/>
    </visual>
  </link>
  <joint name="base_sonar_03_joint" type="fixed">
    <origin rpy="0 0 2.35619449019" xyz="-0.183 0.183 0.173"/>
    <axis xyz="0 0 1"/>
    <parent link="base_link"/>
    <child link="base_sonar_03_link"/>
  </joint>
  <gazebo reference="base_sonar_03_link">
    <sensor name="base_sonar_03" type="ray">
      <pose>0 0 0 0 0 0</pose>
      <update_rate>5</update_rate>
      <visualize>false</visualize>
      <ray>
        <scan>
          <horizontal>
            <samples>5</samples>
            <resolution>1</resolution>
            <min_angle>-0.25</min_angle>
            <max_angle>0.25</max_angle>
          </horizontal>
          <vertical>
            <samples>5</samples>
            <resolution>1</resolution>
            <min_angle>-0.25</min_angle>
            <max_angle>0.25</max_angle>
          </vertical>
        </scan>
        <range>
          <min>0.02</min>
          <max>1.0</max>
          <resolution>0.01</resolution>
        </range>
      </ray>
      <plugin filename="libgazebo_ros_range.so" name="gazebo_ros_range">
        <gaussianNoise>0.005</gaussianNoise>
        <alwaysOn>true</alwaysOn>
        <updateRate>5</updateRate>
        <topicName>sonar_base</topicName>
        <frameName>base_sonar_03_link</frameName>
        <minRange>0.02</minRange>
        <maxRange>1.0</maxRange>
        <fov>0.5</fov>
        <radiation>ultrasound</radiation>
      </plugin>
    </sensor>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <link name="base_sonar_02_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.001"/>
      <inertia ixx="0.000001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.000001"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://pmb2_description/meshes/sensors/srf05.stl"/>
      </geometry>
      <material name="DarkGrey"/>
    </visual>
  </link>
  <joint name="base_sonar_02_joint" type="fixed">
    <origin rpy="0 0 3.14159265359" xyz="-0.259 0.0 0.173"/>
    <axis xyz="0 0 1"/>
    <parent link="base_link"/>
    <child link="base_sonar_02_link"/>
  </joint>
  <gazebo reference="base_sonar_02_link">
    <sensor name="base_sonar_02" type="ray">
      <pose>0 0 0 0 0 0</pose>
      <update_rate>5</update_rate>
      <visualize>false</visualize>
      <ray>
        <scan>
          <horizontal>
            <samples>5</samples>
            <resolution>1</resolution>
            <min_angle>-0.25</min_angle>
            <max_angle>0.25</max_angle>
          </horizontal>
          <vertical>
            <samples>5</samples>
            <resolution>1</resolution>
            <min_angle>-0.25</min_angle>
            <max_angle>0.25</max_angle>
          </vertical>
        </scan>
        <range>
          <min>0.02</min>
          <max>1.0</max>
          <resolution>0.01</resolution>
        </range>
      </ray>
      <plugin filename="libgazebo_ros_range.so" name="gazebo_ros_range">
        <gaussianNoise>0.005</gaussianNoise>
        <alwaysOn>true</alwaysOn>
        <updateRate>5</updateRate>
        <topicName>sonar_base</topicName>
        <frameName>base_sonar_02_link</frameName>
        <minRange>0.02</minRange>
        <maxRange>1.0</maxRange>
        <fov>0.5</fov>
        <radiation>ultrasound</radiation>
      </plugin>
    </sensor>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <link name="base_sonar_01_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.001"/>
      <inertia ixx="0.000001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.000001"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://pmb2_description/meshes/sensors/srf05.stl"/>
      </geometry>
      <material name="DarkGrey"/>
    </visual>
  </link>
  <joint name="base_sonar_01_joint" type="fixed">
    <origin rpy="0 0 3.92699081699" xyz="-0.183 -0.183 0.173"/>
    <axis xyz="0 0 1"/>
    <parent link="base_link"/>
    <child link="base_sonar_01_link"/>
  </joint>
  <gazebo reference="base_sonar_01_link">
    <sensor name="base_sonar_01" type="ray">
      <pose>0 0 0 0 0 0</pose>
      <update_rate>5</update_rate>
      <visualize>false</visualize>
      <ray>
        <scan>
          <horizontal>
            <samples>5</samples>
            <resolution>1</resolution>
            <min_angle>-0.25</min_angle>
            <max_angle>0.25</max_angle>
          </horizontal>
          <vertical>
            <samples>5</samples>
            <resolution>1</resolution>
            <min_angle>-0.25</min_angle>
            <max_angle>0.25</max_angle>
          </vertical>
        </scan>
        <range>
          <min>0.02</min>
          <max>1.0</max>
          <resolution>0.01</resolution>
        </range>
      </ray>
      <plugin filename="libgazebo_ros_range.so" name="gazebo_ros_range">
        <gaussianNoise>0.005</gaussianNoise>
        <alwaysOn>true</alwaysOn>
        <updateRate>5</updateRate>
        <topicName>sonar_base</topicName>
        <frameName>base_sonar_01_link</frameName>
        <minRange>0.02</minRange>
        <maxRange>1.0</maxRange>
        <fov>0.5</fov>
        <radiation>ultrasound</radiation>
      </plugin>
    </sensor>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <link name="base_mic_front_left_link"/>
  <joint name="base_mic_front_left_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.23182 0.03902 0.195"/>
    <child link="base_mic_front_left_link"/>
    <parent link="base_link"/>
  </joint>
  <link name="base_mic_front_right_link"/>
  <joint name="base_mic_front_right_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.23182 -0.03902 0.195"/>
    <child link="base_mic_front_right_link"/>
    <parent link="base_link"/>
  </joint>
  <link name="base_mic_back_left_link"/>
  <joint name="base_mic_back_left_joint" type="fixed">
    <origin rpy="0 0 0" xyz="-0.1082 -0.0599 0.195"/>
    <child link="base_mic_back_left_link"/>
    <parent link="base_link"/>
  </joint>
  <link name="base_mic_back_right_link"/>
  <joint name="base_mic_back_right_joint" type="fixed">
    <origin rpy="0 0 0" xyz="-0.1082 -0.13789 0.195"/>
    <child link="base_mic_back_right_link"/>
    <parent link="base_link"/>
  </joint>
  <!-- Base -->
  <link name="base_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.00000106 0.00000109 0.0000387"/>
      <mass value="28.26649"/>
      <inertia ixx="0.465408937" ixy="0.002160024" ixz="-0.001760255" iyy="0.483193291" iyz="-0.000655952" izz="0.550939703"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://pmb2_description/meshes/base/base.stl"/>
      </geometry>
      <material name="White"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://pmb2_description/meshes/base/base_collision.stl"/>
      </geometry>
      <material name="White"/>
    </collision>
  </link>
  <gazebo reference="base_link">
    <material>Gazebo/White</material>
  </gazebo>
  <!-- Base footprint -->
  <link name="base_footprint"/>
  <joint name="base_footprint_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.0985"/>
    <child link="base_link"/>
    <parent link="base_footprint"/>
  </joint>
  <!-- gazebo parameters are given here for base_link thru base_footprint -->
  <gazebo reference="base_footprint">
    <kp>100000000.0</kp>
    <kd>10.0</kd>
    <mu1>0.1</mu1>
    <mu2>0.1</mu2>
    <fdir1>1 0 0</fdir1>
    <maxVel>10.0</maxVel>
    <minDepth>0.0005</minDepth>
    <laserRetro>0</laserRetro>
  </gazebo>
  <link name="base_cover_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.22414000000 0.00002000000 0.04681000000"/>
      <mass value="0.25821000000"/>
      <inertia ixx="0.00023719188" ixy="0.00000006119" ixz="-0.00000011799" iyy="0.00028335052" iyz="0.00000000774" izz="0.00038497063"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://pmb2_description/meshes/base/base_ring.stl"/>
      </geometry>
      <material name="Orange"/>
    </visual>
  </link>
  <joint name="cover_joint" type="fixed">
    <parent link="base_footprint"/>
    <child link="base_cover_link"/>
    <origin rpy="0 0 0" xyz="0 0 0.13"/>
  </joint>
  <gazebo reference="base_cover_link">
    <material>Gazebo/Orange</material>
  </gazebo>
  <link name="base_antenna_left_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.000001"/>
      <inertia ixx="0.00000002371" ixy="0.00000006119" ixz="0.00000001179" iyy="0.00000002833" iyz="0.00000000774" izz="0.00000003849"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://pmb2_description/meshes/objects/antenna.stl"/>
      </geometry>
      <material name="Black"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.085"/>
      <geometry>
        <cylinder length="0.17" radius="0.005"/>
      </geometry>
      <material name="Black"/>
    </collision>
  </link>
  <gazebo reference="base_antenna_left_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="base_antenna_left_joint" type="fixed">
    <parent link="base_link"/>
    <child link="base_antenna_left_link"/>
    <origin rpy="0 0 0" xyz="-0.201 0.1062 0.195"/>
  </joint>
  <link name="base_antenna_right_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.000001"/>
      <inertia ixx="0.00000002371" ixy="0.00000006119" ixz="0.00000001179" iyy="0.00000002833" iyz="0.00000000774" izz="0.00000003849"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://pmb2_description/meshes/objects/antenna.stl"/>
      </geometry>
      <material name="Black"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.085"/>
      <geometry>
        <cylinder length="0.17" radius="0.005"/>
      </geometry>
      <material name="Black"/>
    </collision>
  </link>
  <gazebo reference="base_antenna_right_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="base_antenna_right_joint" type="fixed">
    <parent link="base_link"/>
    <child link="base_antenna_right_link"/>
    <origin rpy="0 0 0" xyz="-0.201 -0.1062 0.195"/>
  </joint>
  <link name="base_imu_link">
    <inertial>
      <mass value="0.01"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.000001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.000001"/>
    </inertial>
    <!--visual>
        <origin rpy="0 0 0" xyz="0 0 0"/>
        <geometry>
          <box size="0.01 0.01 0.01"/>
        </geometry>
      </visual-->
  </link>
  <joint name="base_imu_joint" type="fixed">
    <parent link="base_link"/>
    <child link="base_imu_link"/>
    <origin rpy="0 0 -1.57079632679" xyz="0.10171 0 0.11282"/>
  </joint>
  <gazebo reference="base_imu_link">
    <sensor name="base_imu_sensor" type="imu">
      <always_on>1</always_on>
      <update_rate>100.0</update_rate>
      <imu>
        <noise>
          <type>gaussian</type>
          <rate>
            <mean>0.0</mean>
            <stddev>2e-4</stddev>
            <bias_mean>0.0000075</bias_mean>
            <bias_stddev>0.0000008</bias_stddev>
          </rate>
          <accel>
            <mean>0.0</mean>
            <stddev>1.7e-2</stddev>
            <bias_mean>0.1</bias_mean>
            <bias_stddev>0.001</bias_stddev>
          </accel>
        </noise>
      </imu>
    </sensor>
  </gazebo>
  <link name="wheel_right_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 -0.00207"/>
      <mass value="1.82362"/>
      <inertia ixx="0.00499743171" ixy="4.464e-08" ixz="-0.00000002245" iyy="0.00499741733" iyz="-1.64e-09" izz="0.00839239692"/>
    </inertial>
    <visual>
      <origin rpy="-0.0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://pmb2_description/meshes/wheels/wheel.stl"/>
      </geometry>
      <material name="DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <sphere radius="0.0985"/>
        <!-- length="${width}"/> -->
      </geometry>
    </collision>
  </link>
  <link name="suspension_right_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 -0.02"/>
      <mass value="10"/>
      <inertia ixx="1.0" ixy="0" ixz="0" iyy="1.0" iyz="0" izz="1.0"/>
    </inertial>
    <kinematic>0</kinematic>
    <gravity>1</gravity>
  </link>
  <joint name="suspension_right_joint" type="prismatic">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="100.0" lower="-0.005" upper="0.005" velocity="1.0"/>
    <child link="suspension_right_link"/>
    <parent link="base_link"/>
    <calibration rising="0.0"/>
    <dynamics damping="100"/>
    <safety_controller k_position="0" k_velocity="10" soft_lower_limit="-0.005" soft_upper_limit="0.005"/>
  </joint>
  <joint name="wheel_right_joint" type="continuous">
    <parent link="suspension_right_link"/>
    <child link="wheel_right_link"/>
    <origin rpy="-1.57079632679 0 0" xyz="0 -0.2022 0.0"/>
    <axis xyz="0 0 1"/>
    <limit effort="6.0" velocity="10.152284264"/>
  </joint>
  <transmission name="wheel_right_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="wheel_right_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="wheel_right_joint">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
  </transmission>
  <!-- Contact model for the wheel surface -->
  <gazebo reference="wheel_right_link">
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <!--
      <mu1>10.0</mu1>
      <mu2>10.0</mu2>
      -->
    <mu1>100000000000000.0</mu1>
    <mu2>100000000000000.0</mu2>
    <!--<fdir1>1 0 0</fdir1>-->
    <maxVel>1.0</maxVel>
    <minDepth>0.002</minDepth>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <gazebo reference="wheel_{side}_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <link name="wheel_left_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0.00207"/>
      <mass value="1.82362"/>
      <inertia ixx="0.00499743171" ixy="-4.464e-08" ixz="-0.00000002245" iyy="0.00499741733" iyz="1.64e-09" izz="0.00839239692"/>
    </inertial>
    <visual>
      <origin rpy="-3.14159265359 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://pmb2_description/meshes/wheels/wheel.stl"/>
      </geometry>
      <material name="DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <sphere radius="0.0985"/>
        <!-- length="${width}"/> -->
      </geometry>
    </collision>
  </link>
  <link name="suspension_left_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 -0.02"/>
      <mass value="10"/>
      <inertia ixx="1.0" ixy="0" ixz="0" iyy="1.0" iyz="0" izz="1.0"/>
    </inertial>
    <kinematic>0</kinematic>
    <gravity>1</gravity>
  </link>
  <joint name="suspension_left_joint" type="prismatic">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="100.0" lower="-0.005" upper="0.005" velocity="1.0"/>
    <child link="suspension_left_link"/>
    <parent link="base_link"/>
    <calibration rising="0.0"/>
    <dynamics damping="100"/>
    <safety_controller k_position="0" k_velocity="10" soft_lower_limit="-0.005" soft_upper_limit="0.005"/>
  </joint>
  <joint name="wheel_left_joint" type="continuous">
    <parent link="suspension_left_link"/>
    <child link="wheel_left_link"/>
    <origin rpy="-1.57079632679 0 0" xyz="0 0.2022 0.0"/>
    <axis xyz="0 0 1"/>
    <limit effort="6.0" velocity="10.152284264"/>
  </joint>
  <transmission name="wheel_left_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="wheel_left_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="wheel_left_joint">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
  </transmission>
  <!-- Contact model for the wheel surface -->
  <gazebo reference="wheel_left_link">
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <!--
      <mu1>10.0</mu1>
      <mu2>10.0</mu2>
      -->
    <mu1>100000000000000.0</mu1>
    <mu2>100000000000000.0</mu2>
    <!--<fdir1>1 0 0</fdir1>-->
    <maxVel>1.0</maxVel>
    <minDepth>0.002</minDepth>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <gazebo reference="wheel_{side}_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <link name="caster_front_right_1_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.00634599 0.00005347 -0.02037519"/>
      <mass value="0.05144761"/>
      <inertia ixx="0.00001513" ixy="-0.00000003" ixz="0.00000267" iyy="0.00001309" iyz="-0.00000005" izz="0.00001198"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://pmb2_description/meshes/wheels/caster_1.stl"/>
      </geometry>
      <material name="Grey"/>
    </visual>
    <!--collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <cylinder radius="${radius}" length="${width}"/>
        </geometry>
      </collision-->
  </link>
  <link name="caster_front_right_2_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.08855784"/>
      <inertia ixx="0.00001075" ixy="0" ixz="0" iyy="0.00001541" iyz="0" izz="0.00001075"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://pmb2_description/meshes/wheels/caster_2.stl"/>
      </geometry>
      <material name="DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <sphere radius="0.025"/>
      </geometry>
    </collision>
  </link>
  <joint name="caster_front_right_1_joint" type="continuous">
    <parent link="base_link"/>
    <child link="caster_front_right_1_link"/>
    <origin rpy="0 0 0" xyz="0.1695 -0.102 -0.0335"/>
    <axis xyz="0 0 1"/>
    <dynamics damping="0.005" friction="0.0"/>
  </joint>
  <joint name="caster_front_right_2_joint" type="continuous">
    <parent link="caster_front_right_1_link"/>
    <child link="caster_front_right_2_link"/>
    <origin rpy="-1.57079632679 0 0" xyz="-0.016 0.0000 -0.040"/>
    <axis xyz="0 0 1"/>
  </joint>
  <!-- Contact model for the caster wheel surface -->
  <gazebo reference="caster_front_right_1_link">
    <kp>100000000.0</kp>
    <kd>10.0</kd>
    <mu1>0.1</mu1>
    <mu2>0.1</mu2>
    <!--<fdir1>1 0 0</fdir1>-->
    <maxVel>10.0</maxVel>
    <minDepth>0.0005</minDepth>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <gazebo reference="caster_front_right_2_link">
    <kp>10000000.0</kp>
    <kd>10.0</kd>
    <mu1>0.1</mu1>
    <mu2>0.1</mu2>
    <!--<fdir1>1 0 0</fdir1>-->
    <maxVel>10.0</maxVel>
    <minDepth>0.0005</minDepth>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <gazebo reference="caster_{side}_1_joint">
    <implicitSpringDamper>0</implicitSpringDamper>
  </gazebo>
  <gazebo reference="caster_{side}_2_joint">
    <implicitSpringDamper>0</implicitSpringDamper>
  </gazebo>
  <link name="caster_front_left_1_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.00634599 0.00005347 -0.02037519"/>
      <mass value="0.05144761"/>
      <inertia ixx="0.00001513" ixy="-0.00000003" ixz="0.00000267" iyy="0.00001309" iyz="-0.00000005" izz="0.00001198"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://pmb2_description/meshes/wheels/caster_1.stl"/>
      </geometry>
      <material name="Grey"/>
    </visual>
    <!--collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <cylinder radius="${radius}" length="${width}"/>
        </geometry>
      </collision-->
  </link>
  <link name="caster_front_left_2_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.08855784"/>
      <inertia ixx="0.00001075" ixy="0" ixz="0" iyy="0.00001541" iyz="0" izz="0.00001075"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://pmb2_description/meshes/wheels/caster_2.stl"/>
      </geometry>
      <material name="DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <sphere radius="0.025"/>
      </geometry>
    </collision>
  </link>
  <joint name="caster_front_left_1_joint" type="continuous">
    <parent link="base_link"/>
    <child link="caster_front_left_1_link"/>
    <origin rpy="0 0 0" xyz="0.1695 0.102 -0.0335"/>
    <axis xyz="0 0 1"/>
    <dynamics damping="0.005" friction="0.0"/>
  </joint>
  <joint name="caster_front_left_2_joint" type="continuous">
    <parent link="caster_front_left_1_link"/>
    <child link="caster_front_left_2_link"/>
    <origin rpy="-1.57079632679 0 0" xyz="-0.016 0.0000 -0.040"/>
    <axis xyz="0 0 1"/>
  </joint>
  <!-- Contact model for the caster wheel surface -->
  <gazebo reference="caster_front_left_1_link">
    <kp>100000000.0</kp>
    <kd>10.0</kd>
    <mu1>0.1</mu1>
    <mu2>0.1</mu2>
    <!--<fdir1>1 0 0</fdir1>-->
    <maxVel>10.0</maxVel>
    <minDepth>0.0005</minDepth>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <gazebo reference="caster_front_left_2_link">
    <kp>10000000.0</kp>
    <kd>10.0</kd>
    <mu1>0.1</mu1>
    <mu2>0.1</mu2>
    <!--<fdir1>1 0 0</fdir1>-->
    <maxVel>10.0</maxVel>
    <minDepth>0.0005</minDepth>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <gazebo reference="caster_{side}_1_joint">
    <implicitSpringDamper>0</implicitSpringDamper>
  </gazebo>
  <gazebo reference="caster_{side}_2_joint">
    <implicitSpringDamper>0</implicitSpringDamper>
  </gazebo>
  <link name="caster_back_right_1_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.00634599 0.00005347 -0.02037519"/>
      <mass value="0.05144761"/>
      <inertia ixx="0.00001513" ixy="-0.00000003" ixz="0.00000267" iyy="0.00001309" iyz="-0.00000005" izz="0.00001198"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://pmb2_description/meshes/wheels/caster_1.stl"/>
      </geometry>
      <material name="Grey"/>
    </visual>
    <!--collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <cylinder radius="${radius}" length="${width}"/>
        </geometry>
      </collision-->
  </link>
  <link name="caster_back_right_2_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.08855784"/>
      <inertia ixx="0.00001075" ixy="0" ixz="0" iyy="0.00001541" iyz="0" izz="0.00001075"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://pmb2_description/meshes/wheels/caster_2.stl"/>
      </geometry>
      <material name="DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <sphere radius="0.025"/>
      </geometry>
    </collision>
  </link>
  <joint name="caster_back_right_1_joint" type="continuous">
    <parent link="base_link"/>
    <child link="caster_back_right_1_link"/>
    <origin rpy="0 0 0" xyz="-0.1735 -0.102 -0.0335"/>
    <axis xyz="0 0 1"/>
    <dynamics damping="0.005" friction="0.0"/>
  </joint>
  <joint name="caster_back_right_2_joint" type="continuous">
    <parent link="caster_back_right_1_link"/>
    <child link="caster_back_right_2_link"/>
    <origin rpy="-1.57079632679 0 0" xyz="-0.016 0.0000 -0.040"/>
    <axis xyz="0 0 1"/>
  </joint>
  <!-- Contact model for the caster wheel surface -->
  <gazebo reference="caster_back_right_1_link">
    <kp>100000000.0</kp>
    <kd>10.0</kd>
    <mu1>0.1</mu1>
    <mu2>0.1</mu2>
    <!--<fdir1>1 0 0</fdir1>-->
    <maxVel>10.0</maxVel>
    <minDepth>0.0005</minDepth>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <gazebo reference="caster_back_right_2_link">
    <kp>10000000.0</kp>
    <kd>10.0</kd>
    <mu1>0.1</mu1>
    <mu2>0.1</mu2>
    <!--<fdir1>1 0 0</fdir1>-->
    <maxVel>10.0</maxVel>
    <minDepth>0.0005</minDepth>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <gazebo reference="caster_{side}_1_joint">
    <implicitSpringDamper>0</implicitSpringDamper>
  </gazebo>
  <gazebo reference="caster_{side}_2_joint">
    <implicitSpringDamper>0</implicitSpringDamper>
  </gazebo>
  <link name="caster_back_left_1_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.00634599 0.00005347 -0.02037519"/>
      <mass value="0.05144761"/>
      <inertia ixx="0.00001513" ixy="-0.00000003" ixz="0.00000267" iyy="0.00001309" iyz="-0.00000005" izz="0.00001198"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://pmb2_description/meshes/wheels/caster_1.stl"/>
      </geometry>
      <material name="Grey"/>
    </visual>
    <!--collision>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <cylinder radius="${radius}" length="${width}"/>
        </geometry>
      </collision-->
  </link>
  <link name="caster_back_left_2_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.08855784"/>
      <inertia ixx="0.00001075" ixy="0" ixz="0" iyy="0.00001541" iyz="0" izz="0.00001075"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://pmb2_description/meshes/wheels/caster_2.stl"/>
      </geometry>
      <material name="DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <sphere radius="0.025"/>
      </geometry>
    </collision>
  </link>
  <joint name="caster_back_left_1_joint" type="continuous">
    <parent link="base_link"/>
    <child link="caster_back_left_1_link"/>
    <origin rpy="0 0 0" xyz="-0.1735 0.102 -0.0335"/>
    <axis xyz="0 0 1"/>
    <dynamics damping="0.005" friction="0.0"/>
  </joint>
  <joint name="caster_back_left_2_joint" type="continuous">
    <parent link="caster_back_left_1_link"/>
    <child link="caster_back_left_2_link"/>
    <origin rpy="-1.57079632679 0 0" xyz="-0.016 0.0000 -0.040"/>
    <axis xyz="0 0 1"/>
  </joint>
  <!-- Contact model for the caster wheel surface -->
  <gazebo reference="caster_back_left_1_link">
    <kp>100000000.0</kp>
    <kd>10.0</kd>
    <mu1>0.1</mu1>
    <mu2>0.1</mu2>
    <!--<fdir1>1 0 0</fdir1>-->
    <maxVel>10.0</maxVel>
    <minDepth>0.0005</minDepth>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <gazebo reference="caster_back_left_2_link">
    <kp>10000000.0</kp>
    <kd>10.0</kd>
    <mu1>0.1</mu1>
    <mu2>0.1</mu2>
    <!--<fdir1>1 0 0</fdir1>-->
    <maxVel>10.0</maxVel>
    <minDepth>0.0005</minDepth>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <gazebo reference="caster_{side}_1_joint">
    <implicitSpringDamper>0</implicitSpringDamper>
  </gazebo>
  <gazebo reference="caster_{side}_2_joint">
    <implicitSpringDamper>0</implicitSpringDamper>
  </gazebo>
  <!-- Bottom fixed part of column -->
  <link name="torso_fixed_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="0.02435 -0.00464 0.15900"/>
      <mass value="14.01360"/>
      <inertia ixx="0.38341100000" ixy="0.00036400000" ixz="-0.02438900000" iyy="0.40577300000" iyz="-0.00078200000" izz="0.07699800000"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tiago_dual_description/meshes/torso/torso_fix.stl" scale="1 1 1"/>
      </geometry>
      <material name="LightGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.085 0.0 0.02"/>
      <geometry>
        <box size="0.4 0.25 0.04"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.05"/>
      <geometry>
        <box size="0.25 0.25 0.1"/>
      </geometry>
    </collision>
  </link>
  <joint name="torso_fixed_joint" type="fixed">
    <origin rpy="0 0 0" xyz="-0.062 0.0 0.193"/>
    <parent link="base_link"/>
    <child link="torso_fixed_link"/>
    <axis xyz="0 0 0"/>
  </joint>
  <!-- Middle extending  -->
  <link name="torso_fixed_column_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.01"/>
      <inertia ixx="0.000001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.000001"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0.275"/>
      <geometry>
        <box size="0.18 0.2 0.55"/>
      </geometry>
      <material name="DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0.275"/>
      <geometry>
        <box size="0.23 0.23 0.55"/>
      </geometry>
      <material name="DarkGrey"/>
    </collision>
  </link>
  <gazebo reference="torso_fixed_column_link">
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <joint name="torso_fixed_column_joint" type="fixed">
    <origin rpy="0 0 0" xyz="-0.062 0 0.195"/>
    <parent link="base_link"/>
    <child link="torso_fixed_column_link"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="torso_lift_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="0.04361 -0.00116 -0.17334"/>
      <mass value="9.594213"/>
      <inertia ixx="0.326395" ixy="-0.000844" ixz="-0.018905" iyy="0.235332" iyz="0.009042" izz="0.176168"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tiago_dual_description/meshes/torso/torso_lift_dual_arm.stl" scale="1 1 1"/>
      </geometry>
      <material name="White"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tiago_description/meshes/torso/torso_lift_collision_core.dae" scale="1 1 1"/>
      </geometry>
      <material name="LightGrey"/>
    </collision>
    <!-- left shoulder collision-->
    <collision>
      <origin rpy="0 0 0" xyz="-0.131 0.17 -0.02556"/>
      <geometry>
        <mesh filename="package://tiago_description/meshes/torso/torso_lift_collision_shoulder.dae" scale="1 1 1"/>
      </geometry>
      <material name="LightGrey"/>
    </collision>
    <!-- right shoulder collision-->
    <collision>
      <origin rpy="0 0 0" xyz="-0.131 -0.19 -0.02556"/>
      <geometry>
        <mesh filename="package://tiago_description/meshes/torso/torso_lift_collision_shoulder.dae" scale="1 1 1"/>
      </geometry>
      <material name="LightGrey"/>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tiago_description/meshes/torso/torso_lift_collision_neck.dae" scale="1 1 1"/>
      </geometry>
      <material name="LightGrey"/>
    </collision>
  </link>
  <joint name="torso_lift_joint" type="prismatic">
    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.597"/>
    <parent link="torso_fixed_link"/>
    <child link="torso_lift_link"/>
    <axis xyz="0 0 1"/>
    <limit effort="2000" lower="0" upper="0.35" velocity="0.07"/>
    <calibration rising="0.0"/>
    <dynamics damping="1000"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="0.0" soft_upper_limit="0.35"/>
  </joint>
  <transmission name="torso_lift_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="torso_lift_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="torso_lift_joint">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
  </transmission>
  <link name="head_1_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="-0.00508 0.00237 0.07229"/>
      <mass value="0.62220"/>
      <inertia ixx="0.00119200000" ixy="-0.00000700000" ixz="0.00009600000" iyy="0.00140200000" iyz="0.00000400000" izz="0.00088900000"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tiago_description/meshes/head/head_1.stl" scale="1 1 1"/>
      </geometry>
      <material name="White"/>
    </visual>
    <collision>
      <origin rpy="0 0 0.0" xyz="0.0 0 0"/>
      <geometry>
        <mesh filename="package://tiago_description/meshes/head/head_1_collision.dae" scale="1 1 1"/>
      </geometry>
      <material name="White"/>
    </collision>
  </link>
  <joint name="head_1_joint" type="revolute">
    <parent link="torso_lift_link"/>
    <child link="head_1_link"/>
    <origin rpy="0 0 0" xyz="0.182 0.0 0.0"/>
    <axis xyz="0 0 1"/>
    <limit effort="5.197" lower="-1.308996939" upper="1.308996939" velocity="3.0"/>
    <dynamics damping="0.5" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-1.238996939" soft_upper_limit="1.238996939"/>
  </joint>
  <gazebo reference="head_1_link">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="head_1_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <transmission name="head_1_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="head_1_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name="head_1_joint">
      <offset>0.0</offset>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
  </transmission>
  <link name="head_2_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="0.04700 0.05542 0.00022"/>
      <mass value="0.66123"/>
      <inertia ixx="0.00462000000" ixy="0.00068900000" ixz="0.00000600000" iyy="0.00486100000" iyz="0.00000600000" izz="0.00313200000"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tiago_description/meshes/head/head_2.stl" scale="1 1 1"/>
      </geometry>
      <material name="White"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tiago_description/meshes/head/head_2_collision.dae" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <joint name="head_2_joint" type="revolute">
    <parent link="head_1_link"/>
    <child link="head_2_link"/>
    <origin rpy="1.57079632679 0 0" xyz="0.005 0.0 0.098"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.77" lower="-1.0471975512" upper="0.785398163397" velocity="3.0"/>
    <dynamics damping="0.5" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-0.977197551197" soft_upper_limit="0.715398163397"/>
  </joint>
  <transmission name="head_2_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="head_2_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name="head_2_joint">
      <offset>0.0</offset>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
  </transmission>
  <!-- frames in the center of the camera -->
  <joint name="xtion_joint" type="fixed">
    <origin rpy="-1.57079632679 0.0 0.0" xyz="0.0908 0.08 0.0"/>
    <parent link="head_2_link"/>
    <child link="xtion_link"/>
  </joint>
  <link name="xtion_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.00936000000 -0.00003000000 -0.00273000000"/>
      <mass value="0.21970000000"/>
      <inertia ixx="0.00000429247" ixy="0.00000000000" ixz="0.00000002565" iyy="0.00000008027" iyz="0.00000000000" izz="0.00000427339"/>
    </inertial>
    <!--
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0"/>
        <geometry>
          <mesh filename="package://tiago_description/meshes/sensors/xtion_pro_live/xtion_pro_live.dae" />
        </geometry>
        <material name="Grey">
          <color rgba="0.5 0.5 0.5 1"/>
        </material>
      </visual>
      <collision>
        <origin xyz="-0.01 0.0025 0" rpy="0 0 0"/>
        <geometry>
          <box size="0.04 0.185 0.03"/>
        </geometry>
      </collision>
      -->
  </link>
  <joint name="xtion_optical_joint" type="fixed">
    <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
    <parent link="xtion_link"/>
    <child link="xtion_optical_frame"/>
  </joint>
  <link name="xtion_optical_frame">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.001"/>
      <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
    </inertial>
  </link>
  <!-- frame of camera origin provided by Orbbec, need this intermediate frame for backwards compatibility with xtion_pro_live
         to reuse same camera origin -->
  <joint name="xtion_orbbec_aux_joint" type="fixed">
    <!-- This is the real origin as in the robot solidworks, it must match: 0.1015 0.095 0.008 -->
    <!-- Assuming we have 0.0908 0.08 0.0 from default camera_position, and rotated -90 deg on x.-->
    <!-- Pending confirmation from Orbbec about the position of the frames, going with old defaults-->
    <!-- <origin xyz="0.0107 0.008 0.015" rpy="0 0 0" />-->
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="xtion_link"/>
    <child link="xtion_orbbec_aux_joint_frame"/>
  </joint>
  <link name="xtion_orbbec_aux_joint_frame">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.001"/>
      <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
    </inertial>
  </link>
  <!-- frames of the depth sensor -->
  <joint name="xtion_depth_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.0 0.025 0.0"/>
    <parent link="xtion_rgb_frame"/>
    <child link="xtion_depth_frame"/>
  </joint>
  <link name="xtion_depth_frame">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.001"/>
      <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
    </inertial>
  </link>
  <joint name="xtion_depth_optical_joint" type="fixed">
    <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
    <parent link="xtion_depth_frame"/>
    <child link="xtion_depth_optical_frame"/>
  </joint>
  <link name="xtion_depth_optical_frame">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.001"/>
      <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
    </inertial>
  </link>
  <!-- frames of the rgb sensor -->
  <joint name="xtion_rgb_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.0 0.022 0.0"/>
    <parent link="xtion_orbbec_aux_joint_frame"/>
    <child link="xtion_rgb_frame"/>
  </joint>
  <link name="xtion_rgb_frame">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.001"/>
      <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
    </inertial>
  </link>
  <joint name="xtion_rgb_optical_joint" type="fixed">
    <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
    <parent link="xtion_rgb_frame"/>
    <child link="xtion_rgb_optical_frame"/>
  </joint>
  <link name="xtion_rgb_optical_frame">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.001"/>
      <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
    </inertial>
  </link>
  <gazebo reference="xtion_rgb_frame">
    <sensor name="xtion_frame_sensor" type="depth">
      <always_on>true</always_on>
      <update_rate>30.0</update_rate>
      <camera>
        <horizontal_fov>1.09955742876</horizontal_fov>
        <image>
          <format>B8G8R8</format>
          <width>640</width>
          <height>480</height>
        </image>
        <clip>
          <near>0.3</near>
          <far>8.0</far>
        </clip>
      </camera>
      <plugin filename="libgazebo_ros_openni_kinect.so" name="xtion_frame_controller">
        <alwaysOn>true</alwaysOn>
        <updateRate>30.0</updateRate>
        <cameraName>xtion</cameraName>
        <imageTopicName>rgb/image_raw</imageTopicName>
        <cameraInfoTopicName>rgb/camera_info</cameraInfoTopicName>
        <depthImageTopicName>depth_registered/image_raw</depthImageTopicName>
        <depthImageCameraInfoTopicName>depth_registered/camera_info</depthImageCameraInfoTopicName>
        <pointCloudTopicName>depth_registered/points</pointCloudTopicName>
        <frameName>xtion_rgb_optical_frame</frameName>
        <pointCloudCutoff>0.5</pointCloudCutoff>
        <pointCloudCutoffMax>8.0</pointCloudCutoffMax>
        <distortionK1>0.00000001</distortionK1>
        <distortionK2>0.00000001</distortionK2>
        <distortionK3>0.00000001</distortionK3>
        <distortionT1>0.00000001</distortionT1>
        <distortionT2>0.00000001</distortionT2>
      </plugin>
    </sensor>
  </gazebo>
  <!--************************-->
  <!--        SHOULDER        -->
  <!--************************-->
  <link name="arm_left_1_link">
    <inertial>
      <!--<origin xyz="0.061191 ${reflect * -0.022397} -0.012835" rpy="0.00000 0.00000 0.00000"/>-->
      <origin rpy="0.00000 0.00000 0.00000" xyz="0.061191 -0.022397 0.012835"/>
      <mass value="1.563428"/>
      <inertia ixx="0.002471" ixy="0.001809" ixz="-0.001202" iyy="0.006132" iyz="-0.000494" izz="0.006704"/>
    </inertial>
    <!-- M90 module -->
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tiago_description/meshes/arm/arm_1.stl" scale="1 -1 -1"/>
      </geometry>
      <material name="White"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tiago_description/meshes/arm/arm_1_collision.dae" scale="1 -1 -1"/>
      </geometry>
    </collision>
  </link>
  <joint name="arm_left_1_joint" type="revolute">
    <parent link="torso_lift_link"/>
    <child link="arm_left_1_link"/>
    <origin rpy="3.14159265359 0 1.57079632679" xyz="0.02556 0.19 -0.171"/>
    <axis xyz="0 0 1"/>
    <limit effort="43.0" lower="-1.1780972451" upper="1.57079632679" velocity="1.95"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-1.1080972451" soft_upper_limit="1.50079632679"/>
  </joint>
  <link name="arm_left_2_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="0.030432 0.000229 0.005942"/>
      <mass value="1.800397"/>
      <inertia ixx="0.001809" ixy="-1.5e-05" ixz="-0.000254" iyy="0.004333" iyz="-1.4e-05" izz="0.004341"/>
    </inertial>
    <visual>
      <!-- <origin xyz="0 0 0" rpy="${180 * deg_to_rad} ${0 * deg_to_rad} ${-90 * deg_to_rad}" /> -->
      <origin rpy="0.0 0.0 0.0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tiago_description/meshes/arm/arm_2.stl" scale="1 1 -1"/>
      </geometry>
      <material name="DarkGrey"/>
    </visual>
    <collision>
      <!-- <origin xyz="0 0 0" rpy="${180 * deg_to_rad} ${0 * deg_to_rad} ${-90 * deg_to_rad}" /> -->
      <origin rpy="0.0 0.0 0.0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tiago_description/meshes/arm/arm_2_collision.dae" scale="1 1 -1"/>
      </geometry>
    </collision>
  </link>
  <joint name="arm_left_2_joint" type="revolute">
    <parent link="arm_left_1_link"/>
    <child link="arm_left_2_link"/>
    <origin rpy="1.57079632679 0.0 0.0" xyz="0.125 -0.0195 0.031"/>
    <axis xyz="0 0 1"/>
    <limit effort="43.0" lower="-1.1780972451" upper="1.57079632679" velocity="1.95"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-1.1080972451" soft_upper_limit="1.50079632679"/>
  </joint>
  <link name="arm_left_3_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="0.007418 -0.004361 0.134194"/>
      <mass value="1.8"/>
      <inertia ixx="0.019541" ixy="-0.000105" ixz="-0.001717" iyy="0.019936" iyz="-0.000852" izz="0.002864"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tiago_description/meshes/arm/arm_3.stl" scale="1 1 -1"/>
      </geometry>
      <material name="DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tiago_description/meshes/arm/arm_3_collision.dae" scale="1 1 -1"/>
      </geometry>
    </collision>
  </link>
  <joint name="arm_left_3_joint" type="revolute">
    <parent link="arm_left_2_link"/>
    <child link="arm_left_3_link"/>
    <origin rpy="-1.57079632679 3.14159265359 -1.57079632679" xyz="0.0895 0.0 -0.0015"/>
    <axis xyz="0 0 1"/>
    <limit effort="26" lower="-0.785398163397" upper="3.92699081699" velocity="2.35"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-0.715398163397" soft_upper_limit="3.85699081699"/>
  </joint>
  <!--************************-->
  <!--        ELBOW           -->
  <!--************************-->
  <link name="arm_left_4_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="0.095658 -0.014666 -0.018133"/>
      <mass value="1.432697"/>
      <inertia ixx="0.002040" ixy="-0.000725" ixz="0.00115" iyy="0.008424" iyz="-0.000204" izz="0.008686"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tiago_description/meshes/arm/arm_4.stl" scale="-1 -1 -1"/>
      </geometry>
      <material name="DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tiago_description/meshes/arm/arm_4_collision.dae" scale="-1 -1 -1"/>
      </geometry>
    </collision>
  </link>
  <joint name="arm_left_4_joint" type="revolute">
    <parent link="arm_left_3_link"/>
    <child link="arm_left_4_link"/>
    <origin rpy="1.57079632679 -1.57079632679 0.0" xyz="-0.02 -0.027 0.222"/>
    <axis xyz="0 0 1"/>
    <limit effort="26" lower="-0.392699081699" upper="2.35619449019" velocity="2.35"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-0.322699081699" soft_upper_limit="2.28619449019"/>
  </joint>
  <gazebo reference="arm_left_1_link">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <gazebo reference="arm_left_2_link">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <gazebo reference="arm_left_3_link">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <gazebo reference="arm_left_4_link">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <gazebo reference="arm_left_1_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="arm_left_2_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="arm_left_3_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="arm_left_4_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!--************************-->
  <!--        WRIST           -->
  <!--************************-->
  <!--************************-->
  <!--        WRIST           -->
  <!--************************-->
  <link name="arm_left_5_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="0.001078 -0.000151 0.077173"/>
      <mass value="1.655819"/>
      <inertia ixx="0.00613800000" ixy="-0.00000400000" ixz="0.00015600000" iyy="0.00564000000" iyz="0.00002200000" izz="0.00179700000"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tiago_description/meshes/arm/arm_5-wrist-2010.stl" scale="-1 -1 -1"/>
      </geometry>
      <material name="LightGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tiago_description/meshes/arm/arm_5_collision-wrist-2010.dae" scale="-1 -1 -1"/>
      </geometry>
    </collision>
  </link>
  <joint name="arm_left_5_joint" type="revolute">
    <parent link="arm_left_4_link"/>
    <child link="arm_left_5_link"/>
    <origin rpy="1.57079632679 -1.57079632679 -1.57079632679" xyz="0.162 -0.02 -0.027"/>
    <axis xyz="0 0 1"/>
    <limit effort="3" lower="-2.09439510239" upper="2.09439510239" velocity="1.95"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-2.07439510239" soft_upper_limit="2.07439510239"/>
  </joint>
  <link name="arm_left_6_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="-0.000153 -0.003122 0.000183"/>
      <mass value="0.370063"/>
      <inertia ixx="0.00021500000" ixy="0.00000000000" ixz="0.00000000000" iyy="0.00017000000" iyz="0.00000200000" izz="0.00017400000"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tiago_description/meshes/arm/arm_6-wrist-2010.stl" scale="-1 1 1"/>
      </geometry>
      <material name="LightGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tiago_description/meshes/arm/arm_6_collision-wrist-2010.dae" scale="-1 1 1"/>
      </geometry>
    </collision>
  </link>
  <joint name="arm_left_6_joint" type="revolute">
    <parent link="arm_left_5_link"/>
    <child link="arm_left_6_link"/>
    <origin rpy="0.0 -1.57079632679 -1.57079632679" xyz="0 0 -0.15"/>
    <axis xyz="0 0 1"/>
    <limit effort="6.6" lower="-1.41371669412" upper="1.41371669412" velocity="1.76"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-1.39371669412" soft_upper_limit="1.39371669412"/>
  </joint>
  <link name="arm_left_7_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="0.000010 -0.000003 -0.007774"/>
      <mass value="0.234252"/>
      <inertia ixx="0.00014200000" ixy="0.00000000000" ixz="0.00000000000" iyy="0.00011300000" iyz="0.00000000000" izz="0.00005200000"/>
    </inertial>
  </link>
  <joint name="arm_left_7_joint" type="revolute">
    <parent link="arm_left_6_link"/>
    <child link="arm_left_7_link"/>
    <origin rpy="1.57079632679 0.0 1.57079632679" xyz="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="6.6" lower="-2.09439510239" upper="2.09439510239" velocity="1.76"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-2.07439510239" soft_upper_limit="2.07439510239"/>
  </joint>
  <gazebo reference="arm_left_5_link">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="arm_left_6_link">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="arm_left_7_link">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="arm_left_5_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="arm_left_6_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="arm_left_7_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
    <provideFeedback>1</provideFeedback>
  </gazebo>
  <transmission name="arm_left_5_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="arm_left_5_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="arm_left_5_joint">
      <offset>0.0</offset>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
  </transmission>
  <transmission name="arm_left_wrist_trans">
    <type>transmission_interface/DifferentialTransmission</type>
    <actuator name="arm_left_6_motor">
      <role>actuator1</role>
      <mechanicalReduction>-1.0</mechanicalReduction>
    </actuator>
    <actuator name="arm_left_7_motor">
      <role>actuator2</role>
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="arm_left_6_joint">
      <role>joint1</role>
      <offset>0.0</offset>
      <mechanicalReduction>1.0</mechanicalReduction>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <ignoreTransmissionAbsoluteEncoder>1</ignoreTransmissionAbsoluteEncoder>
    </joint>
    <joint name="arm_left_7_joint">
      <role>joint2</role>
      <offset>0.0</offset>
      <mechanicalReduction>1</mechanicalReduction>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <ignoreTransmissionAbsoluteEncoder>1</ignoreTransmissionAbsoluteEncoder>
    </joint>
  </transmission>
  <!--***********************-->
  <!--        TOOL           -->
  <!--***********************-->
  <link name="arm_left_tool_link">
    <inertial>
      <mass value="0.1"/>
      <inertia ixx="8.33E-07" ixy="0.0" ixz="0.0" iyy="8.33E-07" iyz="0.0" izz="1.25E-06"/>
    </inertial>
    <visual>
      <origin rpy="0 1.57079632679 0" xyz="0.001 0 0"/>
      <geometry>
        <cylinder length="0.005" radius="0.005"/>
      </geometry>
      <material name="LightGrey"/>
    </visual>
    <collision>
      <origin rpy="0 1.57079632679 0" xyz="0.001 0 0"/>
      <geometry>
        <cylinder length="0.005" radius="0.005"/>
      </geometry>
    </collision>
  </link>
  <joint name="arm_left_tool_joint" type="fixed">
    <parent link="arm_left_7_link"/>
    <child link="arm_left_tool_link"/>
    <origin rpy="1.57079632679 1.57079632679 3.14159265359" xyz="0 0 -0.046"/>
  </joint>
  <transmission name="arm_left_1_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="arm_left_1_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="arm_left_1_joint">
      <offset>0.0</offset>
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
  </transmission>
  <transmission name="arm_left_2_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="arm_left_2_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="arm_left_2_joint">
      <offset>0.0</offset>
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
  </transmission>
  <transmission name="arm_left_3_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="arm_left_3_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="arm_left_3_joint">
      <offset>0.0</offset>
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
  </transmission>
  <transmission name="arm_left_4_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="arm_left_4_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="arm_left_4_joint">
      <offset>0.0</offset>
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
  </transmission>
  <!--************************-->
  <!--        ft sensor       -->
  <!--************************-->
  <link name="wrist_left_ft_link">
    <inertial>
      <mass value="0.1"/>
      <inertia ixx="1.6E-05" ixy="-2.0E-06" ixz="0.0" iyy="1.6E-05" iyz="0.0" izz="2.7E-05"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.0157" radius="0.0225"/>
      </geometry>
      <material name="LightGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.0157" radius="0.0225"/>
      </geometry>
    </collision>
  </link>
  <joint name="wrist_left_ft_joint" type="fixed">
    <parent link="arm_left_tool_link"/>
    <child link="wrist_left_ft_link"/>
    <origin rpy="1.57079632679 0 1.57079632679" xyz="0.00785 0 0"/>
  </joint>
  <!--***********************-->
  <!--       FT TOOL         -->
  <!--***********************-->
  <link name="wrist_left_ft_tool_link">
    <inertial>
      <mass value="0.1"/>
      <inertia ixx="1.64E-05" ixy="0.0" ixz="0.0" iyy="1.64E-05" iyz="0.0" izz="3.125E-05"/>
    </inertial>
    <visual>
      <origin rpy="0 1.57079632679 0" xyz="0.0 0 0"/>
      <geometry>
        <cylinder length="0.00975" radius="0.025"/>
      </geometry>
      <material name="FlatBlack"/>
    </visual>
    <collision>
      <origin rpy="0 1.57079632679 0" xyz="0.0 0 0"/>
      <geometry>
        <cylinder length="0.00975" radius="0.025"/>
      </geometry>
    </collision>
  </link>
  <joint name="wrist_left_tool_joint" type="fixed">
    <parent link="wrist_left_ft_link"/>
    <child link="wrist_left_ft_tool_link"/>
    <origin rpy="-1.57079632679 -1.57079632679 0" xyz="0 0 0.012725"/>
  </joint>
  <!-- Tool Link -->
  <link name="hand_left_tool_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.0001"/>
      <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001"/>
    </inertial>
  </link>
  <joint name="hand_left_tool_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="wrist_left_ft_tool_link"/>
    <child link="hand_left_tool_link"/>
    <axis xyz="0 0 0"/>
  </joint>
  <joint name="hand_left_palm_joint" type="fixed">
    <parent link="wrist_left_ft_tool_link"/>
    <child link="hand_left_palm_link"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <link name="hand_left_palm_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0641 -0.0022 0.0021"/>
      <!-- NOTE: Less than total palm mass of 0.598kg because actuator masses are specified separately -->
      <mass value="0.4"/>
      <inertia ixx="0.000305100" ixy="0.000005037" ixz="0.000015302" iyy="0.000811920" iyz="0.000007622" izz="0.000655851"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/palm.stl" scale="1 1 -1"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/palm_collision.stl" scale="1 1 -1"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_left_palm_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_left_thumb_joint" type="revolute">
    <parent link="hand_left_palm_link"/>
    <child link="hand_left_thumb_link"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="-2.61799387799" upper="6.28318530718" velocity="3.35"/>
    <dynamics damping="0.5" friction="0.0"/>
    <!-- TODO: Tune -->
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-2.61799387799" soft_upper_limit="6.28318530718"/>
  </joint>
  <link name="hand_left_thumb_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.05"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!--To stabilize simulation-->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <sphere radius="0.001"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <sphere radius="0.001"/>
      </geometry>
    </collision>
  </link>
  <transmission name="hand_left_thumb_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="hand_left_thumb_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="hand_left_thumb_joint">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
  </transmission>
  <joint name="hand_left_index_joint" type="revolute">
    <parent link="hand_left_palm_link"/>
    <child link="hand_left_index_link"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="-3.05432619099" upper="6.80678408278" velocity="3.35"/>
    <dynamics damping="0.5" friction="0.0"/>
    <!-- TODO: Tune -->
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-3.05432619099" soft_upper_limit="6.80678408278"/>
  </joint>
  <link name="hand_left_index_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.05"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!--To stabilize simulation-->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <sphere radius="0.001"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <sphere radius="0.001"/>
      </geometry>
    </collision>
  </link>
  <transmission name="hand_left_index_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="hand_left_index_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="hand_left_index_joint">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
  </transmission>
  <joint name="hand_left_mrl_joint" type="revolute">
    <parent link="hand_left_palm_link"/>
    <child link="hand_left_mrl_link"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="-2.53072741539" upper="9.25024503557" velocity="3.56"/>
    <dynamics damping="0.5" friction="0.0"/>
    <!-- TODO: Tune -->
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-2.53072741539" soft_upper_limit="9.25024503557"/>
  </joint>
  <link name="hand_left_mrl_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.05"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!--To stabilize simulation-->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <sphere radius="0.001"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <sphere radius="0.001"/>
      </geometry>
    </collision>
  </link>
  <transmission name="hand_left_mrl_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="hand_left_mrl_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="hand_left_mrl_joint">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
  </transmission>
  <joint name="hand_left_thumb_abd_joint" type="revolute">
    <parent link="hand_left_palm_link"/>
    <child link="hand_left_thumb_abd_link"/>
    <origin rpy="0 1.1344640138 0" xyz="0.0901 0.0134 -0.038"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="1.57079632679" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_left_thumb_abd_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.0132 0.0037 -0.0017"/>
      <mass value="0.014"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/thumb_abd_collision.stl" scale="1 1 -1"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/thumb_abd_collision.stl" scale="1 -1 1"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_left_thumb_abd_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_left_thumb_virtual_1_joint" type="revolute">
    <parent link="hand_left_thumb_abd_link"/>
    <child link="hand_left_thumb_virtual_1_link"/>
    <origin rpy="1.14423785761 0 0" xyz="0.0225 0.003 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_left_thumb_virtual_1_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <mass value="0.01"/>
      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_left_thumb_virtual_1_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_left_thumb_flex_1_joint" type="revolute">
    <parent link="hand_left_thumb_virtual_1_link"/>
    <child link="hand_left_thumb_flex_1_link"/>
    <origin rpy="0 0 0" xyz="0.013 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_left_thumb_flex_1_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
      <mass value="0.009"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_left_thumb_flex_1_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_left_thumb_virtual_2_joint" type="revolute">
    <parent link="hand_left_thumb_flex_1_link"/>
    <child link="hand_left_thumb_virtual_2_link"/>
    <origin rpy="0 0 0" xyz="0.009 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_left_thumb_virtual_2_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <mass value="0.01"/>
      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_left_thumb_virtual_2_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_left_thumb_flex_2_joint" type="revolute">
    <parent link="hand_left_thumb_virtual_2_link"/>
    <child link="hand_left_thumb_flex_2_link"/>
    <origin rpy="0 0 0" xyz="0.013 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_left_thumb_flex_2_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0096 0.003 0.0"/>
      <mass value="0.015"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_left_thumb_flex_2_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_left_index_abd_joint" type="revolute">
    <parent link="hand_left_palm_link"/>
    <child link="hand_left_index_abd_link"/>
    <origin rpy="1.57079632679 0.523598775598 0" xyz="0.1265 0.0042 -0.0265"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="-0.523598775598" upper="0.523598775598" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_left_index_abd_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.0018 0.0 0.0028"/>
      <mass value="0.018"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_abd_collision.stl" scale="1 1 -1"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_abd_collision.stl" scale="1 1 -1"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_left_index_abd_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_left_index_virtual_1_joint" type="revolute">
    <parent link="hand_left_index_abd_link"/>
    <child link="hand_left_index_virtual_1_link"/>
    <origin rpy="-1.57079632679 0 0" xyz="0.009 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_left_index_virtual_1_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <mass value="0.01"/>
      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_left_index_virtual_1_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_left_index_flex_1_joint" type="revolute">
    <parent link="hand_left_index_virtual_1_link"/>
    <child link="hand_left_index_flex_1_link"/>
    <origin rpy="0 0 0" xyz="0.013 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_left_index_flex_1_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
      <mass value="0.009"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_left_index_flex_1_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_left_index_virtual_2_joint" type="revolute">
    <parent link="hand_left_index_flex_1_link"/>
    <child link="hand_left_index_virtual_2_link"/>
    <origin rpy="0 0 0" xyz="0.009 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_left_index_virtual_2_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <mass value="0.01"/>
      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_left_index_virtual_2_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_left_index_flex_2_joint" type="revolute">
    <parent link="hand_left_index_virtual_2_link"/>
    <child link="hand_left_index_flex_2_link"/>
    <origin rpy="0 0 0" xyz="0.013 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_left_index_flex_2_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
      <mass value="0.009"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_left_index_flex_2_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_left_index_virtual_3_joint" type="revolute">
    <parent link="hand_left_index_flex_2_link"/>
    <child link="hand_left_index_virtual_3_link"/>
    <origin rpy="0 0 0" xyz="0.009 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_left_index_virtual_3_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <mass value="0.01"/>
      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_left_index_virtual_3_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_left_index_flex_3_joint" type="revolute">
    <parent link="hand_left_index_virtual_3_link"/>
    <child link="hand_left_index_flex_3_link"/>
    <origin rpy="0 0 0" xyz="0.013 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_left_index_flex_3_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0096 0.003 0.0"/>
      <mass value="0.015"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_left_index_flex_3_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_left_middle_abd_joint" type="revolute">
    <parent link="hand_left_palm_link"/>
    <child link="hand_left_middle_abd_link"/>
    <origin rpy="1.57079632679 0 0" xyz="0.1335 0.0042 -0.007"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="-0.523598775598" upper="0.523598775598" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_left_middle_abd_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.0018 0.0 0.0028"/>
      <mass value="0.018"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_abd_collision.stl" scale="1 1 -1"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_abd_collision.stl" scale="1 1 -1"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_left_middle_abd_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_left_middle_virtual_1_joint" type="revolute">
    <parent link="hand_left_middle_abd_link"/>
    <child link="hand_left_middle_virtual_1_link"/>
    <origin rpy="-1.57079632679 0 0" xyz="0.009 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_left_middle_virtual_1_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <mass value="0.01"/>
      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_left_middle_virtual_1_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_left_middle_flex_1_joint" type="revolute">
    <parent link="hand_left_middle_virtual_1_link"/>
    <child link="hand_left_middle_flex_1_link"/>
    <origin rpy="0 0 0" xyz="0.013 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_left_middle_flex_1_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
      <mass value="0.009"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_left_middle_flex_1_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_left_middle_virtual_2_joint" type="revolute">
    <parent link="hand_left_middle_flex_1_link"/>
    <child link="hand_left_middle_virtual_2_link"/>
    <origin rpy="0 0 0" xyz="0.009 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_left_middle_virtual_2_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <mass value="0.01"/>
      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_left_middle_virtual_2_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_left_middle_flex_2_joint" type="revolute">
    <parent link="hand_left_middle_virtual_2_link"/>
    <child link="hand_left_middle_flex_2_link"/>
    <origin rpy="0 0 0" xyz="0.013 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_left_middle_flex_2_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
      <mass value="0.009"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_left_middle_flex_2_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_left_middle_virtual_3_joint" type="revolute">
    <parent link="hand_left_middle_flex_2_link"/>
    <child link="hand_left_middle_virtual_3_link"/>
    <origin rpy="0 0 0" xyz="0.009 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_left_middle_virtual_3_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <mass value="0.01"/>
      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_left_middle_virtual_3_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_left_middle_flex_3_joint" type="revolute">
    <parent link="hand_left_middle_virtual_3_link"/>
    <child link="hand_left_middle_flex_3_link"/>
    <origin rpy="0 0 0" xyz="0.013 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_left_middle_flex_3_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0096 0.003 0.0"/>
      <mass value="0.015"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_left_middle_flex_3_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_left_ring_abd_joint" type="revolute">
    <parent link="hand_left_palm_link"/>
    <child link="hand_left_ring_abd_link"/>
    <origin rpy="1.57079632679 -0.261799387799 0" xyz="0.1265 0.0042 0.0125"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="-0.523598775598" upper="0.523598775598" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_left_ring_abd_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.0018 0.0 0.0028"/>
      <mass value="0.018"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_abd_collision.stl" scale="1 1 -1"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_abd_collision.stl" scale="1 1 -1"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_left_ring_abd_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_left_ring_virtual_1_joint" type="revolute">
    <parent link="hand_left_ring_abd_link"/>
    <child link="hand_left_ring_virtual_1_link"/>
    <origin rpy="-1.57079632679 0 0" xyz="0.009 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_left_ring_virtual_1_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <mass value="0.01"/>
      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_left_ring_virtual_1_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_left_ring_flex_1_joint" type="revolute">
    <parent link="hand_left_ring_virtual_1_link"/>
    <child link="hand_left_ring_flex_1_link"/>
    <origin rpy="0 0 0" xyz="0.013 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_left_ring_flex_1_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
      <mass value="0.009"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_left_ring_flex_1_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_left_ring_virtual_2_joint" type="revolute">
    <parent link="hand_left_ring_flex_1_link"/>
    <child link="hand_left_ring_virtual_2_link"/>
    <origin rpy="0 0 0" xyz="0.009 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_left_ring_virtual_2_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <mass value="0.01"/>
      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_left_ring_virtual_2_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_left_ring_flex_2_joint" type="revolute">
    <parent link="hand_left_ring_virtual_2_link"/>
    <child link="hand_left_ring_flex_2_link"/>
    <origin rpy="0 0 0" xyz="0.013 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_left_ring_flex_2_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
      <mass value="0.009"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_left_ring_flex_2_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_left_ring_virtual_3_joint" type="revolute">
    <parent link="hand_left_ring_flex_2_link"/>
    <child link="hand_left_ring_virtual_3_link"/>
    <origin rpy="0 0 0" xyz="0.009 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_left_ring_virtual_3_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <mass value="0.01"/>
      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_left_ring_virtual_3_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_left_ring_flex_3_joint" type="revolute">
    <parent link="hand_left_ring_virtual_3_link"/>
    <child link="hand_left_ring_flex_3_link"/>
    <origin rpy="0 0 0" xyz="0.013 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_left_ring_flex_3_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0096 0.003 0.0"/>
      <mass value="0.015"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_left_ring_flex_3_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_left_little_abd_joint" type="revolute">
    <parent link="hand_left_palm_link"/>
    <child link="hand_left_little_abd_link"/>
    <origin rpy="1.57079632679 -0.523598775598 0" xyz="0.1165 0.0042 0.032"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="-0.523598775598" upper="0.523598775598" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_left_little_abd_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.0018 0.0 0.0028"/>
      <mass value="0.018"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_abd_collision.stl" scale="1 1 -1"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_abd_collision.stl" scale="1 1 -1"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_left_little_abd_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_left_little_virtual_1_joint" type="revolute">
    <parent link="hand_left_little_abd_link"/>
    <child link="hand_left_little_virtual_1_link"/>
    <origin rpy="-1.57079632679 0 0" xyz="0.009 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_left_little_virtual_1_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <mass value="0.01"/>
      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_left_little_virtual_1_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_left_little_flex_1_joint" type="revolute">
    <parent link="hand_left_little_virtual_1_link"/>
    <child link="hand_left_little_flex_1_link"/>
    <origin rpy="0 0 0" xyz="0.013 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_left_little_flex_1_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
      <mass value="0.009"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_left_little_flex_1_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_left_little_virtual_2_joint" type="revolute">
    <parent link="hand_left_little_flex_1_link"/>
    <child link="hand_left_little_virtual_2_link"/>
    <origin rpy="0 0 0" xyz="0.009 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_left_little_virtual_2_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <mass value="0.01"/>
      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_left_little_virtual_2_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_left_little_flex_2_joint" type="revolute">
    <parent link="hand_left_little_virtual_2_link"/>
    <child link="hand_left_little_flex_2_link"/>
    <origin rpy="0 0 0" xyz="0.013 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_left_little_flex_2_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
      <mass value="0.009"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_left_little_flex_2_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_left_little_virtual_3_joint" type="revolute">
    <parent link="hand_left_little_flex_2_link"/>
    <child link="hand_left_little_virtual_3_link"/>
    <origin rpy="0 0 0" xyz="0.009 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_left_little_virtual_3_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <mass value="0.01"/>
      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_left_little_virtual_3_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_left_little_flex_3_joint" type="revolute">
    <parent link="hand_left_little_virtual_3_link"/>
    <child link="hand_left_little_flex_3_link"/>
    <origin rpy="0 0 0" xyz="0.013 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_left_little_flex_3_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0096 0.003 0.0"/>
      <mass value="0.015"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_left_little_flex_3_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <!--Abductor-->
  <transmission name="hand_left_thumb_abd_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_left_thumb_abd_joint">
      <saturationLower>0.0</saturationLower>
      <saturationUpper>1.57079632679</saturationUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_left_thumb_motor">
      <mechanicalReduction>0.654498469498</mechanicalReduction>
    </actuator>
  </transmission>
  <!--First flexor-->
  <transmission name="hand_left_thumb_virtual_1_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_left_thumb_virtual_1_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>2.0</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_left_thumb_motor">
      <mechanicalReduction>0.162993578547</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="hand_left_thumb_flex_1_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_left_thumb_flex_1_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>2.0</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_left_thumb_motor">
      <mechanicalReduction>0.162993578547</mechanicalReduction>
    </actuator>
  </transmission>
  <!--Second flexor-->
  <transmission name="hand_left_thumb_virtual_2_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_left_thumb_virtual_2_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>2.0</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_left_thumb_motor">
      <mechanicalReduction>0.162993578547</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="hand_left_thumb_flex_2_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_left_thumb_flex_2_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>2.0</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_left_thumb_motor">
      <mechanicalReduction>0.162993578547</mechanicalReduction>
    </actuator>
  </transmission>
  <!--Abductor-->
  <transmission name="hand_left_index_abd_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_left_index_abd_joint">
      <saturationLower>0.0</saturationLower>
      <saturationUpper>0.523598775598</saturationUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_left_index_motor">
      <mechanicalReduction>0.49866550057</mechanicalReduction>
    </actuator>
  </transmission>
  <!--First flexor-->
  <transmission name="hand_left_index_virtual_1_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_left_index_virtual_1_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.7</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_left_index_motor">
      <mechanicalReduction>0.102888610798</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="hand_left_index_flex_1_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_left_index_flex_1_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.7</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_left_index_motor">
      <mechanicalReduction>0.102888610798</mechanicalReduction>
    </actuator>
  </transmission>
  <!--Second flexor-->
  <transmission name="hand_left_index_virtual_2_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_left_index_virtual_2_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.7</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_left_index_motor">
      <mechanicalReduction>0.102888610798</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="hand_left_index_flex_2_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_left_index_flex_2_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.7</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_left_index_motor">
      <mechanicalReduction>0.102888610798</mechanicalReduction>
    </actuator>
  </transmission>
  <!--Third flexor-->
  <transmission name="hand_left_index_virtual_3_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_left_index_virtual_3_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.7</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_left_index_motor">
      <mechanicalReduction>0.102888610798</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="hand_left_index_flex_3_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_left_index_flex_3_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.7</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_left_index_motor">
      <mechanicalReduction>0.102888610798</mechanicalReduction>
    </actuator>
  </transmission>
  <!--Abductor-->
  <transmission name="hand_left_middle_abd_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_left_middle_abd_joint">
      <saturationLower>0.0</saturationLower>
      <saturationUpper>0.0</saturationUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_left_mrl_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
  </transmission>
  <!--First flexor-->
  <transmission name="hand_left_middle_virtual_1_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_left_middle_virtual_1_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.0</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_left_mrl_motor">
      <mechanicalReduction>0.0679245283019</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="hand_left_middle_flex_1_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_left_middle_flex_1_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.0</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_left_mrl_motor">
      <mechanicalReduction>0.0679245283019</mechanicalReduction>
    </actuator>
  </transmission>
  <!--Second flexor-->
  <transmission name="hand_left_middle_virtual_2_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_left_middle_virtual_2_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.0</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_left_mrl_motor">
      <mechanicalReduction>0.0679245283019</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="hand_left_middle_flex_2_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_left_middle_flex_2_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.0</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_left_mrl_motor">
      <mechanicalReduction>0.0679245283019</mechanicalReduction>
    </actuator>
  </transmission>
  <!--Third flexor-->
  <transmission name="hand_left_middle_virtual_3_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_left_middle_virtual_3_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.0</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_left_mrl_motor">
      <mechanicalReduction>0.0679245283019</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="hand_left_middle_flex_3_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_left_middle_flex_3_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.0</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_left_mrl_motor">
      <mechanicalReduction>0.0679245283019</mechanicalReduction>
    </actuator>
  </transmission>
  <!--Abductor-->
  <transmission name="hand_left_ring_abd_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_left_ring_abd_joint">
      <saturationLower>-0.261799387799</saturationLower>
      <saturationUpper>0.0</saturationUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_left_mrl_motor">
      <mechanicalReduction>-0.232710566933</mechanicalReduction>
    </actuator>
  </transmission>
  <!--First flexor-->
  <transmission name="hand_left_ring_virtual_1_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_left_ring_virtual_1_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.75</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_left_mrl_motor">
      <mechanicalReduction>0.0739176962651</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="hand_left_ring_flex_1_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_left_ring_flex_1_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.75</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_left_mrl_motor">
      <mechanicalReduction>0.0739176962651</mechanicalReduction>
    </actuator>
  </transmission>
  <!--Second flexor-->
  <transmission name="hand_left_ring_virtual_2_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_left_ring_virtual_2_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.75</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_left_mrl_motor">
      <mechanicalReduction>0.0739176962651</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="hand_left_ring_flex_2_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_left_ring_flex_2_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.75</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_left_mrl_motor">
      <mechanicalReduction>0.0739176962651</mechanicalReduction>
    </actuator>
  </transmission>
  <!--Third flexor-->
  <transmission name="hand_left_ring_virtual_3_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_left_ring_virtual_3_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.75</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_left_mrl_motor">
      <mechanicalReduction>0.0739176962651</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="hand_left_ring_flex_3_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_left_ring_flex_3_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.75</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_left_mrl_motor">
      <mechanicalReduction>0.0739176962651</mechanicalReduction>
    </actuator>
  </transmission>
  <!--Abductor-->
  <transmission name="hand_left_little_abd_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_left_little_abd_joint">
      <saturationLower>-0.523598775598</saturationLower>
      <saturationUpper>0.0</saturationUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_left_mrl_motor">
      <mechanicalReduction>-0.465421133865</mechanicalReduction>
    </actuator>
  </transmission>
  <!--First flexor-->
  <transmission name="hand_left_little_virtual_1_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_left_little_virtual_1_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.75</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_left_mrl_motor">
      <mechanicalReduction>0.0739176962651</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="hand_left_little_flex_1_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_left_little_flex_1_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.75</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_left_mrl_motor">
      <mechanicalReduction>0.0739176962651</mechanicalReduction>
    </actuator>
  </transmission>
  <!--Second flexor-->
  <transmission name="hand_left_little_virtual_2_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_left_little_virtual_2_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.75</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_left_mrl_motor">
      <mechanicalReduction>0.0739176962651</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="hand_left_little_flex_2_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_left_little_flex_2_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.75</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_left_mrl_motor">
      <mechanicalReduction>0.0739176962651</mechanicalReduction>
    </actuator>
  </transmission>
  <!--Third flexor-->
  <transmission name="hand_left_little_virtual_3_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_left_little_virtual_3_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.75</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_left_mrl_motor">
      <mechanicalReduction>0.0739176962651</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="hand_left_little_flex_3_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_left_little_flex_3_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.75</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_left_mrl_motor">
      <mechanicalReduction>0.0739176962651</mechanicalReduction>
    </actuator>
  </transmission>
  <link name="hand_left_grasping_frame"/>
  <joint name="hand_left_grasping_fixed_joint" type="fixed">
    <parent link="hand_left_palm_link"/>
    <child link="hand_left_grasping_frame"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 0" xyz="0.13 0.02 0"/>
  </joint>
  <gazebo>
    <plugin filename="libgazebo_underactuated_finger.so" name="gazebo_hand_left_thumb">
      <actuatedJoint>hand_left_thumb_joint</actuatedJoint>
      <virtualJoint>
        <name>hand_left_thumb_abd_joint</name>
        <scale_factor>2.0</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_left_thumb_flex_1_joint</name>
        <scale_factor>2.5</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_left_thumb_virtual_1_joint</name>
        <scale_factor>3.0</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_left_thumb_flex_2_joint</name>
        <scale_factor>3.5</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_left_thumb_virtual_2_joint</name>
        <scale_factor>3.0</scale_factor>
      </virtualJoint>
    </plugin>
    <plugin filename="libgazebo_underactuated_finger.so" name="gazebo_hand_left_index">
      <actuatedJoint>hand_left_index_joint</actuatedJoint>
      <virtualJoint>
        <name>hand_left_index_abd_joint</name>
        <scale_factor>5.0</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_left_index_flex_1_joint</name>
        <scale_factor>2.5</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_left_index_virtual_1_joint</name>
        <scale_factor>3.0</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_left_index_flex_2_joint</name>
        <scale_factor>3.5</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_left_index_virtual_2_joint</name>
        <scale_factor>3.5</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_left_index_flex_3_joint</name>
        <scale_factor>4.0</scale_factor>
      </virtualJoint>
    </plugin>
    <plugin filename="libgazebo_underactuated_finger.so" name="gazebo_hand_left_middle">
      <actuatedJoint>hand_left_mrl_joint</actuatedJoint>
      <virtualJoint>
        <name>hand_left_middle_flex_1_joint</name>
        <scale_factor>2.5</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_left_middle_virtual_1_joint</name>
        <scale_factor>3.0</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_left_middle_flex_2_joint</name>
        <scale_factor>3.5</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_left_middle_virtual_2_joint</name>
        <scale_factor>3.5</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_left_middle_flex_3_joint</name>
        <scale_factor>4.0</scale_factor>
      </virtualJoint>
    </plugin>
    <plugin filename="libgazebo_underactuated_finger.so" name="gazebo_hand_left_ring">
      <actuatedJoint>hand_left_mrl_joint</actuatedJoint>
      <virtualJoint>
        <name>hand_left_ring_flex_1_joint</name>
        <scale_factor>2.5</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_left_ring_virtual_1_joint</name>
        <scale_factor>3.0</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_left_ring_flex_2_joint</name>
        <scale_factor>3.5</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_left_ring_virtual_2_joint</name>
        <scale_factor>3.5</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_left_ring_flex_3_joint</name>
        <scale_factor>4.0</scale_factor>
      </virtualJoint>
    </plugin>
    <plugin filename="libgazebo_underactuated_finger.so" name="gazebo_hand_left_little">
      <actuatedJoint>hand_left_mrl_joint</actuatedJoint>
      <virtualJoint>
        <name>hand_left_little_flex_1_joint</name>
        <scale_factor>2.5</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_left_little_virtual_1_joint</name>
        <scale_factor>3.0</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_left_little_flex_2_joint</name>
        <scale_factor>3.5</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_left_little_virtual_2_joint</name>
        <scale_factor>3.5</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_left_little_flex_3_joint</name>
        <scale_factor>4.0</scale_factor>
      </virtualJoint>
    </plugin>
  </gazebo>
  <joint name="hand_left_safety_box_joint" type="fixed">
    <parent link="wrist_left_ft_tool_link"/>
    <child link="hand_left_safety_box"/>
    <origin rpy="0 0 0" xyz="0.1 0.02 -0.02"/>
  </joint>
  <link name="hand_left_safety_box">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.0001"/>
      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
    </inertial>
    <!--
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <box size="0.2 0.115 0.18" />
        </geometry>
      </visual>
      -->
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.2 0.115 0.18"/>
      </geometry>
    </collision>
  </link>
  <!--************************-->
  <!--        SHOULDER        -->
  <!--************************-->
  <link name="arm_right_1_link">
    <inertial>
      <!--<origin xyz="0.061191 ${reflect * -0.022397} -0.012835" rpy="0.00000 0.00000 0.00000"/>-->
      <origin rpy="0.00000 0.00000 0.00000" xyz="0.061191 -0.022397 -0.012835"/>
      <mass value="1.563428"/>
      <inertia ixx="0.002471" ixy="-0.001809" ixz="-0.001202" iyy="0.006132" iyz="0.000494" izz="0.006704"/>
    </inertial>
    <!-- M90 module -->
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tiago_description/meshes/arm/arm_1.stl" scale="1 -1 1"/>
      </geometry>
      <material name="White"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tiago_description/meshes/arm/arm_1_collision.dae" scale="1 -1 1"/>
      </geometry>
    </collision>
  </link>
  <joint name="arm_right_1_joint" type="revolute">
    <parent link="torso_lift_link"/>
    <child link="arm_right_1_link"/>
    <origin rpy="0 0 -1.57079632679" xyz="0.02556 -0.19 -0.171"/>
    <axis xyz="0 0 1"/>
    <limit effort="43.0" lower="-1.1780972451" upper="1.57079632679" velocity="1.95"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-1.1080972451" soft_upper_limit="1.50079632679"/>
  </joint>
  <link name="arm_right_2_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="0.030432 0.000229 -0.005942"/>
      <mass value="1.800397"/>
      <inertia ixx="0.001809" ixy="1.5e-05" ixz="0.000254" iyy="0.004333" iyz="1.4e-05" izz="0.004341"/>
    </inertial>
    <visual>
      <!-- <origin xyz="0 0 0" rpy="${180 * deg_to_rad} ${0 * deg_to_rad} ${-90 * deg_to_rad}" /> -->
      <origin rpy="0.0 0.0 0.0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tiago_description/meshes/arm/arm_2.stl" scale="1 1 1"/>
      </geometry>
      <material name="DarkGrey"/>
    </visual>
    <collision>
      <!-- <origin xyz="0 0 0" rpy="${180 * deg_to_rad} ${0 * deg_to_rad} ${-90 * deg_to_rad}" /> -->
      <origin rpy="0.0 0.0 0.0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tiago_description/meshes/arm/arm_2_collision.dae" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <joint name="arm_right_2_joint" type="revolute">
    <parent link="arm_right_1_link"/>
    <child link="arm_right_2_link"/>
    <origin rpy="-1.57079632679 0.0 0.0" xyz="0.125 -0.0195 -0.031"/>
    <axis xyz="0 0 1"/>
    <limit effort="43.0" lower="-1.1780972451" upper="1.57079632679" velocity="1.95"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-1.1080972451" soft_upper_limit="1.50079632679"/>
  </joint>
  <link name="arm_right_3_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="0.007418 -0.004361 -0.134194"/>
      <mass value="1.8"/>
      <inertia ixx="0.019541" ixy="-0.000105" ixz="-0.001717" iyy="0.019936" iyz="0.000852" izz="0.002864"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tiago_description/meshes/arm/arm_3.stl" scale="1 1 1"/>
      </geometry>
      <material name="DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tiago_description/meshes/arm/arm_3_collision.dae" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <joint name="arm_right_3_joint" type="revolute">
    <parent link="arm_right_2_link"/>
    <child link="arm_right_3_link"/>
    <origin rpy="-1.57079632679 0.0 1.57079632679" xyz="0.0895 0.0 -0.0015"/>
    <axis xyz="0 0 1"/>
    <limit effort="26" lower="-0.785398163397" upper="3.92699081699" velocity="2.35"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-0.715398163397" soft_upper_limit="3.85699081699"/>
  </joint>
  <!--************************-->
  <!--        ELBOW           -->
  <!--************************-->
  <link name="arm_right_4_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="-0.095658 0.014666 0.018133"/>
      <mass value="1.432697"/>
      <inertia ixx="0.002040" ixy="0.000725" ixz="-0.00115" iyy="0.008424" iyz="-0.000204" izz="0.008686"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tiago_description/meshes/arm/arm_4.stl" scale="1 1 1"/>
      </geometry>
      <material name="DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tiago_description/meshes/arm/arm_4_collision.dae" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <joint name="arm_right_4_joint" type="revolute">
    <parent link="arm_right_3_link"/>
    <child link="arm_right_4_link"/>
    <origin rpy="-1.57079632679 -1.57079632679 0.0" xyz="-0.02 -0.027 -0.222"/>
    <axis xyz="0 0 1"/>
    <limit effort="26" lower="-0.392699081699" upper="2.35619449019" velocity="2.35"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-0.322699081699" soft_upper_limit="2.28619449019"/>
  </joint>
  <gazebo reference="arm_right_1_link">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <gazebo reference="arm_right_2_link">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <gazebo reference="arm_right_3_link">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <gazebo reference="arm_right_4_link">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <gazebo reference="arm_right_1_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="arm_right_2_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="arm_right_3_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="arm_right_4_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!--************************-->
  <!--        WRIST           -->
  <!--************************-->
  <!--************************-->
  <!--        WRIST           -->
  <!--************************-->
  <link name="arm_right_5_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="0.001078 -0.000151 -0.077173"/>
      <mass value="1.655819"/>
      <inertia ixx="0.00613800000" ixy="-0.00000400000" ixz="0.00015600000" iyy="0.00564000000" iyz="0.00002200000" izz="0.00179700000"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tiago_description/meshes/arm/arm_5-wrist-2010.stl" scale="1 1 1"/>
      </geometry>
      <material name="LightGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tiago_description/meshes/arm/arm_5_collision-wrist-2010.dae" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <joint name="arm_right_5_joint" type="revolute">
    <parent link="arm_right_4_link"/>
    <child link="arm_right_5_link"/>
    <origin rpy="1.57079632679 -1.57079632679 -1.57079632679" xyz="-0.162 0.02 0.027"/>
    <axis xyz="0 0 1"/>
    <limit effort="3" lower="-2.09439510239" upper="2.09439510239" velocity="1.95"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-2.07439510239" soft_upper_limit="2.07439510239"/>
  </joint>
  <link name="arm_right_6_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="-0.000153 -0.003122 0.000183"/>
      <mass value="0.370063"/>
      <inertia ixx="0.00021500000" ixy="0.00000000000" ixz="0.00000000000" iyy="0.00017000000" iyz="0.00000200000" izz="0.00017400000"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tiago_description/meshes/arm/arm_6-wrist-2010.stl" scale="1 1 1"/>
      </geometry>
      <material name="LightGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tiago_description/meshes/arm/arm_6_collision-wrist-2010.dae" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <joint name="arm_right_6_joint" type="revolute">
    <parent link="arm_right_5_link"/>
    <child link="arm_right_6_link"/>
    <origin rpy="0.0 -1.57079632679 -1.57079632679" xyz="0 0 0.15"/>
    <axis xyz="0 0 1"/>
    <limit effort="6.6" lower="-1.41371669412" upper="1.41371669412" velocity="1.76"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-1.39371669412" soft_upper_limit="1.39371669412"/>
  </joint>
  <link name="arm_right_7_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="0.000010 -0.000003 -0.007774"/>
      <mass value="0.234252"/>
      <inertia ixx="0.00014200000" ixy="0.00000000000" ixz="0.00000000000" iyy="0.00011300000" iyz="0.00000000000" izz="0.00005200000"/>
    </inertial>
  </link>
  <joint name="arm_right_7_joint" type="revolute">
    <parent link="arm_right_6_link"/>
    <child link="arm_right_7_link"/>
    <origin rpy="1.57079632679 0.0 1.57079632679" xyz="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="6.6" lower="-2.09439510239" upper="2.09439510239" velocity="1.76"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-2.07439510239" soft_upper_limit="2.07439510239"/>
  </joint>
  <gazebo reference="arm_right_5_link">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="arm_right_6_link">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="arm_right_7_link">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="arm_right_5_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="arm_right_6_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="arm_right_7_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
    <provideFeedback>1</provideFeedback>
  </gazebo>
  <transmission name="arm_right_5_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="arm_right_5_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="arm_right_5_joint">
      <offset>0.0</offset>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    </joint>
  </transmission>
  <transmission name="arm_right_wrist_trans">
    <type>transmission_interface/DifferentialTransmission</type>
    <actuator name="arm_right_6_motor">
      <role>actuator1</role>
      <mechanicalReduction>-1.0</mechanicalReduction>
    </actuator>
    <actuator name="arm_right_7_motor">
      <role>actuator2</role>
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="arm_right_6_joint">
      <role>joint1</role>
      <offset>0.0</offset>
      <mechanicalReduction>1.0</mechanicalReduction>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <ignoreTransmissionAbsoluteEncoder>1</ignoreTransmissionAbsoluteEncoder>
    </joint>
    <joint name="arm_right_7_joint">
      <role>joint2</role>
      <offset>0.0</offset>
      <mechanicalReduction>-1</mechanicalReduction>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <ignoreTransmissionAbsoluteEncoder>1</ignoreTransmissionAbsoluteEncoder>
    </joint>
  </transmission>
  <!--***********************-->
  <!--        TOOL           -->
  <!--***********************-->
  <link name="arm_right_tool_link">
    <inertial>
      <mass value="0.1"/>
      <inertia ixx="8.33E-07" ixy="0.0" ixz="0.0" iyy="8.33E-07" iyz="0.0" izz="1.25E-06"/>
    </inertial>
    <visual>
      <origin rpy="0 1.57079632679 0" xyz="0.001 0 0"/>
      <geometry>
        <cylinder length="0.005" radius="0.005"/>
      </geometry>
      <material name="LightGrey"/>
    </visual>
    <collision>
      <origin rpy="0 1.57079632679 0" xyz="0.001 0 0"/>
      <geometry>
        <cylinder length="0.005" radius="0.005"/>
      </geometry>
    </collision>
  </link>
  <joint name="arm_right_tool_joint" type="fixed">
    <parent link="arm_right_7_link"/>
    <child link="arm_right_tool_link"/>
    <origin rpy="1.57079632679 -1.57079632679 3.14159265359" xyz="0 0 0.046"/>
  </joint>
  <transmission name="arm_right_1_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="arm_right_1_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="arm_right_1_joint">
      <offset>0.0</offset>
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
  </transmission>
  <transmission name="arm_right_2_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="arm_right_2_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="arm_right_2_joint">
      <offset>0.0</offset>
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
  </transmission>
  <transmission name="arm_right_3_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="arm_right_3_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="arm_right_3_joint">
      <offset>0.0</offset>
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
  </transmission>
  <transmission name="arm_right_4_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="arm_right_4_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="arm_right_4_joint">
      <offset>0.0</offset>
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
  </transmission>
  <!--************************-->
  <!--        ft sensor       -->
  <!--************************-->
  <link name="wrist_right_ft_link">
    <inertial>
      <mass value="0.1"/>
      <inertia ixx="1.6E-05" ixy="-2.0E-06" ixz="0.0" iyy="1.6E-05" iyz="0.0" izz="2.7E-05"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.0157" radius="0.0225"/>
      </geometry>
      <material name="LightGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.0157" radius="0.0225"/>
      </geometry>
    </collision>
  </link>
  <joint name="wrist_right_ft_joint" type="fixed">
    <parent link="arm_right_tool_link"/>
    <child link="wrist_right_ft_link"/>
    <origin rpy="1.57079632679 0 1.57079632679" xyz="0.00785 0 0"/>
  </joint>
  <!--***********************-->
  <!--       FT TOOL         -->
  <!--***********************-->
  <link name="wrist_right_ft_tool_link">
    <inertial>
      <mass value="0.1"/>
      <inertia ixx="1.64E-05" ixy="0.0" ixz="0.0" iyy="1.64E-05" iyz="0.0" izz="3.125E-05"/>
    </inertial>
    <visual>
      <origin rpy="0 1.57079632679 0" xyz="0.0 0 0"/>
      <geometry>
        <cylinder length="0.00975" radius="0.025"/>
      </geometry>
      <material name="FlatBlack"/>
    </visual>
    <collision>
      <origin rpy="0 1.57079632679 0" xyz="0.0 0 0"/>
      <geometry>
        <cylinder length="0.00975" radius="0.025"/>
      </geometry>
    </collision>
  </link>
  <joint name="wrist_right_tool_joint" type="fixed">
    <parent link="wrist_right_ft_link"/>
    <child link="wrist_right_ft_tool_link"/>
    <origin rpy="-1.57079632679 -1.57079632679 0" xyz="0 0 0.012725"/>
  </joint>
  <!-- Tool Link -->
  <link name="hand_right_tool_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.0001"/>
      <inertia ixx="0.00001" ixy="0.0" ixz="0.0" iyy="0.00001" iyz="0.0" izz="0.00001"/>
    </inertial>
  </link>
  <joint name="hand_right_tool_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="wrist_right_ft_tool_link"/>
    <child link="hand_right_tool_link"/>
    <axis xyz="0 0 0"/>
  </joint>
  <joint name="hand_right_palm_joint" type="fixed">
    <parent link="wrist_right_ft_tool_link"/>
    <child link="hand_right_palm_link"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <link name="hand_right_palm_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0641 -0.0022 0.0021"/>
      <!-- NOTE: Less than total palm mass of 0.598kg because actuator masses are specified separately -->
      <mass value="0.4"/>
      <inertia ixx="0.000305100" ixy="0.000005037" ixz="0.000015302" iyy="0.000811920" iyz="0.000007622" izz="0.000655851"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/palm.stl" scale="1 1 1"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/palm_collision.stl" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_right_palm_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_right_thumb_joint" type="revolute">
    <parent link="hand_right_palm_link"/>
    <child link="hand_right_thumb_link"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="-2.61799387799" upper="6.28318530718" velocity="3.35"/>
    <dynamics damping="0.5" friction="0.0"/>
    <!-- TODO: Tune -->
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-2.61799387799" soft_upper_limit="6.28318530718"/>
  </joint>
  <link name="hand_right_thumb_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.05"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!--To stabilize simulation-->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <sphere radius="0.001"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <sphere radius="0.001"/>
      </geometry>
    </collision>
  </link>
  <transmission name="hand_right_thumb_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="hand_right_thumb_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="hand_right_thumb_joint">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
  </transmission>
  <joint name="hand_right_index_joint" type="revolute">
    <parent link="hand_right_palm_link"/>
    <child link="hand_right_index_link"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="-3.05432619099" upper="6.80678408278" velocity="3.35"/>
    <dynamics damping="0.5" friction="0.0"/>
    <!-- TODO: Tune -->
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-3.05432619099" soft_upper_limit="6.80678408278"/>
  </joint>
  <link name="hand_right_index_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.05"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!--To stabilize simulation-->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <sphere radius="0.001"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <sphere radius="0.001"/>
      </geometry>
    </collision>
  </link>
  <transmission name="hand_right_index_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="hand_right_index_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="hand_right_index_joint">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
  </transmission>
  <joint name="hand_right_mrl_joint" type="revolute">
    <parent link="hand_right_palm_link"/>
    <child link="hand_right_mrl_link"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="-2.53072741539" upper="9.25024503557" velocity="3.56"/>
    <dynamics damping="0.5" friction="0.0"/>
    <!-- TODO: Tune -->
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-2.53072741539" soft_upper_limit="9.25024503557"/>
  </joint>
  <link name="hand_right_mrl_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.05"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!--To stabilize simulation-->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <sphere radius="0.001"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <sphere radius="0.001"/>
      </geometry>
    </collision>
  </link>
  <transmission name="hand_right_mrl_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="hand_right_mrl_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="hand_right_mrl_joint">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
  </transmission>
  <joint name="hand_right_thumb_abd_joint" type="revolute">
    <parent link="hand_right_palm_link"/>
    <child link="hand_right_thumb_abd_link"/>
    <origin rpy="0 -1.1344640138 0" xyz="0.0901 0.0134 0.038"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="1.57079632679" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_right_thumb_abd_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.0132 0.0037 -0.0017"/>
      <mass value="0.014"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/thumb_abd_collision.stl" scale="1 1 1"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/thumb_abd_collision.stl" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_right_thumb_abd_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_right_thumb_virtual_1_joint" type="revolute">
    <parent link="hand_right_thumb_abd_link"/>
    <child link="hand_right_thumb_virtual_1_link"/>
    <origin rpy="-1.14423785761 0 0" xyz="0.0225 0.003 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_right_thumb_virtual_1_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <mass value="0.01"/>
      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_right_thumb_virtual_1_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_right_thumb_flex_1_joint" type="revolute">
    <parent link="hand_right_thumb_virtual_1_link"/>
    <child link="hand_right_thumb_flex_1_link"/>
    <origin rpy="0 0 0" xyz="0.013 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_right_thumb_flex_1_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
      <mass value="0.009"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_right_thumb_flex_1_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_right_thumb_virtual_2_joint" type="revolute">
    <parent link="hand_right_thumb_flex_1_link"/>
    <child link="hand_right_thumb_virtual_2_link"/>
    <origin rpy="0 0 0" xyz="0.009 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_right_thumb_virtual_2_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <mass value="0.01"/>
      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_right_thumb_virtual_2_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_right_thumb_flex_2_joint" type="revolute">
    <parent link="hand_right_thumb_virtual_2_link"/>
    <child link="hand_right_thumb_flex_2_link"/>
    <origin rpy="0 0 0" xyz="0.013 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_right_thumb_flex_2_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0096 0.003 0.0"/>
      <mass value="0.015"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_right_thumb_flex_2_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_right_index_abd_joint" type="revolute">
    <parent link="hand_right_palm_link"/>
    <child link="hand_right_index_abd_link"/>
    <origin rpy="-1.57079632679 -0.523598775598 0" xyz="0.1265 0.0042 0.0265"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="-0.523598775598" upper="0.523598775598" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_right_index_abd_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.0018 0.0 0.0028"/>
      <mass value="0.018"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_abd_collision.stl" scale="1 1 1"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_abd_collision.stl" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_right_index_abd_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_right_index_virtual_1_joint" type="revolute">
    <parent link="hand_right_index_abd_link"/>
    <child link="hand_right_index_virtual_1_link"/>
    <origin rpy="1.57079632679 0 0" xyz="0.009 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_right_index_virtual_1_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <mass value="0.01"/>
      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_right_index_virtual_1_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_right_index_flex_1_joint" type="revolute">
    <parent link="hand_right_index_virtual_1_link"/>
    <child link="hand_right_index_flex_1_link"/>
    <origin rpy="0 0 0" xyz="0.013 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_right_index_flex_1_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
      <mass value="0.009"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_right_index_flex_1_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_right_index_virtual_2_joint" type="revolute">
    <parent link="hand_right_index_flex_1_link"/>
    <child link="hand_right_index_virtual_2_link"/>
    <origin rpy="0 0 0" xyz="0.009 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_right_index_virtual_2_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <mass value="0.01"/>
      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_right_index_virtual_2_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_right_index_flex_2_joint" type="revolute">
    <parent link="hand_right_index_virtual_2_link"/>
    <child link="hand_right_index_flex_2_link"/>
    <origin rpy="0 0 0" xyz="0.013 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_right_index_flex_2_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
      <mass value="0.009"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_right_index_flex_2_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_right_index_virtual_3_joint" type="revolute">
    <parent link="hand_right_index_flex_2_link"/>
    <child link="hand_right_index_virtual_3_link"/>
    <origin rpy="0 0 0" xyz="0.009 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_right_index_virtual_3_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <mass value="0.01"/>
      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_right_index_virtual_3_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_right_index_flex_3_joint" type="revolute">
    <parent link="hand_right_index_virtual_3_link"/>
    <child link="hand_right_index_flex_3_link"/>
    <origin rpy="0 0 0" xyz="0.013 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_right_index_flex_3_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0096 0.003 0.0"/>
      <mass value="0.015"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_right_index_flex_3_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_right_middle_abd_joint" type="revolute">
    <parent link="hand_right_palm_link"/>
    <child link="hand_right_middle_abd_link"/>
    <origin rpy="-1.57079632679 0 0" xyz="0.1335 0.0042 0.007"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="-0.523598775598" upper="0.523598775598" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_right_middle_abd_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.0018 0.0 0.0028"/>
      <mass value="0.018"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_abd_collision.stl" scale="1 1 1"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_abd_collision.stl" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_right_middle_abd_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_right_middle_virtual_1_joint" type="revolute">
    <parent link="hand_right_middle_abd_link"/>
    <child link="hand_right_middle_virtual_1_link"/>
    <origin rpy="1.57079632679 0 0" xyz="0.009 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_right_middle_virtual_1_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <mass value="0.01"/>
      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_right_middle_virtual_1_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_right_middle_flex_1_joint" type="revolute">
    <parent link="hand_right_middle_virtual_1_link"/>
    <child link="hand_right_middle_flex_1_link"/>
    <origin rpy="0 0 0" xyz="0.013 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_right_middle_flex_1_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
      <mass value="0.009"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_right_middle_flex_1_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_right_middle_virtual_2_joint" type="revolute">
    <parent link="hand_right_middle_flex_1_link"/>
    <child link="hand_right_middle_virtual_2_link"/>
    <origin rpy="0 0 0" xyz="0.009 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_right_middle_virtual_2_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <mass value="0.01"/>
      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_right_middle_virtual_2_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_right_middle_flex_2_joint" type="revolute">
    <parent link="hand_right_middle_virtual_2_link"/>
    <child link="hand_right_middle_flex_2_link"/>
    <origin rpy="0 0 0" xyz="0.013 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_right_middle_flex_2_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
      <mass value="0.009"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_right_middle_flex_2_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_right_middle_virtual_3_joint" type="revolute">
    <parent link="hand_right_middle_flex_2_link"/>
    <child link="hand_right_middle_virtual_3_link"/>
    <origin rpy="0 0 0" xyz="0.009 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_right_middle_virtual_3_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <mass value="0.01"/>
      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_right_middle_virtual_3_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_right_middle_flex_3_joint" type="revolute">
    <parent link="hand_right_middle_virtual_3_link"/>
    <child link="hand_right_middle_flex_3_link"/>
    <origin rpy="0 0 0" xyz="0.013 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_right_middle_flex_3_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0096 0.003 0.0"/>
      <mass value="0.015"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_right_middle_flex_3_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_right_ring_abd_joint" type="revolute">
    <parent link="hand_right_palm_link"/>
    <child link="hand_right_ring_abd_link"/>
    <origin rpy="-1.57079632679 0.261799387799 0" xyz="0.1265 0.0042 -0.0125"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="-0.523598775598" upper="0.523598775598" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_right_ring_abd_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.0018 0.0 0.0028"/>
      <mass value="0.018"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_abd_collision.stl" scale="1 1 1"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_abd_collision.stl" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_right_ring_abd_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_right_ring_virtual_1_joint" type="revolute">
    <parent link="hand_right_ring_abd_link"/>
    <child link="hand_right_ring_virtual_1_link"/>
    <origin rpy="1.57079632679 0 0" xyz="0.009 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_right_ring_virtual_1_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <mass value="0.01"/>
      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_right_ring_virtual_1_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_right_ring_flex_1_joint" type="revolute">
    <parent link="hand_right_ring_virtual_1_link"/>
    <child link="hand_right_ring_flex_1_link"/>
    <origin rpy="0 0 0" xyz="0.013 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_right_ring_flex_1_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
      <mass value="0.009"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_right_ring_flex_1_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_right_ring_virtual_2_joint" type="revolute">
    <parent link="hand_right_ring_flex_1_link"/>
    <child link="hand_right_ring_virtual_2_link"/>
    <origin rpy="0 0 0" xyz="0.009 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_right_ring_virtual_2_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <mass value="0.01"/>
      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_right_ring_virtual_2_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_right_ring_flex_2_joint" type="revolute">
    <parent link="hand_right_ring_virtual_2_link"/>
    <child link="hand_right_ring_flex_2_link"/>
    <origin rpy="0 0 0" xyz="0.013 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_right_ring_flex_2_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
      <mass value="0.009"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_right_ring_flex_2_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_right_ring_virtual_3_joint" type="revolute">
    <parent link="hand_right_ring_flex_2_link"/>
    <child link="hand_right_ring_virtual_3_link"/>
    <origin rpy="0 0 0" xyz="0.009 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_right_ring_virtual_3_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <mass value="0.01"/>
      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_right_ring_virtual_3_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_right_ring_flex_3_joint" type="revolute">
    <parent link="hand_right_ring_virtual_3_link"/>
    <child link="hand_right_ring_flex_3_link"/>
    <origin rpy="0 0 0" xyz="0.013 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_right_ring_flex_3_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0096 0.003 0.0"/>
      <mass value="0.015"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_right_ring_flex_3_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_right_little_abd_joint" type="revolute">
    <parent link="hand_right_palm_link"/>
    <child link="hand_right_little_abd_link"/>
    <origin rpy="-1.57079632679 0.523598775598 0" xyz="0.1165 0.0042 -0.032"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="-0.523598775598" upper="0.523598775598" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_right_little_abd_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.0018 0.0 0.0028"/>
      <mass value="0.018"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_abd_collision.stl" scale="1 1 1"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_abd_collision.stl" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_right_little_abd_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_right_little_virtual_1_joint" type="revolute">
    <parent link="hand_right_little_abd_link"/>
    <child link="hand_right_little_virtual_1_link"/>
    <origin rpy="1.57079632679 0 0" xyz="0.009 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_right_little_virtual_1_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <mass value="0.01"/>
      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_right_little_virtual_1_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_right_little_flex_1_joint" type="revolute">
    <parent link="hand_right_little_virtual_1_link"/>
    <child link="hand_right_little_flex_1_link"/>
    <origin rpy="0 0 0" xyz="0.013 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_right_little_flex_1_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
      <mass value="0.009"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_right_little_flex_1_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_right_little_virtual_2_joint" type="revolute">
    <parent link="hand_right_little_flex_1_link"/>
    <child link="hand_right_little_virtual_2_link"/>
    <origin rpy="0 0 0" xyz="0.009 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_right_little_virtual_2_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <mass value="0.01"/>
      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_right_little_virtual_2_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_right_little_flex_2_joint" type="revolute">
    <parent link="hand_right_little_virtual_2_link"/>
    <child link="hand_right_little_flex_2_link"/>
    <origin rpy="0 0 0" xyz="0.013 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_right_little_flex_2_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0047 0.0031 0.0"/>
      <mass value="0.009"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_collision.stl"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_right_little_flex_2_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_right_little_virtual_3_joint" type="revolute">
    <parent link="hand_right_little_flex_2_link"/>
    <child link="hand_right_little_virtual_3_link"/>
    <origin rpy="0 0 0" xyz="0.009 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_right_little_virtual_3_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <mass value="0.01"/>
      <!--TODO: can we make it zero and still have a stable dynamic simulation?-->
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.0065 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.013 0.002 0.012"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_right_little_virtual_3_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <joint name="hand_right_little_flex_3_joint" type="revolute">
    <parent link="hand_right_little_virtual_3_link"/>
    <child link="hand_right_little_flex_3_link"/>
    <origin rpy="0 0 0" xyz="0.013 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="2.0" lower="0.0" upper="0.785398163397" velocity="2.0"/>
    <dynamics damping="0.5" friction="0.0"/>
  </joint>
  <link name="hand_right_little_flex_3_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0096 0.003 0.0"/>
      <mass value="0.015"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      <!-- To stabilize simulation -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
      </geometry>
      <material name="Hey5DarkGrey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://hey5_description/meshes/finger_flex_tip_collision.stl"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="hand_right_little_flex_3_link">
    <material>Gazebo/Black</material>
  </gazebo>
  <!--Abductor-->
  <transmission name="hand_right_thumb_abd_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_right_thumb_abd_joint">
      <saturationLower>0.0</saturationLower>
      <saturationUpper>1.57079632679</saturationUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_right_thumb_motor">
      <mechanicalReduction>0.654498469498</mechanicalReduction>
    </actuator>
  </transmission>
  <!--First flexor-->
  <transmission name="hand_right_thumb_virtual_1_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_right_thumb_virtual_1_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>2.0</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_right_thumb_motor">
      <mechanicalReduction>0.162993578547</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="hand_right_thumb_flex_1_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_right_thumb_flex_1_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>2.0</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_right_thumb_motor">
      <mechanicalReduction>0.162993578547</mechanicalReduction>
    </actuator>
  </transmission>
  <!--Second flexor-->
  <transmission name="hand_right_thumb_virtual_2_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_right_thumb_virtual_2_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>2.0</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_right_thumb_motor">
      <mechanicalReduction>0.162993578547</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="hand_right_thumb_flex_2_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_right_thumb_flex_2_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>2.0</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_right_thumb_motor">
      <mechanicalReduction>0.162993578547</mechanicalReduction>
    </actuator>
  </transmission>
  <!--Abductor-->
  <transmission name="hand_right_index_abd_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_right_index_abd_joint">
      <saturationLower>0.0</saturationLower>
      <saturationUpper>0.523598775598</saturationUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_right_index_motor">
      <mechanicalReduction>0.49866550057</mechanicalReduction>
    </actuator>
  </transmission>
  <!--First flexor-->
  <transmission name="hand_right_index_virtual_1_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_right_index_virtual_1_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.7</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_right_index_motor">
      <mechanicalReduction>0.102888610798</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="hand_right_index_flex_1_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_right_index_flex_1_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.7</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_right_index_motor">
      <mechanicalReduction>0.102888610798</mechanicalReduction>
    </actuator>
  </transmission>
  <!--Second flexor-->
  <transmission name="hand_right_index_virtual_2_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_right_index_virtual_2_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.7</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_right_index_motor">
      <mechanicalReduction>0.102888610798</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="hand_right_index_flex_2_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_right_index_flex_2_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.7</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_right_index_motor">
      <mechanicalReduction>0.102888610798</mechanicalReduction>
    </actuator>
  </transmission>
  <!--Third flexor-->
  <transmission name="hand_right_index_virtual_3_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_right_index_virtual_3_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.7</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_right_index_motor">
      <mechanicalReduction>0.102888610798</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="hand_right_index_flex_3_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_right_index_flex_3_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.7</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_right_index_motor">
      <mechanicalReduction>0.102888610798</mechanicalReduction>
    </actuator>
  </transmission>
  <!--Abductor-->
  <transmission name="hand_right_middle_abd_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_right_middle_abd_joint">
      <saturationLower>0.0</saturationLower>
      <saturationUpper>0.0</saturationUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_right_mrl_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
  </transmission>
  <!--First flexor-->
  <transmission name="hand_right_middle_virtual_1_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_right_middle_virtual_1_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.0</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_right_mrl_motor">
      <mechanicalReduction>0.0679245283019</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="hand_right_middle_flex_1_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_right_middle_flex_1_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.0</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_right_mrl_motor">
      <mechanicalReduction>0.0679245283019</mechanicalReduction>
    </actuator>
  </transmission>
  <!--Second flexor-->
  <transmission name="hand_right_middle_virtual_2_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_right_middle_virtual_2_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.0</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_right_mrl_motor">
      <mechanicalReduction>0.0679245283019</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="hand_right_middle_flex_2_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_right_middle_flex_2_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.0</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_right_mrl_motor">
      <mechanicalReduction>0.0679245283019</mechanicalReduction>
    </actuator>
  </transmission>
  <!--Third flexor-->
  <transmission name="hand_right_middle_virtual_3_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_right_middle_virtual_3_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.0</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_right_mrl_motor">
      <mechanicalReduction>0.0679245283019</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="hand_right_middle_flex_3_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_right_middle_flex_3_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.0</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_right_mrl_motor">
      <mechanicalReduction>0.0679245283019</mechanicalReduction>
    </actuator>
  </transmission>
  <!--Abductor-->
  <transmission name="hand_right_ring_abd_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_right_ring_abd_joint">
      <saturationLower>-0.261799387799</saturationLower>
      <saturationUpper>0.0</saturationUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_right_mrl_motor">
      <mechanicalReduction>-0.232710566933</mechanicalReduction>
    </actuator>
  </transmission>
  <!--First flexor-->
  <transmission name="hand_right_ring_virtual_1_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_right_ring_virtual_1_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.75</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_right_mrl_motor">
      <mechanicalReduction>0.0739176962651</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="hand_right_ring_flex_1_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_right_ring_flex_1_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.75</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_right_mrl_motor">
      <mechanicalReduction>0.0739176962651</mechanicalReduction>
    </actuator>
  </transmission>
  <!--Second flexor-->
  <transmission name="hand_right_ring_virtual_2_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_right_ring_virtual_2_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.75</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_right_mrl_motor">
      <mechanicalReduction>0.0739176962651</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="hand_right_ring_flex_2_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_right_ring_flex_2_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.75</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_right_mrl_motor">
      <mechanicalReduction>0.0739176962651</mechanicalReduction>
    </actuator>
  </transmission>
  <!--Third flexor-->
  <transmission name="hand_right_ring_virtual_3_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_right_ring_virtual_3_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.75</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_right_mrl_motor">
      <mechanicalReduction>0.0739176962651</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="hand_right_ring_flex_3_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_right_ring_flex_3_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.75</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_right_mrl_motor">
      <mechanicalReduction>0.0739176962651</mechanicalReduction>
    </actuator>
  </transmission>
  <!--Abductor-->
  <transmission name="hand_right_little_abd_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_right_little_abd_joint">
      <saturationLower>-0.523598775598</saturationLower>
      <saturationUpper>0.0</saturationUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_right_mrl_motor">
      <mechanicalReduction>-0.465421133865</mechanicalReduction>
    </actuator>
  </transmission>
  <!--First flexor-->
  <transmission name="hand_right_little_virtual_1_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_right_little_virtual_1_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.75</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_right_mrl_motor">
      <mechanicalReduction>0.0739176962651</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="hand_right_little_flex_1_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_right_little_flex_1_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.75</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_right_mrl_motor">
      <mechanicalReduction>0.0739176962651</mechanicalReduction>
    </actuator>
  </transmission>
  <!--Second flexor-->
  <transmission name="hand_right_little_virtual_2_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_right_little_virtual_2_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.75</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_right_mrl_motor">
      <mechanicalReduction>0.0739176962651</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="hand_right_little_flex_2_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_right_little_flex_2_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.75</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_right_mrl_motor">
      <mechanicalReduction>0.0739176962651</mechanicalReduction>
    </actuator>
  </transmission>
  <!--Third flexor-->
  <transmission name="hand_right_little_virtual_3_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_right_little_virtual_3_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.75</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_right_mrl_motor">
      <mechanicalReduction>0.0739176962651</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="hand_right_little_flex_3_trans">
    <type>hey5_transmissions/Hey5Reducer</type>
    <joint name="hand_right_little_flex_3_joint">
      <saturationLower>0.0</saturationLower>
      <deadBandUpper>0.75</deadBandUpper>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
    </joint>
    <actuator name="hand_right_mrl_motor">
      <mechanicalReduction>0.0739176962651</mechanicalReduction>
    </actuator>
  </transmission>
  <link name="hand_right_grasping_frame"/>
  <joint name="hand_right_grasping_fixed_joint" type="fixed">
    <parent link="hand_right_palm_link"/>
    <child link="hand_right_grasping_frame"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 0" xyz="0.13 0.02 0"/>
  </joint>
  <gazebo>
    <plugin filename="libgazebo_underactuated_finger.so" name="gazebo_hand_right_thumb">
      <actuatedJoint>hand_right_thumb_joint</actuatedJoint>
      <virtualJoint>
        <name>hand_right_thumb_abd_joint</name>
        <scale_factor>2.0</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_right_thumb_flex_1_joint</name>
        <scale_factor>2.5</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_right_thumb_virtual_1_joint</name>
        <scale_factor>3.0</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_right_thumb_flex_2_joint</name>
        <scale_factor>3.5</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_right_thumb_virtual_2_joint</name>
        <scale_factor>3.0</scale_factor>
      </virtualJoint>
    </plugin>
    <plugin filename="libgazebo_underactuated_finger.so" name="gazebo_hand_right_index">
      <actuatedJoint>hand_right_index_joint</actuatedJoint>
      <virtualJoint>
        <name>hand_right_index_abd_joint</name>
        <scale_factor>5.0</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_right_index_flex_1_joint</name>
        <scale_factor>2.5</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_right_index_virtual_1_joint</name>
        <scale_factor>3.0</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_right_index_flex_2_joint</name>
        <scale_factor>3.5</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_right_index_virtual_2_joint</name>
        <scale_factor>3.5</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_right_index_flex_3_joint</name>
        <scale_factor>4.0</scale_factor>
      </virtualJoint>
    </plugin>
    <plugin filename="libgazebo_underactuated_finger.so" name="gazebo_hand_right_middle">
      <actuatedJoint>hand_right_mrl_joint</actuatedJoint>
      <virtualJoint>
        <name>hand_right_middle_flex_1_joint</name>
        <scale_factor>2.5</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_right_middle_virtual_1_joint</name>
        <scale_factor>3.0</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_right_middle_flex_2_joint</name>
        <scale_factor>3.5</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_right_middle_virtual_2_joint</name>
        <scale_factor>3.5</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_right_middle_flex_3_joint</name>
        <scale_factor>4.0</scale_factor>
      </virtualJoint>
    </plugin>
    <plugin filename="libgazebo_underactuated_finger.so" name="gazebo_hand_right_ring">
      <actuatedJoint>hand_right_mrl_joint</actuatedJoint>
      <virtualJoint>
        <name>hand_right_ring_flex_1_joint</name>
        <scale_factor>2.5</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_right_ring_virtual_1_joint</name>
        <scale_factor>3.0</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_right_ring_flex_2_joint</name>
        <scale_factor>3.5</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_right_ring_virtual_2_joint</name>
        <scale_factor>3.5</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_right_ring_flex_3_joint</name>
        <scale_factor>4.0</scale_factor>
      </virtualJoint>
    </plugin>
    <plugin filename="libgazebo_underactuated_finger.so" name="gazebo_hand_right_little">
      <actuatedJoint>hand_right_mrl_joint</actuatedJoint>
      <virtualJoint>
        <name>hand_right_little_flex_1_joint</name>
        <scale_factor>2.5</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_right_little_virtual_1_joint</name>
        <scale_factor>3.0</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_right_little_flex_2_joint</name>
        <scale_factor>3.5</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_right_little_virtual_2_joint</name>
        <scale_factor>3.5</scale_factor>
      </virtualJoint>
      <virtualJoint>
        <name>hand_right_little_flex_3_joint</name>
        <scale_factor>4.0</scale_factor>
      </virtualJoint>
    </plugin>
  </gazebo>
  <joint name="hand_right_safety_box_joint" type="fixed">
    <parent link="wrist_right_ft_tool_link"/>
    <child link="hand_right_safety_box"/>
    <origin rpy="0 0 0" xyz="0.1 0.02 0.02"/>
  </joint>
  <link name="hand_right_safety_box">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.0001"/>
      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
    </inertial>
    <!--
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <box size="0.2 0.115 0.18" />
        </geometry>
      </visual>
      -->
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.2 0.115 0.18"/>
      </geometry>
    </collision>
  </link>
  <!-- RGBD Laser Link -->
  <joint name="rgbd_laser_joint" type="fixed">
    <parent link="base_footprint"/>
    <child link="rgbd_laser_link"/>
    <origin rpy="0 0 0" xyz="-0.9 0 0"/>
  </joint>
  <link name="rgbd_laser_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.0001"/>
      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
    </inertial>
  </link>
</robot>
