<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from tiago.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://ros.org/wiki/xacro">
  <material name="Hey5DarkGrey">
    <color rgba="0.1 0.1 0.1 1.0"/>
  </material>
  <material name="Black">
    <color rgba="0 0 0 1"/>
  </material>
  <material name="FlatBlack">
    <color rgba="0.1 0.1 0.1 1"/>
  </material>
  <material name="Orange">
    <color rgba="1 0.5088 0.0468 1"/>
  </material>
  <!--TODO: Check-->
  <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>
  <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>
  <!-- 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://example-robot-data/robots/tiago_description/meshes/pmb2/base/base.stl"/>
      </geometry>
      <material name="White"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/tiago_description/meshes/pmb2/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">
    <inertial>
      <mass value="0.0"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.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>
  <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://example-robot-data/robots/tiago_description/meshes/pmb2/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://example-robot-data/robots/tiago_description/meshes/pmb2/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://example-robot-data/robots/tiago_description/meshes/pmb2/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 3.14159265359 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://example-robot-data/robots/tiago_description/meshes/pmb2/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>
  <joint name="wheel_right_joint" type="continuous">
    <parent link="base_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 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>
    </joint>
  </transmission>
  <!-- Contact model for the wheel surface -->
  <gazebo reference="wheel_right_link">
    <!--
      <kp>1000000.0</kp>
      <kd>100.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://example-robot-data/robots/tiago_description/meshes/pmb2/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>
  <joint name="wheel_left_joint" type="continuous">
    <parent link="base_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 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>
    </joint>
  </transmission>
  <!-- Contact model for the wheel surface -->
  <gazebo reference="wheel_left_link">
    <!--
      <kp>1000000.0</kp>
      <kd>100.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="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://example-robot-data/robots/tiago_description/meshes/pmb2/wheels/caster_1.stl"/>
      </geometry>
      <material name="Grey"/>
    </visual>
  </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://example-robot-data/robots/tiago_description/meshes/pmb2/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="fixed">
    <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="fixed">
    <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>
  <!-- Transmission -->
  <!--xacro:caster_transmission name="" side="${side}" number="" reduction=""-->
  <!-- Contact model for the caster wheel surface -->
  <gazebo reference="caster_front_right_1_link">
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>0</mu1>
    <mu2>0</mu2>
    <fdir1>1 0 0</fdir1>
    <maxVel>1.0</maxVel>
    <minDepth>0.00</minDepth>
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="caster_front_right_2_link">
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>0</mu1>
    <mu2>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="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://example-robot-data/robots/tiago_description/meshes/pmb2/wheels/caster_1.stl"/>
      </geometry>
      <material name="Grey"/>
    </visual>
  </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://example-robot-data/robots/tiago_description/meshes/pmb2/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="fixed">
    <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="fixed">
    <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>
  <!-- Transmission -->
  <!--xacro:caster_transmission name="" side="${side}" number="" reduction=""-->
  <!-- Contact model for the caster wheel surface -->
  <gazebo reference="caster_front_left_1_link">
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>0</mu1>
    <mu2>0</mu2>
    <fdir1>1 0 0</fdir1>
    <maxVel>1.0</maxVel>
    <minDepth>0.00</minDepth>
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="caster_front_left_2_link">
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>0</mu1>
    <mu2>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="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://example-robot-data/robots/tiago_description/meshes/pmb2/wheels/caster_1.stl"/>
      </geometry>
      <material name="Grey"/>
    </visual>
  </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://example-robot-data/robots/tiago_description/meshes/pmb2/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="fixed">
    <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="fixed">
    <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>
  <!-- Transmission -->
  <!--xacro:caster_transmission name="" side="${side}" number="" reduction=""-->
  <!-- Contact model for the caster wheel surface -->
  <gazebo reference="caster_back_right_1_link">
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>0</mu1>
    <mu2>0</mu2>
    <fdir1>1 0 0</fdir1>
    <maxVel>1.0</maxVel>
    <minDepth>0.00</minDepth>
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="caster_back_right_2_link">
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>0</mu1>
    <mu2>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="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://example-robot-data/robots/tiago_description/meshes/pmb2/wheels/caster_1.stl"/>
      </geometry>
      <material name="Grey"/>
    </visual>
  </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://example-robot-data/robots/tiago_description/meshes/pmb2/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="fixed">
    <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="fixed">
    <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>
  <!-- Transmission -->
  <!--xacro:caster_transmission name="" side="${side}" number="" reduction=""-->
  <!-- Contact model for the caster wheel surface -->
  <gazebo reference="caster_back_left_1_link">
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>0</mu1>
    <mu2>0</mu2>
    <fdir1>1 0 0</fdir1>
    <maxVel>1.0</maxVel>
    <minDepth>0.00</minDepth>
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="caster_back_left_2_link">
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>0</mu1>
    <mu2>0</mu2>
    <fdir1>1 0 0</fdir1>
    <maxVel>1.0</maxVel>
    <minDepth>0.00</minDepth>
    <implicitSpringDamper>1</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://example-robot-data/robots/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://example-robot-data/robots/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://example-robot-data/robots/tiago_description/meshes/torso/torso_lift_collision_core.dae" scale="1 1 1"/>
      </geometry>
      <material name="LightGrey"/>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/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://example-robot-data/robots/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://example-robot-data/robots/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://example-robot-data/robots/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://example-robot-data/robots/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://example-robot-data/robots/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.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">
      <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://example-robot-data/robots/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.047 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>
  <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_1_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="0.063989 0.022546 -0.014608"/>
      <mass value="2.083078"/>
      <inertia ixx="0.003060504572445" ixy="0.002364723186022" ixz="0.001495245689439" iyy="0.008630326004235" iyz="0.000522540423519" izz="0.009347757530069"/>
    </inertial>
    <!-- M90 module -->
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/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://example-robot-data/robots/tiago_description/meshes/arm/arm_1_collision.dae" scale="1 1 1"/>
      </geometry>
    </collision>
  </link>
  <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.07" soft_upper_limit="2.67889357189"/>
  </joint>
  <link name="arm_2_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="0.035264 0.000327 0.008489"/>
      <mass value="1.792499"/>
      <inertia ixx="0.001683" ixy="-0.000010" ixz="-0.000387" iyy="0.004708" iyz="-0.000028" izz="0.004780"/>
    </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://example-robot-data/robots/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://example-robot-data/robots/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.50079632679" soft_upper_limit="1.0208307825"/>
  </joint>
  <link name="arm_3_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="-0.007956 0.005368 -0.136320"/>
      <mass value="2.275860"/>
      <inertia ixx="0.018178" ixy="-0.000143" ixz="0.001379" iyy="0.018552" iyz="-0.000888" izz="0.002774"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/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://example-robot-data/robots/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.46429173529" soft_upper_limit="1.50079632679"/>
  </joint>
  <!--************************-->
  <!--        ELBOW           -->
  <!--************************-->
  <link name="arm_4_link">
    <inertial>
      <origin rpy="0.00000 0.00000 0.00000" xyz="-0.095658 0.014666 0.018133"/>
      <mass value="1.892788"/>
      <inertia ixx="0.002542" ixy="-0.000956" ixz="-0.001457" iyy="0.011789" iyz="0.000229" izz="0.012058"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/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://example-robot-data/robots/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.322699081699" soft_upper_limit="2.28619449019"/>
  </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           -->
  <!--************************-->
  <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://example-robot-data/robots/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://example-robot-data/robots/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://example-robot-data/robots/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://example-robot-data/robots/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.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_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>
  <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">
      <offset>0.0</offset>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
      <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/JointStateInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <ignoreTransmissionAbsoluteEncoder>1</ignoreTransmissionAbsoluteEncoder>
    </joint>
    <joint name="arm_7_joint">
      <role>joint2</role>
      <offset>0.0</offset>
      <mechanicalReduction>-1</mechanicalReduction>
      <hardwareInterface>hardware_interface/JointStateInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <ignoreTransmissionAbsoluteEncoder>1</ignoreTransmissionAbsoluteEncoder>
    </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>
  <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">
      <offset>0.0</offset>
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/JointStateInterface</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">
      <offset>0.0</offset>
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/JointStateInterface</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">
      <offset>0.0</offset>
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
      <hardwareInterface>hardware_interface/JointStateInterface</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">
      <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_ft_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 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_ft_joint" type="fixed">
    <parent link="arm_tool_link"/>
    <child link="wrist_ft_link"/>
    <origin rpy="1.57079632679 0 1.57079632679" xyz="0.00785 0 0"/>
  </joint>
  <!--***********************-->
  <!--       FT TOOL         -->
  <!--***********************-->
  <link name="wrist_ft_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.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_tool_joint" type="fixed">
    <parent link="wrist_ft_link"/>
    <child link="wrist_ft_tool_link"/>
    <origin rpy="-1.57079632679 -1.57079632679 0" xyz="0 0 0.012725"/>
  </joint>
  <!-- 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>
