<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from src/tiago_robot/tiago_description/robots/tiago_steel.urdf.xacro | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<!--
  Copyright (c) 2011, 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" xmlns:xacro="http://www.ros.org/wiki/xacro">
  <!-- The following included files set up definitions of parts of the robot body -->
  <!-- Base -->
  <!-- Wheel transmission -->
  <!-- Wheel characteristics -->
  <!-- Caster wheel characteristics -->
  <!-- Macro -->
  <!-- Laser characteristics -->
  <!-- Torso -->
  <!--File includes-->
  <!--Constant parameters-->
  <!-- Arm -->
  <!--File includes-->
  <!--File includes-->
  <!--Constant parameters-->
  <!--Constant parameters-->
  <!-- Head -->
  <!--File includes-->
  <!-- Macro -->
  <!--Constant parameters-->
  <!--************************-->
  <!--     HEAD_1 (PAN)       -->
  <!--************************-->
  <!--************************-->
  <!--      HEAD_2 (TILT)     -->
  <!--************************-->
  <!-- Gripper -->
  <!--File includes-->
  <!-- Generic simulator_gazebo plugins -->
  <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_link</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>
  <!-- define global properties -->
  <!-- Materials for visualization -->
  <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>
  <!-- Now we can start using the macros included above to define the actual robot -->
  <!-- Laser -->
  <!-- Theoretic FOV = 220 -->
  <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 extensions -->
  <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.518518517</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>10.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</topicName>
        <frameName>base_laser_link</frameName>
      </plugin>
    </sensor>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <!-- Sonars -->
  <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 extensions -->
  <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 extensions -->
  <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 extensions -->
  <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>
  <!-- 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/high_resolution/base_collision.stl"/>
      </geometry>
      <material name="White"/>
    </collision>
  </link>
  <gazebo reference="base_link">
    <material>Gazebo/White</material>
  </gazebo>
  <!-- Base footprint -->
  <link name="base_footprint">
    <inertial>
        <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0"/>
      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
    </inertial>
  </link>
  <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>
  <!-- Orange cover & antennas -->
  <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>
  <!-- IMU -->
  <!-- @todo pmb2_hardware_gazebo needs an IMU!  -->
  <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>
  <!-- Wheels -->
  <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>
        <cylinder length="0.04" radius="0.0985"/>
      </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"/>
    <axis xyz="0 0 1"/>
    <limit effort="6.0" velocity="10.152284264"/>
  </joint>
  <!-- Transmission -->
  <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>
  <!-- Gazebo extensions -->
  <!-- Contact model for the wheel surface -->
  <gazebo reference="wheel_right_link">
    <kp>1000000.0</kp>
    <kd>1000.0</kd>
    <mu1>1.0</mu1>
    <mu2>1.0</mu2>
    <fdir1>1 0 0</fdir1>
    <maxVel>1.0</maxVel>
    <minDepth>0.00</minDepth>
    <implicitSpringDamper>1</implicitSpringDamper>
    <material>Gazebo/DarkGrey</material>
  </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>
        <cylinder length="0.04" radius="0.0985"/>
      </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"/>
    <axis xyz="0 0 1"/>
    <limit effort="6.0" velocity="10.152284264"/>
  </joint>
  <!-- Transmission -->
  <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>
  <!-- Gazebo extensions -->
  <!-- Contact model for the wheel surface -->
  <gazebo reference="wheel_left_link">
    <kp>1000000.0</kp>
    <kd>1000.0</kd>
    <mu1>1.0</mu1>
    <mu2>1.0</mu2>
    <fdir1>1 0 0</fdir1>
    <maxVel>1.0</maxVel>
    <minDepth>0.00</minDepth>
    <implicitSpringDamper>1</implicitSpringDamper>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <!-- Casters wheels -->
  <link name="caster_front_right_1_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.01108 0.00003 -0.030"/>
      <mass value="0.101"/>
      <inertia ixx="0.00002779423" ixy="-0.00000001384" ixz="0.0000074479" iyy="0.00002767062" iyz="-0.00000002774" izz="0.00001727605"/>
    </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.03677"/>
      <inertia ixx="0.00000755221" ixy="0" ixz="0" iyy="0.00000755221" iyz="0" izz="0.00001281996"/>
    </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>
        <cylinder length="0.015" 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"/>
  </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>
  <!-- Gazebo extensions -->
  <!-- Contact model for the caster wheel surface -->
  <gazebo reference="caster_front_right_1_link">
    <kp>1000000.0</kp>
    <kd>1000.0</kd>
    <mu1>1.0</mu1>
    <mu2>1.0</mu2>
    <fdir1>0 0 1</fdir1>
    <maxVel>1.0</maxVel>
    <minDepth>0.0</minDepth>
    <implicitSpringDamper>0</implicitSpringDamper>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <gazebo reference="caster_front_right_2_link">
    <kp>1000000.0</kp>
    <kd>1000.0</kd>
    <mu1>0.4</mu1>
    <mu2>0.4</mu2>
    <fdir1>1 0 0</fdir1>
    <maxVel>1.0</maxVel>
    <minDepth>0.00</minDepth>
    <implicitSpringDamper>0</implicitSpringDamper>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <link name="caster_front_left_1_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.01108 0.00003 -0.030"/>
      <mass value="0.101"/>
      <inertia ixx="0.00002779423" ixy="-0.00000001384" ixz="0.0000074479" iyy="0.00002767062" iyz="-0.00000002774" izz="0.00001727605"/>
    </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.03677"/>
      <inertia ixx="0.00000755221" ixy="0" ixz="0" iyy="0.00000755221" iyz="0" izz="0.00001281996"/>
    </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>
        <cylinder length="0.015" 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"/>
  </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>
  <!-- Gazebo extensions -->
  <!-- Contact model for the caster wheel surface -->
  <gazebo reference="caster_front_left_1_link">
    <kp>1000000.0</kp>
    <kd>1000.0</kd>
    <mu1>1.0</mu1>
    <mu2>1.0</mu2>
    <fdir1>0 0 1</fdir1>
    <maxVel>1.0</maxVel>
    <minDepth>0.0</minDepth>
    <implicitSpringDamper>0</implicitSpringDamper>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <gazebo reference="caster_front_left_2_link">
    <kp>1000000.0</kp>
    <kd>1000.0</kd>
    <mu1>0.4</mu1>
    <mu2>0.4</mu2>
    <fdir1>1 0 0</fdir1>
    <maxVel>1.0</maxVel>
    <minDepth>0.00</minDepth>
    <implicitSpringDamper>0</implicitSpringDamper>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <link name="caster_back_right_1_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.01108 0.00003 -0.030"/>
      <mass value="0.101"/>
      <inertia ixx="0.00002779423" ixy="-0.00000001384" ixz="0.0000074479" iyy="0.00002767062" iyz="-0.00000002774" izz="0.00001727605"/>
    </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.03677"/>
      <inertia ixx="0.00000755221" ixy="0" ixz="0" iyy="0.00000755221" iyz="0" izz="0.00001281996"/>
    </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>
        <cylinder length="0.015" 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"/>
  </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>
  <!-- Gazebo extensions -->
  <!-- Contact model for the caster wheel surface -->
  <gazebo reference="caster_back_right_1_link">
    <kp>1000000.0</kp>
    <kd>1000.0</kd>
    <mu1>1.0</mu1>
    <mu2>1.0</mu2>
    <fdir1>0 0 1</fdir1>
    <maxVel>1.0</maxVel>
    <minDepth>0.0</minDepth>
    <implicitSpringDamper>0</implicitSpringDamper>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <gazebo reference="caster_back_right_2_link">
    <kp>1000000.0</kp>
    <kd>1000.0</kd>
    <mu1>0.4</mu1>
    <mu2>0.4</mu2>
    <fdir1>1 0 0</fdir1>
    <maxVel>1.0</maxVel>
    <minDepth>0.00</minDepth>
    <implicitSpringDamper>0</implicitSpringDamper>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <link name="caster_back_left_1_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.01108 0.00003 -0.030"/>
      <mass value="0.101"/>
      <inertia ixx="0.00002779423" ixy="-0.00000001384" ixz="0.0000074479" iyy="0.00002767062" iyz="-0.00000002774" izz="0.00001727605"/>
    </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.03677"/>
      <inertia ixx="0.00000755221" ixy="0" ixz="0" iyy="0.00000755221" iyz="0" izz="0.00001281996"/>
    </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>
        <cylinder length="0.015" 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"/>
  </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>
  <!-- Gazebo extensions -->
  <!-- Contact model for the caster wheel surface -->
  <gazebo reference="caster_back_left_1_link">
    <kp>1000000.0</kp>
    <kd>1000.0</kd>
    <mu1>1.0</mu1>
    <mu2>1.0</mu2>
    <fdir1>0 0 1</fdir1>
    <maxVel>1.0</maxVel>
    <minDepth>0.0</minDepth>
    <implicitSpringDamper>0</implicitSpringDamper>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <gazebo reference="caster_back_left_2_link">
    <kp>1000000.0</kp>
    <kd>1000.0</kd>
    <mu1>0.4</mu1>
    <mu2>0.4</mu2>
    <fdir1>1 0 0</fdir1>
    <maxVel>1.0</maxVel>
    <minDepth>0.00</minDepth>
    <implicitSpringDamper>0</implicitSpringDamper>
    <material>Gazebo/DarkGrey</material>
  </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_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="6.93198"/>
      <inertia ixx="0.24487000000" ixy="-0.00145600000" ixz="-0.00974200000" iyy="0.24926900000" iyz="0.00828100000" izz="0.09039500000"/>
    </inertial>
    <!-- Lifting body -->
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tiago_description/meshes/torso/torso_lift_with_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.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>
    </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.288996939" soft_upper_limit="1.288996939"/>
  </joint>
  <gazebo reference="head_1_link">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="head_1_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!-- extensions -->
  <transmission name="head_1_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="head_1_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
    <joint name="head_1_joint">
      <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="-1.0271975512" soft_upper_limit="0.785398163397"/>
  </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">
      <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" xyz="0.0908 0.08 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>
  <!-- frames of the depth sensor -->
  <joint name="xtion_depth_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.0 0.049 0.0"/>
    <parent link="xtion_link"/>
    <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_link"/>
    <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>
  <!-- extensions -->
  <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.01229096616</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_1_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="0.08169 0.01822 -0.01884"/>
      <mass value="2.31325"/>
      <inertia ixx="0.00269400000" ixy="0.00115700000" ixz="-0.00142300000" iyy="0.00704100000" iyz="-0.00031200000" izz="0.00731200000"/>
    </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>
  <gazebo reference="arm_1_link">
    <material>Gazebo/Orange</material>
  </gazebo>
  <joint name="arm_1_joint" type="revolute">
    <parent link="torso_lift_link"/>
    <child link="arm_1_link"/>
    <origin rpy="0 0 -1.57079632679" xyz="0.15505 0.014 -0.151"/>
    <axis xyz="0 0 1"/>
    <limit effort="39.0" lower="0.0" upper="2.74889357189" velocity="2.7"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="0.02" soft_upper_limit="2.72889357189"/>
  </joint>
  <link name="arm_2_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="-0.03609 -0.00006 0.01649"/>
      <mass value="1.10224"/>
      <inertia ixx="0.00114600000" ixy="0.00000100000" ixz="0.00043500000" iyy="0.00218200000" iyz="-0.00000100000" izz="0.00237300000"/>
    </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_2_joint" type="revolute">
    <parent link="arm_1_link"/>
    <child link="arm_2_link"/>
    <origin rpy="1.57079632679 0.0 0.0" xyz="0.125 0.0165 -0.031"/>
    <axis xyz="0 0 1"/>
    <limit effort="39.0" lower="-1.57079632679" upper="1.0908307825" velocity="3.66"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-1.55079632679" soft_upper_limit="1.0708307825"/>
  </joint>
  <link name="arm_3_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="0.00752 0.00244 0.12957"/>
      <mass value="2.42460"/>
      <inertia ixx="0.01441800000" ixy="0.00008400000" ixz="0.00114300000" iyy="0.01478800000" iyz="0.00064200000" izz="0.00256000000"/>
    </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_3_joint" type="revolute">
    <parent link="arm_2_link"/>
    <child link="arm_3_link"/>
    <origin rpy="-1.57079632679 0.0 1.57079632679" xyz="0.0895 0.0 0.0015"/>
    <axis xyz="0 0 1"/>
    <limit effort="17.86" lower="-3.53429173529" upper="1.57079632679" velocity="4.58"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-3.51429173529" soft_upper_limit="1.55079632679"/>
  </joint>
  <!--************************-->
  <!--        ELBOW           -->
  <!--************************-->
  <link name="arm_4_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="0.07426 -0.01306 0.01549"/>
      <mass value="1.45429"/>
      <inertia ixx="0.00195300000" ixy="-0.00059600000" ixz="0.00088000000" iyy="0.00709700000" iyz="-0.00017100000" izz="0.00740500000"/>
    </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_4_joint" type="revolute">
    <parent link="arm_3_link"/>
    <child link="arm_4_link"/>
    <origin rpy="-1.57079632679 -1.57079632679 0.0" xyz="-0.02 -0.027 -0.222"/>
    <axis xyz="0 0 1"/>
    <limit effort="17.86" lower="-0.392699081699" upper="2.35619449019" velocity="4.58"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-0.372699081699" soft_upper_limit="2.33619449019"/>
  </joint>
  <gazebo reference="arm_1_link">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <gazebo reference="arm_2_link">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <gazebo reference="arm_3_link">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <gazebo reference="arm_4_link">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
    <material>Gazebo/DarkGrey</material>
  </gazebo>
  <gazebo reference="arm_1_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="arm_2_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="arm_3_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="arm_4_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <!--************************-->
  <!--        WRIST           -->
  <!--************************-->
  <!--************************-->
  <!--        WRIST           -->
  <!--************************-->
  <link name="arm_5_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="0.00003 -0.00028 0.07831"/>
      <mass value="1.07424"/>
      <inertia ixx="0.00168000000" ixy="0.00000100000" ixz="-0.00001500000" iyy="0.00160300000" iyz="-0.00001900000" izz="0.00040400000"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tiago_description/meshes/arm/arm_5.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.dae" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <joint name="arm_5_joint" type="revolute">
    <parent link="arm_4_link"/>
    <child link="arm_5_link"/>
    <origin rpy="0.0 -1.57079632679 0.0" 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_6_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="0.00531 0.00021 0.00002"/>
      <mass value="0.28722"/>
      <inertia ixx="0.00005900000" ixy="-0.00000000000" ixz="-0.00000000000" iyy="0.00010600000" iyz="0.00000000000" izz="0.00012600000"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://tiago_description/meshes/arm/arm_6.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.dae" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <joint name="arm_6_joint" type="revolute">
    <parent link="arm_5_link"/>
    <child link="arm_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.57079632679" upper="1.57079632679" velocity="1.76"/>
    <dynamics damping="1.0" friction="1.0"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="-1.55079632679" soft_upper_limit="1.55079632679"/>
  </joint>
  <link name="arm_7_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="0.00011 -0.00034 0.02405"/>
      <mass value="0.05935"/>
      <inertia ixx="0.00001400000" ixy="-0.00000000000" ixz="-0.00000000000" iyy="0.00001400000" iyz="0.00000000000" izz="0.00000400000"/>
    </inertial>
  </link>
  <joint name="arm_7_joint" type="revolute">
    <parent link="arm_6_link"/>
    <child link="arm_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_5_link">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="arm_6_link">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="arm_7_link">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="arm_5_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="arm_6_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="arm_7_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
    <provideFeedback>1</provideFeedback>
  </gazebo>
  <!-- extensions -->
  <transmission name="arm_5_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="arm_5_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="arm_5_joint">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
  </transmission>
  <transmission name="wrist_trans">
    <type>transmission_interface/DifferentialTransmission</type>
    <actuator name="arm_6_motor">
      <role>actuator1</role>
      <mechanicalReduction>-1.0</mechanicalReduction>
    </actuator>
    <actuator name="arm_7_motor">
      <role>actuator2</role>
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="arm_6_joint">
      <role>joint1</role>
      <offset>0.0</offset>
      <mechanicalReduction>1.0</mechanicalReduction>
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <joint name="arm_7_joint">
      <role>joint2</role>
      <offset>0.0</offset>
      <mechanicalReduction>-1.0</mechanicalReduction>
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
  </transmission>
  <!--***********************-->
  <!--        TOOL           -->
  <!--***********************-->
  <link name="arm_tool_link">
    <inertial>
      <mass value="0.1"/>
      <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
    </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_tool_joint" type="fixed">
    <parent link="arm_7_link"/>
    <child link="arm_tool_link"/>
    <origin rpy="1.57079632679 -1.57079632679 3.14159265359" xyz="0 0 0.046"/>
  </joint>
  <!-- extensions -->
  <transmission name="arm_1_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="arm_1_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="arm_1_joint">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <!--
        <hardwareInterface>hardware_interface/JointModeInterface</hardwareInterface>
-->
    </joint>
  </transmission>
  <transmission name="arm_2_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="arm_2_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="arm_2_joint">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <!--
        <hardwareInterface>hardware_interface/JointModeInterface</hardwareInterface>
-->
    </joint>
  </transmission>
  <transmission name="arm_3_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="arm_3_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="arm_3_joint">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <!--
        <hardwareInterface>hardware_interface/JointModeInterface</hardwareInterface>
-->
    </joint>
  </transmission>
  <transmission name="arm_4_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="arm_4_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="arm_4_joint">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <!--
        <hardwareInterface>hardware_interface/JointModeInterface</hardwareInterface>
-->
    </joint>
  </transmission>
  <link name="gripper_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="gripper_tool_joint" type="fixed">
    <origin rpy="1.57079632679 1.57079632679 -1.57079632679" xyz="0.01 0 0"/>
    <parent link="arm_tool_link"/>
    <child link="gripper_tool_link"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="gripper_link">
    <inertial>
      <origin rpy="0.0 0.0 0.0" xyz="-0.000294 0.000291 -0.033086"/>
      <mass value="0.602660"/>
      <inertia ixx="0.000693" ixy="0.000045" ixz="-0.000000" iyy="0.001024" iyz="0.000000" izz="0.000672"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://pal_gripper_description/meshes/gripper_link.stl"/>
      </geometry>
      <material name="White"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://pal_gripper_description/meshes/gripper_link_collision.stl"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="gripper_link">
    <material>Gazebo/White</material>
  </gazebo>
  <joint name="gripper_joint" type="fixed">
    <origin rpy="1.57079632679 1.57079632679 -1.57079632679" xyz="0.01 0 0"/>
    <parent link="arm_tool_link"/>
    <child link="gripper_link"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="gripper_right_finger_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.004470 0.010766 -0.121013"/>
      <mass value="0.109980"/>
      <inertia ixx="0.000232" ixy="-0.000005" ixz="-0.000032" iyy="0.000224" iyz="0.000036" izz="0.000029"/>
    </inertial>
    <visual>
      <origin rpy="0 0 3.1415" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://pal_gripper_description/meshes/gripper_finger_link.stl" scale="-1 -1 1"/>
      </geometry>
      <material name="White"/>
    </visual>
    <collision>
      <origin rpy="0 0 3.1415" xyz="0.005 0 -0.165"/>
      <geometry>
        <box size="0.01 0.05 0.11"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="gripper_right_finger_link">
    <material>Gazebo/White</material>
  </gazebo>
  <joint name="gripper_right_finger_joint" type="prismatic">
    <origin rpy="0 0 0" xyz="0. 0 0"/>
    <parent link="gripper_link"/>
    <child link="gripper_right_finger_link"/>
    <axis xyz="1 0 0"/>
    <limit effort="16" lower="0.0" upper="0.045" velocity="0.05"/>
    <calibration rising="0.0"/>
    <dynamics damping="80"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="0.001" soft_upper_limit="0.044"/>
  </joint>
  <link name="gripper_left_finger_link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.004470 0.010766 -0.121013"/>
      <mass value="0.109980"/>
      <inertia ixx="0.000232" ixy="-0.000005" ixz="-0.000032" iyy="0.000224" iyz="0.000036" izz="0.000029"/>
    </inertial>
    <visual>
      <origin rpy="0 0 3.1415" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://pal_gripper_description/meshes/gripper_finger_link.stl"/>
      </geometry>
      <material name="White"/>
    </visual>
    <collision>
      <origin rpy="0 0 3.1415" xyz="-0.005 0 -0.165"/>
      <geometry>
        <box size="0.01 0.05 0.11"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="gripper_left_finger_link">
    <material>Gazebo/White</material>
  </gazebo>
  <joint name="gripper_left_finger_joint" type="prismatic">
    <origin rpy="0 0 0" xyz="0. 0 0"/>
    <parent link="gripper_link"/>
    <child link="gripper_left_finger_link"/>
    <axis xyz="-1 0 0"/>
    <limit effort="16" lower="0" upper="0.045" velocity="0.05"/>
    <calibration rising="0.0"/>
    <dynamics damping="80"/>
    <safety_controller k_position="20" k_velocity="20" soft_lower_limit="0.001" soft_upper_limit="0.044"/>
  </joint>
  <!-- Grasping frame -->
  <joint name="gripper_grasping_frame_joint" type="fixed">
    <parent link="gripper_link"/>
    <child link="gripper_grasping_frame"/>
    <origin rpy="-1.5708 1.5708 0" xyz="0 0 -0.12"/>
  </joint>
  <link name="gripper_grasping_frame">
    <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>
  <transmission name="gripper_left_finger_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="gripper_left_finger_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="gripper_left_finger_joint">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
  </transmission>
  <transmission name="gripper_right_finger_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <actuator name="gripper_right_finger_motor">
      <mechanicalReduction>1.0</mechanicalReduction>
    </actuator>
    <joint name="gripper_right_finger_joint">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
  </transmission>
  <gazebo reference="gripper_left_finger_link">
    <kp>1000000.0</kp>
    <kd>1.0</kd>
    <mu1>1.0</mu1>
    <mu2>1.0</mu2>
    <fdir1>0 0 0</fdir1>
    <maxVel>1.0</maxVel>
    <minDepth>0.1</minDepth>
    <material>Gazebo/Black</material>
  </gazebo>
  <gazebo reference="gripper_left_finger_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
    <provideFeedback>1</provideFeedback>
  </gazebo>
  <gazebo reference="gripper_right_finger_link">
    <kp>1000000.0</kp>
    <kd>1.0</kd>
    <mu1>1.0</mu1>
    <mu2>1.0</mu2>
    <fdir1>0 0 0</fdir1>
    <maxVel>1.0</maxVel>
    <minDepth>0.1</minDepth>
    <material>Gazebo/Black</material>
  </gazebo>
  <gazebo reference="gripper_right_finger_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
    <provideFeedback>1</provideFeedback>
  </gazebo>
</robot>