pr2.urdf

<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from pr2.urdf.xacro                 | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="pr2" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#slider" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:xacro="http://ros.org/wiki/xacro">
  <!-- The following included files set up definitions of parts of the robot body -->
  <!-- misc common stuff? -->
  <!-- Gear Ratio Corrections: Divide transmission reductions by these factors (Nominally 1)-->
  <!-- ********** Right Arm ********** -->
  <!-- Flag offsets: Add to the optical flag locations (Nominally 0) -->
  <!-- ********** Head ********** -->
  <!-- Head Location: Add to the transform from torso to head_pan_link (Nominally 0) -->
  <!-- Flag Offsets -->
  <!-- Casters -->
  <!-- Stereo Camera Location (Nominally 0) -->
  <!-- Kinect Camera Location (Nominally 0) -->
  <!-- HighDef Camera Location (Nominally 0) -->
  <!-- Throwing in left arm constants to appease the xacro parser -->
  <!-- PR2 Arm -->
  <!-- Things that are needed only for Gazebo (not the physical robot).  These include
       sensor and controller plugin specifications -->
  <!-- Upperarm roll: internal fixed attchment point for upper arm -->
  <!-- Gear Ratio Corrections: Divide transmission reductions by these factors (Nominally 1)-->
  <!-- ********** Right Arm ********** -->
  <!-- Flag offsets: Add to the optical flag locations (Nominally 0) -->
  <!-- ********** Head ********** -->
  <!-- Head Location: Add to the transform from torso to head_pan_link (Nominally 0) -->
  <!-- Flag Offsets -->
  <!-- Casters -->
  <!-- Stereo Camera Location (Nominally 0) -->
  <!-- Kinect Camera Location (Nominally 0) -->
  <!-- HighDef Camera Location (Nominally 0) -->
  <!-- Throwing in left arm constants to appease the xacro parser -->
  <!--TODO Define and give source-->
  <!--TODO Define and give source-->
  <!-- ============================   Shoulder   ============================ -->
  <!-- Upperarm roll: internal fixed attchment point for upper arm -->
  <!-- Things that are needed only for Gazebo (not the physical robot).  These include
       sensor and controller plugin specifications -->
  <!-- Elbow flex -->
  <!-- Forearm roll -->
  <!-- ============================   Upper Arm   ============================ -->
  <!-- Forearm roll -->
  <!-- Gear Ratio Corrections: Divide transmission reductions by these factors (Nominally 1)-->
  <!-- ********** Right Arm ********** -->
  <!-- Flag offsets: Add to the optical flag locations (Nominally 0) -->
  <!-- ********** Head ********** -->
  <!-- Head Location: Add to the transform from torso to head_pan_link (Nominally 0) -->
  <!-- Flag Offsets -->
  <!-- Casters -->
  <!-- Stereo Camera Location (Nominally 0) -->
  <!-- Kinect Camera Location (Nominally 0) -->
  <!-- HighDef Camera Location (Nominally 0) -->
  <!-- Throwing in left arm constants to appease the xacro parser -->
  <!-- Inertial properties differ slightly for left vs right arm -->
  <!-- ============================   Upper Arm   ============================ -->
  <!-- Includes elbow flex, FA roll joints in macros below -->
  <!-- FA roll joint only -->
  <!-- Things that are needed only for Gazebo (not the physical robot).  These include
       sensor and controller plugin specifications -->
  <!-- Gear Ratio Corrections: Divide transmission reductions by these factors (Nominally 1)-->
  <!-- ********** Right Arm ********** -->
  <!-- Flag offsets: Add to the optical flag locations (Nominally 0) -->
  <!-- ********** Head ********** -->
  <!-- Head Location: Add to the transform from torso to head_pan_link (Nominally 0) -->
  <!-- Flag Offsets -->
  <!-- Casters -->
  <!-- Stereo Camera Location (Nominally 0) -->
  <!-- Kinect Camera Location (Nominally 0) -->
  <!-- HighDef Camera Location (Nominally 0) -->
  <!-- Throwing in left arm constants to appease the xacro parser -->
  <!-- ============================   Forearm   ============================ -->
  <!-- PR2 gripper -->
  <!-- PR2 head -->
  <!-- PR2 tilting laser mount -->
  <!-- PR2 torso -->
  <!-- PR2 base -->
  <!--  Caster wheel transmission   -->
  <!-- Hub transmission only -->
  <!-- DATA SOURCES -->
  <!-- all link offsets, CG, limits are obtained from Function Engineering spreadsheet 090224_link_data.xls unless stated otherwise -->
  <!-- all link geometry sizes are obtained from Function provided CAD model unless stated otherwise -->
  <!-- all simplified collision geometry are hand approximated from CAD model, sometimes from respective bounding boxes -->
  <!-- This is the 'effective' wheel radius. Wheel radius for uncompressed wheel is 0.079.  mp 20080801 -->
  <!-- simplified box collision geometry for base -->
  <!-- simplified box collision geometry for hokuyo laser -->
  <!--                                                      -->
  <!--           wheel                                      -->
  <!--                                                      -->
  <!-- Macro for PR2 Caster hub only -->
  <!-- The xacro macro xacro:pr2_base contains: base, casters and wheels -->
  <!-- Head sensors -->
  <!-- This urdf macro defines following sensors:
       prosilica
       double stereo camera
  -->
  <!-- Made by Kevin for A2 sensor package -->
  <!-- Origin is center mount screw on sensor plate mount -->
  <!-- When mounted properly, should have same origin as head plate frame -->
  <!-- Made by Kevin for double stereo camera for PR-2 Beta. -->
  <!-- Needs calibration verification, and CAD check. -->
  <!-- stereo camera macro uses wge100_camera macros -->
  <!-- hack_baseline is used to simulate right stereo camera projection matrix translation to left stereo camera frame, currently on the hardware,
       custom left stereo camera frame_id is passed to right stereo wge100 camera node at launch time, baseline is stored on the camera. -->
  <!-- this macro is used for creating wide and narrow double stereo camera in simulation -->
  <!-- +y to the left -->
  <!-- this macro is used for creating wide and narrow double stereo camera links -->
  <!-- Made by Kevin for double stereo camera for PR-2 Beta. -->
  <!-- Needs calibration verification, and CAD check. -->
  <!-- Made by Kevin for A2 sensor package -->
  <!-- Origin is center mount screw on sensor plate mount -->
  <!-- When mounted properly, should have same origin as head plate frame -->
  <!-- Camera sensors -->
  <!-- hack_baseline is used to simulate right stereo camera projection matrix translation to left stereo camera frame, currently on the hardware,
       custom left stereo camera frame_id is passed to right stereo wge100 camera node at launch time, baseline is stored on the camera. -->
  <!-- Texture projector -->
  <!-- this controller is pushed into a body scope (previously model scope)
     this will only work with added r36415 patch in simulator_gazebo stack
     otherwise projector will not work without removing the reference tag -->
  <!-- generic simulator_gazebo plugins for starting mechanism control, ros time, ros battery -->
  <gazebo>
    <plugin filename="libgazebo_ros_controller_manager.so" name="gazebo_ros_controller_manager">
      <alwaysOn>true</alwaysOn>
      <updateRate>1000.0</updateRate>
    </plugin>
    <plugin filename="libgazebo_ros_power_monitor.so" name="gazebo_ros_power_monitor_controller">
      <alwaysOn>true</alwaysOn>
      <updateRate>1.0</updateRate>
      <timeout>5</timeout>
      <powerStateTopic>power_state</powerStateTopic>
      <powerStateRate>10.0</powerStateRate>
      <fullChargeCapacity>87.78</fullChargeCapacity>
      <dischargeRate>-474</dischargeRate>
      <chargeRate>525</chargeRate>
      <dischargeVoltage>15.52</dischargeVoltage>
      <chargeVoltage>16.41</chargeVoltage>
    </plugin>
  </gazebo>
  <!-- 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="Grey2">
    <color rgba="0.9 0.9 0.9 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="Black">
    <color rgba="0.1 0.1 0.1 1.0"/>
  </material>
  <material name="LightGrey">
    <color rgba="0.6 0.6 0.6 1.0"/>
  </material>
  <material name="Caster">
    <texture filename="materials/textures/pr2_caster_texture.png"/>
  </material>
  <material name="Wheel_l">
    <texture filename="materials/textures/pr2_wheel_left.png"/>
  </material>
  <material name="Wheel_r">
    <texture filename="materials/textures/pr2_wheel_right.png"/>
  </material>
  <material name="RollLinks">
    <texture filename="materials/textures/pr2_wheel_left.png"/>
  </material>
  <!-- Now we can start using the macros included above to define the actual PR2 -->
  <!-- The first use of a macro.  This one was defined in base.urdf.xacro above.
       A macro like this will expand to a set of link and joint definitions, and to additional
       Gazebo-related extensions (sensor plugins, etc).  The macro takes an argument, name,
       that equals "base", and uses it to generate names for its component links and joints
       (e.g., base_link).  The included origin block is also an argument to the macro.  By convention,
       the origin block defines where the component is w.r.t its parent (in this case the parent
       is the world frame). For more, see http://www.ros.org/wiki/xacro -->
  <link name="base_link">
    <inertial>
      <mass value="116.0"/>
      <origin xyz="-0.061 0.0 0.1465"/>
      <inertia ixx="5.652232699207" ixy="-0.009719934438" ixz="1.293988226423" iyy="5.669473158652" iyz="-0.007379583694" izz="3.683196351726"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/base_v0/base.dae"/>
      </geometry>
      <material name="White"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/base_v0/base_L.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- base_footprint is a fictitious link(frame) that is on the ground right below base_link origin,
         navigation stack dedpends on this frame -->
  <link name="base_footprint">
    <inertial>
      <mass value="1.0"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.01 0.01 0.01"/>
      </geometry>
      <material name="White"/>
    </visual>
    <collision>
      <!-- represent base collision with a simple rectangular model, positioned by base_size_z s.t. top
             surface of the collision box matches the top surface of the PR2 base -->
      <origin rpy="0 0 0" xyz="0 0 0.071"/>
      <geometry>
        <box size="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="base_footprint_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.051"/>
    <child link="base_link"/>
    <parent link="base_footprint"/>
  </joint>
  <!-- visualize bellow -->
  <link name="base_bellow_link">
    <inertial>
      <mass value="1.0"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.05 0.37 0.3"/>
      </geometry>
      <material name="Black"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.05 0.37 0.3"/>
      </geometry>
    </collision>
  </link>
  <joint name="base_bellow_joint" type="fixed">
    <origin rpy="0 0 0" xyz="-0.29 0 0.8"/>
    <parent link="base_link"/>
    <child link="base_bellow_link"/>
  </joint>
  <!-- base laser -->
  <joint name="base_laser_joint" type="fixed">
    <axis xyz="0 1 0"/>
    <origin rpy="0 0 0" xyz="0.275 0.0 0.252"/>
    <parent link="base_link"/>
    <child link="base_laser_link"/>
  </joint>
  <link name="base_laser_link" type="laser">
    <inertial>
      <mass value="0.001"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.0001"/>
    </inertial>
  </link>
  <!-- gazebo extensions -->
  <gazebo reference="base_laser_link">
    <sensor name="base_laser" type="ray">
      <always_on>true</always_on>
      <update_rate>20</update_rate>
      <pose>0 0 0 0 0 0</pose>
      <visualize>false</visualize>
      <ray>
        <scan>
          <horizontal>
            <samples>640</samples>
            <resolution>1</resolution>
            <min_angle>-2.2689</min_angle>
            <max_angle>2.2689</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.08</min>
          <max>10.0</max>
          <resolution>0.01</resolution>
        </range>
      </ray>
      <plugin filename="libgazebo_ros_laser.so" name="gazebo_ros_base_laser_controller">
        <gaussianNoise>0.005</gaussianNoise>
        <alwaysOn>true</alwaysOn>
        <updateRate>20</updateRate>
        <topicName>base_scan</topicName>
        <frameName>base_laser_link</frameName>
        <hokuyoMinIntensity>101</hokuyoMinIntensity>
      </plugin>
    </sensor>
  </gazebo>
  <!-- all four caster macros -->
  <joint name="fl_caster_rotation_joint" type="continuous">
    <axis xyz="0 0 1"/>
    <limit effort="6.5" velocity="10"/>
    <!-- alpha tested velocity and effort limits -->
    <safety_controller k_velocity="10"/>
    <calibration rising="-0.785398163397"/>
    <dynamics damping="1.0" friction="0.0"/>
    <origin rpy="0 0 0" xyz="0.2246 0.2246 0.0282"/>
    <parent link="base_link"/>
    <child link="fl_caster_rotation_link"/>
  </joint>
  <link name="fl_caster_rotation_link">
    <inertial>
      <mass value="3.473082"/>
      <origin xyz="0 0 0.07"/>
      <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/base_v0/caster.stl"/>
      </geometry>
      <material name="Caster"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/base_v0/caster_L.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- extensions -->
  <transmission name="fl_caster_rotation_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="fl_caster_rotation_motor"/>
    <joint name="fl_caster_rotation_joint"/>
    <mechanicalReduction>-79.2380952381</mechanicalReduction>
  </transmission>
  <!-- wheel macros -->
  <joint name="fl_caster_l_wheel_joint" type="continuous">
    <axis xyz="0 1 0"/>
    <limit effort="7" velocity="15"/>
    <!-- alpha tested effort and velocity limits -->
    <safety_controller k_velocity="10"/>
    <dynamics damping="1.0" friction="0.0"/>
    <origin rpy="0 0 0" xyz="0 0.049 0"/>
    <parent link="fl_caster_rotation_link"/>
    <child link="fl_caster_l_wheel_link"/>
  </joint>
  <link name="fl_caster_l_wheel_link">
    <inertial>
      <mass value="0.44036"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/base_v0/wheel.dae"/>
      </geometry>
      <material name="Wheel_l"/>
    </visual>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
      <!-- rotation because cyl. geom primitive has symmetry axis in +x direction -->
      <geometry>
        <cylinder length="0.034" radius="0.074792"/>
      </geometry>
    </collision>
  </link>
  <!-- extensions -->
  <gazebo reference="fl_caster_l_wheel_link">
    <mu1 value="100.0"/>
    <mu2 value="100.0"/>
    <kp value="1000000.0"/>
    <kd value="1.0"/>
    <maxVel value="100.0"/>
    <minDepth value="0.0"/>
  </gazebo>
  <transmission name="fl_caster_l_wheel_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="fl_caster_l_wheel_motor"/>
    <joint name="fl_caster_l_wheel_joint"/>
    <mechanicalReduction>79.2380952381</mechanicalReduction>
  </transmission>
  <joint name="fl_caster_r_wheel_joint" type="continuous">
    <axis xyz="0 1 0"/>
    <limit effort="7" velocity="15"/>
    <!-- alpha tested effort and velocity limits -->
    <safety_controller k_velocity="10"/>
    <dynamics damping="1.0" friction="0.0"/>
    <origin rpy="0 0 0" xyz="0 -0.049 0"/>
    <parent link="fl_caster_rotation_link"/>
    <child link="fl_caster_r_wheel_link"/>
  </joint>
  <link name="fl_caster_r_wheel_link">
    <inertial>
      <mass value="0.44036"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/base_v0/wheel.dae"/>
      </geometry>
      <material name="Wheel_r"/>
    </visual>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
      <!-- rotation because cyl. geom primitive has symmetry axis in +x direction -->
      <geometry>
        <cylinder length="0.034" radius="0.074792"/>
      </geometry>
    </collision>
  </link>
  <!-- extensions -->
  <gazebo reference="fl_caster_r_wheel_link">
    <mu1 value="100.0"/>
    <mu2 value="100.0"/>
    <kp value="1000000.0"/>
    <kd value="1.0"/>
    <maxVel value="100.0"/>
    <minDepth value="0.0"/>
  </gazebo>
  <transmission name="fl_caster_r_wheel_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="fl_caster_r_wheel_motor"/>
    <joint name="fl_caster_r_wheel_joint"/>
    <mechanicalReduction>-79.2380952381</mechanicalReduction>
  </transmission>
  <!-- extensions -->
  <gazebo reference="fl_caster_rotation_link">
    <material value="PR2/caster_texture"/>
  </gazebo>
  <joint name="fr_caster_rotation_joint" type="continuous">
    <axis xyz="0 0 1"/>
    <limit effort="6.5" velocity="10"/>
    <!-- alpha tested velocity and effort limits -->
    <safety_controller k_velocity="10"/>
    <calibration rising="-0.785398163397"/>
    <dynamics damping="1.0" friction="0.0"/>
    <origin rpy="0 0 0" xyz="0.2246 -0.2246 0.0282"/>
    <parent link="base_link"/>
    <child link="fr_caster_rotation_link"/>
  </joint>
  <link name="fr_caster_rotation_link">
    <inertial>
      <mass value="3.473082"/>
      <origin xyz="0 0 0.07"/>
      <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/base_v0/caster.stl"/>
      </geometry>
      <material name="Caster"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/base_v0/caster_L.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- extensions -->
  <transmission name="fr_caster_rotation_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="fr_caster_rotation_motor"/>
    <joint name="fr_caster_rotation_joint"/>
    <mechanicalReduction>-79.2380952381</mechanicalReduction>
  </transmission>
  <!-- wheel macros -->
  <joint name="fr_caster_l_wheel_joint" type="continuous">
    <axis xyz="0 1 0"/>
    <limit effort="7" velocity="15"/>
    <!-- alpha tested effort and velocity limits -->
    <safety_controller k_velocity="10"/>
    <dynamics damping="1.0" friction="0.0"/>
    <origin rpy="0 0 0" xyz="0 0.049 0"/>
    <parent link="fr_caster_rotation_link"/>
    <child link="fr_caster_l_wheel_link"/>
  </joint>
  <link name="fr_caster_l_wheel_link">
    <inertial>
      <mass value="0.44036"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/base_v0/wheel.dae"/>
      </geometry>
      <material name="Wheel_l"/>
    </visual>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
      <!-- rotation because cyl. geom primitive has symmetry axis in +x direction -->
      <geometry>
        <cylinder length="0.034" radius="0.074792"/>
      </geometry>
    </collision>
  </link>
  <!-- extensions -->
  <gazebo reference="fr_caster_l_wheel_link">
    <mu1 value="100.0"/>
    <mu2 value="100.0"/>
    <kp value="1000000.0"/>
    <kd value="1.0"/>
    <maxVel value="100.0"/>
    <minDepth value="0.0"/>
  </gazebo>
  <transmission name="fr_caster_l_wheel_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="fr_caster_l_wheel_motor"/>
    <joint name="fr_caster_l_wheel_joint"/>
    <mechanicalReduction>79.2380952381</mechanicalReduction>
  </transmission>
  <joint name="fr_caster_r_wheel_joint" type="continuous">
    <axis xyz="0 1 0"/>
    <limit effort="7" velocity="15"/>
    <!-- alpha tested effort and velocity limits -->
    <safety_controller k_velocity="10"/>
    <dynamics damping="1.0" friction="0.0"/>
    <origin rpy="0 0 0" xyz="0 -0.049 0"/>
    <parent link="fr_caster_rotation_link"/>
    <child link="fr_caster_r_wheel_link"/>
  </joint>
  <link name="fr_caster_r_wheel_link">
    <inertial>
      <mass value="0.44036"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/base_v0/wheel.dae"/>
      </geometry>
      <material name="Wheel_r"/>
    </visual>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
      <!-- rotation because cyl. geom primitive has symmetry axis in +x direction -->
      <geometry>
        <cylinder length="0.034" radius="0.074792"/>
      </geometry>
    </collision>
  </link>
  <!-- extensions -->
  <gazebo reference="fr_caster_r_wheel_link">
    <mu1 value="100.0"/>
    <mu2 value="100.0"/>
    <kp value="1000000.0"/>
    <kd value="1.0"/>
    <maxVel value="100.0"/>
    <minDepth value="0.0"/>
  </gazebo>
  <transmission name="fr_caster_r_wheel_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="fr_caster_r_wheel_motor"/>
    <joint name="fr_caster_r_wheel_joint"/>
    <mechanicalReduction>-79.2380952381</mechanicalReduction>
  </transmission>
  <!-- extensions -->
  <gazebo reference="fr_caster_rotation_link">
    <material value="PR2/caster_texture"/>
  </gazebo>
  <joint name="bl_caster_rotation_joint" type="continuous">
    <axis xyz="0 0 1"/>
    <limit effort="6.5" velocity="10"/>
    <!-- alpha tested velocity and effort limits -->
    <safety_controller k_velocity="10"/>
    <calibration rising="2.35619449019"/>
    <dynamics damping="1.0" friction="0.0"/>
    <origin rpy="0 0 0" xyz="-0.2246 0.2246 0.0282"/>
    <parent link="base_link"/>
    <child link="bl_caster_rotation_link"/>
  </joint>
  <link name="bl_caster_rotation_link">
    <inertial>
      <mass value="3.473082"/>
      <origin xyz="0 0 0.07"/>
      <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/base_v0/caster.stl"/>
      </geometry>
      <material name="Caster"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/base_v0/caster_L.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- extensions -->
  <transmission name="bl_caster_rotation_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="bl_caster_rotation_motor"/>
    <joint name="bl_caster_rotation_joint"/>
    <mechanicalReduction>-79.2380952381</mechanicalReduction>
  </transmission>
  <!-- wheel macros -->
  <joint name="bl_caster_l_wheel_joint" type="continuous">
    <axis xyz="0 1 0"/>
    <limit effort="7" velocity="15"/>
    <!-- alpha tested effort and velocity limits -->
    <safety_controller k_velocity="10"/>
    <dynamics damping="1.0" friction="0.0"/>
    <origin rpy="0 0 0" xyz="0 0.049 0"/>
    <parent link="bl_caster_rotation_link"/>
    <child link="bl_caster_l_wheel_link"/>
  </joint>
  <link name="bl_caster_l_wheel_link">
    <inertial>
      <mass value="0.44036"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/base_v0/wheel.dae"/>
      </geometry>
      <material name="Wheel_l"/>
    </visual>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
      <!-- rotation because cyl. geom primitive has symmetry axis in +x direction -->
      <geometry>
        <cylinder length="0.034" radius="0.074792"/>
      </geometry>
    </collision>
  </link>
  <!-- extensions -->
  <gazebo reference="bl_caster_l_wheel_link">
    <mu1 value="100.0"/>
    <mu2 value="100.0"/>
    <kp value="1000000.0"/>
    <kd value="1.0"/>
    <maxVel value="100.0"/>
    <minDepth value="0.0"/>
  </gazebo>
  <transmission name="bl_caster_l_wheel_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="bl_caster_l_wheel_motor"/>
    <joint name="bl_caster_l_wheel_joint"/>
    <mechanicalReduction>79.2380952381</mechanicalReduction>
  </transmission>
  <joint name="bl_caster_r_wheel_joint" type="continuous">
    <axis xyz="0 1 0"/>
    <limit effort="7" velocity="15"/>
    <!-- alpha tested effort and velocity limits -->
    <safety_controller k_velocity="10"/>
    <dynamics damping="1.0" friction="0.0"/>
    <origin rpy="0 0 0" xyz="0 -0.049 0"/>
    <parent link="bl_caster_rotation_link"/>
    <child link="bl_caster_r_wheel_link"/>
  </joint>
  <link name="bl_caster_r_wheel_link">
    <inertial>
      <mass value="0.44036"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/base_v0/wheel.dae"/>
      </geometry>
      <material name="Wheel_r"/>
    </visual>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
      <!-- rotation because cyl. geom primitive has symmetry axis in +x direction -->
      <geometry>
        <cylinder length="0.034" radius="0.074792"/>
      </geometry>
    </collision>
  </link>
  <!-- extensions -->
  <gazebo reference="bl_caster_r_wheel_link">
    <mu1 value="100.0"/>
    <mu2 value="100.0"/>
    <kp value="1000000.0"/>
    <kd value="1.0"/>
    <maxVel value="100.0"/>
    <minDepth value="0.0"/>
  </gazebo>
  <transmission name="bl_caster_r_wheel_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="bl_caster_r_wheel_motor"/>
    <joint name="bl_caster_r_wheel_joint"/>
    <mechanicalReduction>-79.2380952381</mechanicalReduction>
  </transmission>
  <!-- extensions -->
  <gazebo reference="bl_caster_rotation_link">
    <material value="PR2/caster_texture"/>
  </gazebo>
  <joint name="br_caster_rotation_joint" type="continuous">
    <axis xyz="0 0 1"/>
    <limit effort="6.5" velocity="10"/>
    <!-- alpha tested velocity and effort limits -->
    <safety_controller k_velocity="10"/>
    <calibration rising="2.35619449019"/>
    <dynamics damping="1.0" friction="0.0"/>
    <origin rpy="0 0 0" xyz="-0.2246 -0.2246 0.0282"/>
    <parent link="base_link"/>
    <child link="br_caster_rotation_link"/>
  </joint>
  <link name="br_caster_rotation_link">
    <inertial>
      <mass value="3.473082"/>
      <origin xyz="0 0 0.07"/>
      <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/base_v0/caster.stl"/>
      </geometry>
      <material name="Caster"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/base_v0/caster_L.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- extensions -->
  <transmission name="br_caster_rotation_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="br_caster_rotation_motor"/>
    <joint name="br_caster_rotation_joint"/>
    <mechanicalReduction>-79.2380952381</mechanicalReduction>
  </transmission>
  <!-- wheel macros -->
  <joint name="br_caster_l_wheel_joint" type="continuous">
    <axis xyz="0 1 0"/>
    <limit effort="7" velocity="15"/>
    <!-- alpha tested effort and velocity limits -->
    <safety_controller k_velocity="10"/>
    <dynamics damping="1.0" friction="0.0"/>
    <origin rpy="0 0 0" xyz="0 0.049 0"/>
    <parent link="br_caster_rotation_link"/>
    <child link="br_caster_l_wheel_link"/>
  </joint>
  <link name="br_caster_l_wheel_link">
    <inertial>
      <mass value="0.44036"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/base_v0/wheel.dae"/>
      </geometry>
      <material name="Wheel_l"/>
    </visual>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
      <!-- rotation because cyl. geom primitive has symmetry axis in +x direction -->
      <geometry>
        <cylinder length="0.034" radius="0.074792"/>
      </geometry>
    </collision>
  </link>
  <!-- extensions -->
  <gazebo reference="br_caster_l_wheel_link">
    <mu1 value="100.0"/>
    <mu2 value="100.0"/>
    <kp value="1000000.0"/>
    <kd value="1.0"/>
    <maxVel value="100.0"/>
    <minDepth value="0.0"/>
  </gazebo>
  <transmission name="br_caster_l_wheel_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="br_caster_l_wheel_motor"/>
    <joint name="br_caster_l_wheel_joint"/>
    <mechanicalReduction>79.2380952381</mechanicalReduction>
  </transmission>
  <joint name="br_caster_r_wheel_joint" type="continuous">
    <axis xyz="0 1 0"/>
    <limit effort="7" velocity="15"/>
    <!-- alpha tested effort and velocity limits -->
    <safety_controller k_velocity="10"/>
    <dynamics damping="1.0" friction="0.0"/>
    <origin rpy="0 0 0" xyz="0 -0.049 0"/>
    <parent link="br_caster_rotation_link"/>
    <child link="br_caster_r_wheel_link"/>
  </joint>
  <link name="br_caster_r_wheel_link">
    <inertial>
      <mass value="0.44036"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/base_v0/wheel.dae"/>
      </geometry>
      <material name="Wheel_r"/>
    </visual>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
      <!-- rotation because cyl. geom primitive has symmetry axis in +x direction -->
      <geometry>
        <cylinder length="0.034" radius="0.074792"/>
      </geometry>
    </collision>
  </link>
  <!-- extensions -->
  <gazebo reference="br_caster_r_wheel_link">
    <mu1 value="100.0"/>
    <mu2 value="100.0"/>
    <kp value="1000000.0"/>
    <kd value="1.0"/>
    <maxVel value="100.0"/>
    <minDepth value="0.0"/>
  </gazebo>
  <transmission name="br_caster_r_wheel_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="br_caster_r_wheel_motor"/>
    <joint name="br_caster_r_wheel_joint"/>
    <mechanicalReduction>-79.2380952381</mechanicalReduction>
  </transmission>
  <!-- extensions -->
  <gazebo reference="br_caster_rotation_link">
    <material value="PR2/caster_texture"/>
  </gazebo>
  <!-- gazebo extensions -->
  <gazebo reference="base_link">
    <selfCollide>false</selfCollide>
    <sensor name="base_contact_sensor" type="contact">
      <always_on>true</always_on>
      <update_rate>100.0</update_rate>
      <contact>
        <collision>base_link_collision</collision>
      </contact>
      <plugin filename="libgazebo_ros_bumper.so" name="base_gazebo_ros_bumper_controller">
        <alwaysOn>true</alwaysOn>
        <updateRate>100.0</updateRate>
        <bumperTopicName>base_bumper</bumperTopicName>
        <frameName>world</frameName>
      </plugin>
    </sensor>
  </gazebo>
  <gazebo reference="base_bellow_link">
    <material value="PR2/Black"/>
  </gazebo>
  <gazebo>
    <plugin filename="libgazebo_ros_p3d.so" name="p3d_base_controller">
      <alwaysOn>true</alwaysOn>
      <updateRate>100.0</updateRate>
      <bodyName>base_link</bodyName>
      <topicName>base_pose_ground_truth</topicName>
      <gaussianNoise>0.01</gaussianNoise>
      <frameName>map</frameName>
      <xyzOffsets>0 0 0</xyzOffsets>
      <rpyOffsets>0 0 0</rpyOffsets>
    </plugin>
  </gazebo>
  <joint name="torso_lift_joint" type="prismatic">
    <axis xyz="0 0 1"/>
    <limit effort="10000" lower="0.0" upper="0.33" velocity="0.013"/>
    <!-- alpha tested velocity and effort limits -->
    <safety_controller k_position="100" k_velocity="2000000" soft_lower_limit="0.0115" soft_upper_limit="0.325"/>
    <calibration falling="0.00475"/>
    <dynamics damping="20000.0"/>
    <origin rpy="0 0 0" xyz="-0.05 0 0.739675"/>
    <parent link="base_link"/>
    <child link="torso_lift_link"/>
  </joint>
  <link name="torso_lift_link">
    <inertial>
      <mass value="36.248046"/>
      <origin xyz="-0.1 0 -0.0885"/>
      <inertia ixx="2.771653750257" ixy="0.004284522609" ixz="-0.160418504506" iyy="2.510019507959" iyz="0.029664468704" izz="0.526432355569"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/torso_v0/torso_lift.dae"/>
      </geometry>
      <material name="Grey2"/>
    </visual>
    <collision name="torso_lift_collision">
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/torso_v0/torso_lift_L.stl"/>
      </geometry>
    </collision>
  </link>
  <joint name="l_torso_lift_side_plate_joint" type="fixed">
    <origin xyz="0.0535 0.209285 0.176625"/>
    <!-- location of bottom front bolt hole location -->
    <parent link="torso_lift_link"/>
    <child link="l_torso_lift_side_plate_link"/>
  </joint>
  <link name="l_torso_lift_side_plate_link">
    <inertial>
      <mass value="0.1"/>
      <origin xyz="-0.0625 0.0 0.05"/>
      <!-- center of the 12.5cm by 10cm bolt hole pattern -->
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
    </inertial>
  </link>
  <joint name="r_torso_lift_side_plate_joint" type="fixed">
    <origin xyz="0.0535  -0.209285 0.176625"/>
    <!-- location of bottom front bolt hole location -->
    <parent link="torso_lift_link"/>
    <child link="r_torso_lift_side_plate_link"/>
  </joint>
  <link name="r_torso_lift_side_plate_link">
    <inertial>
      <mass value="0.1"/>
      <origin xyz="-0.0625 0.0 0.05"/>
      <!-- center of the 12.5cm by 10cm bolt hole pattern -->
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
    </inertial>
  </link>
  <!-- actuated motor screw joint -->
  <link name="torso_lift_motor_screw_link">
    <inertial>
      <mass value="1.0"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
    </inertial>
    <!-- for debugging only
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <box size="0.5 0.7 0.01" />
        </geometry>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <box size="0.5 0.7 0.01" />
        </geometry>
      </collision>
      -->
  </link>
  <joint name="torso_lift_motor_screw_joint" type="continuous">
    <origin xyz="-0.15 0.0 0.7"/>
    <axis xyz="0 0 1"/>
    <parent link="base_link"/>
    <child link="torso_lift_motor_screw_link"/>
    <dynamics damping="0.0001"/>
  </joint>
  <!-- extensions -->
  <gazebo reference="torso_lift_link">
    <sensor name="torso_lift_contact_sensor" type="contact">
      <contact>
        <collision>torso_lift_link_collision</collision>
      </contact>
      <update_rate>100.0</update_rate>
      <plugin filename="libgazebo_ros_bumper.so" name="torso_lift_gazebo_ros_bumper_controller">
        <alwaysOn>true</alwaysOn>
        <updateRate>100.0</updateRate>
        <bumperTopicName>torso_lift_bumper</bumperTopicName>
        <frameName>world</frameName>
      </plugin>
    </sensor>
  </gazebo>
  <gazebo>
    <joint name="torso_lift_screw_torso_lift_joint" type="screw">
      <parent>torso_lift_link</parent>
      <child>torso_lift_motor_screw_link</child>
      <axis>
        <xyz>0 0 1</xyz>
      </axis>
      <thread_pitch>3141.6</thread_pitch>
    </joint>
  </gazebo>
  <transmission name="torso_lift_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="torso_lift_motor"/>
    <joint name="torso_lift_joint"/>
    <mechanicalReduction>-47641.53</mechanicalReduction>
    <simulated_actuated_joint name="torso_lift_motor_screw_joint" simulated_reduction="3141.6"/>
  </transmission>
  <!-- imu -->
  <joint name="imu_joint" type="fixed">
    <axis xyz="0 1 0"/>
    <origin rpy="0 3.14159265359 0" xyz="-0.02977 -0.1497 0.164"/>
    <parent link="torso_lift_link"/>
    <child link="imu_link"/>
  </joint>
  <link name="imu_link">
    <inertial>
      <mass value="0.001"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.0001"/>
    </inertial>
  </link>
  <!-- gazebo extensions -->
  <gazebo>
    <plugin filename="libgazebo_ros_imu.so" name="imu_controller">
      <alwaysOn>true</alwaysOn>
      <updateRate>100.0</updateRate>
      <bodyName>imu_link</bodyName>
      <topicName>torso_lift_imu/data</topicName>
      <gaussianNoise>2.89e-08</gaussianNoise>
      <xyzOffset>0 0 0</xyzOffset>
      <rpyOffset>0 -180.0 0</rpyOffset>
      <serviceName>/default_imu</serviceName>
    </plugin>
  </gazebo>
  <!-- The xacro preprocesser will replace the parameters below, such as ${cal_head_x}, with
       numerical values that were specified in common.xacro which was included above -->
  <joint name="head_pan_joint" type="revolute">
    <axis xyz="0 0 1"/>
    <limit effort="2.645" lower="-3.007" upper="3.007" velocity="6"/>
    <!-- alpha tested velocity and effort limits -->
    <safety_controller k_position="100" k_velocity="1.5" soft_lower_limit="-2.857" soft_upper_limit="2.857"/>
    <calibration rising="0.0"/>
    <dynamics damping="0.5"/>
    <origin rpy="0.0 0.0 0.0" xyz="-0.01707 0.0 0.38145"/>
    <parent link="torso_lift_link"/>
    <child link="head_pan_link"/>
  </joint>
  <link name="head_pan_link">
    <inertial>
      <mass value="6.339"/>
      <!-- mass/cog/moi updated per 100505_link_data.xls -->
      <origin rpy="0 0 0" xyz="0.010907 0.031693 0.090507"/>
      <inertia ixx="0.032497592" ixy="0.00063604088" ixz="0.0025851534" iyy="0.046545627" iyz="-0.0024534295" izz="0.057652724"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0 " xyz="0 0 0.0"/>
      <geometry>
        <mesh filename="meshes/head_v0/head_pan.dae"/>
      </geometry>
      <material name="Blue"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="meshes/head_v0/head_pan_L.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- extensions -->
  <transmission name="head_pan_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="head_pan_motor"/>
    <joint name="head_pan_joint"/>
    <mechanicalReduction>6.0</mechanicalReduction>
  </transmission>
  <joint name="head_tilt_joint" type="revolute">
    <axis xyz="0 1 0"/>
    <limit effort="18" lower="-0.471238" upper="1.39626" velocity="5"/>
    <!-- alpha tested velocity and effort limits -->
    <safety_controller k_position="100" k_velocity="3.0" soft_lower_limit="-0.3712" soft_upper_limit="1.29626"/>
    <calibration falling="0.0"/>
    <dynamics damping="10.0"/>
    <origin rpy="0 0 0" xyz="0.068 0 0"/>
    <parent link="head_pan_link"/>
    <child link="head_tilt_link"/>
  </joint>
  <link name="head_tilt_link">
    <inertial>
      <mass value="4.479"/>
      <!-- mass/cog/moi updated per 100505_link_data.xls -->
      <origin rpy="0 0 0" xyz="0.001716 -0.019556 0.055002"/>
      <inertia ixx="0.024223222" ixy="0.00062063507" ixz="-0.000096909696" iyy="0.054723086" iyz="0.00279702400" izz="0.067306377"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/head_v0/head_tilt.dae"/>
      </geometry>
      <material name="Green"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/head_v0/head_tilt_L.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- extensions -->
  <transmission name="head_tilt_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="head_tilt_motor"/>
    <joint name="head_tilt_joint"/>
    <mechanicalReduction>6.0</mechanicalReduction>
  </transmission>
  <!-- Head plate frame -->
  <joint name="head_plate_frame_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.0232 0 0.0645"/>
    <parent link="head_tilt_link"/>
    <child link="head_plate_frame"/>
  </joint>
  <link name="head_plate_frame">
    <inertial>
      <mass value="0.01"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.01 0.01 0.01"/>
      </geometry>
      <material name="Blue"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.01 0.01 0.01"/>
      </geometry>
    </collision>
  </link>
  <!-- extensions -->
  <gazebo reference="head_plate_frame">
    <material value="Gazebo/Grey"/>
  </gazebo>
  <!-- Camera package: double stereo, prosilica -->
  <joint name="sensor_mount_frame_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
    <parent link="head_plate_frame"/>
    <child link="sensor_mount_link"/>
  </joint>
  <link name="sensor_mount_link">
    <inertial>
      <!-- Needs verification with CAD -->
      <mass value="0.05"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.01"/>
    </inertial>
    <!-- Should probably get real visuals here at some point -->
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.01 0.01 0.01"/>
      </geometry>
      <material name="Blue"/>
    </visual>
  </link>
  <!-- Prosilica Camera -->
  <joint name="high_def_frame_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.046457 -0.110 0.054600"/>
    <parent link="sensor_mount_link"/>
    <child link="high_def_frame"/>
  </joint>
  <link name="high_def_frame">
    <inertial>
      <mass value="0.01"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
    </inertial>
  </link>
  <joint name="high_def_optical_frame_joint" type="fixed">
    <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0.0            0.0         0.0"/>
    <parent link="high_def_frame"/>
    <child link="high_def_optical_frame"/>
  </joint>
  <link name="high_def_optical_frame">
    <inertial>
      <mass value="0.01"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
    </inertial>
  </link>
  <!-- extensions -->
  <gazebo reference="high_def_frame">
    <sensor name="high_def_sensor" type="camera">
      <always_on>true</always_on>
      <update_rate>20.0</update_rate>
      <camera>
        <horizontal_fov>0.785398163397</horizontal_fov>
        <image>
          <format>R8G8B8</format>
          <width>2448</width>
          <height>2050</height>
        </image>
        <clip>
          <near>0.1</near>
          <far>100</far>
        </clip>
      </camera>
      <plugin filename="libgazebo_ros_prosilica.so" name="high_def_controller">
        <updateRate>20.0</updateRate>
        <cameraName>prosilica</cameraName>
        <imageTopicName>image_raw</imageTopicName>
        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
        <pollServiceName>request_image</pollServiceName>
        <frameName>high_def_optical_frame</frameName>
        <CxPrime>1224.5</CxPrime>
        <Cx>1224.5</Cx>
        <Cy>1025.5</Cy>
        <focalLength>2954.998083</focalLength>
        <distortionK1>0.0</distortionK1>
        <distortionK2>0.0</distortionK2>
        <distortionK3>0.0</distortionK3>
        <distortionT1>0.0</distortionT1>
        <distortionT2>0.0</distortionT2>
      </plugin>
    </sensor>
    <material value="Gazebo/Black"/>
  </gazebo>
  <!-- construct wide and narrow double stereo camera -->
  <!-- Define link to stereo cameras, set origin relative to that -->
  <joint name="double_stereo_frame_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
    <parent link="sensor_mount_link"/>
    <child link="double_stereo_link"/>
  </joint>
  <link name="double_stereo_link">
    <inertial>
      <!-- Needs verification with CAD -->
      <mass value="0.1"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.01"/>
    </inertial>
    <!-- Should probably get real visuals here at some point -->
    <visual>
      <origin rpy="0 0 0" xyz="-0.01 0 0.025"/>
      <geometry>
        <box size="0.02 0.12 0.05"/>
      </geometry>
      <material name="Blue"/>
    </visual>
  </link>
  <!-- Wide camera is on robot right (hca1), narrow on left (hca2)? -->
  <!-- 15mm offset from center needs check with CAD -->
  <joint name="wide_stereo_frame_joint" type="fixed">
    <origin rpy="0.0   0.0   0.0" xyz="0.045 0.03 0.0501"/>
    <parent link="double_stereo_link"/>
    <child link="wide_stereo_link"/>
  </joint>
  <!-- camera link is at center of the optical frame, but in x-forward notation -->
  <link name="wide_stereo_link">
    <inertial>
      <mass value="0.1"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
      <!-- this inertia is made up for now. -->
    </inertial>
  </link>
  <!-- attach optical frame to the camera link -->
  <joint name="wide_stereo_optical_frame_joint" type="fixed">
    <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
    <!-- rotate frame from x-forward to z-forward camera coords -->
    <parent link="wide_stereo_link"/>
    <child link="wide_stereo_optical_frame"/>
  </joint>
  <!-- optical frame for the stereo camera, with z-forward notation, this is the frame stereo camera images users should refer to -->
  <link name="wide_stereo_optical_frame" type="camera"/>
  <!-- stereo left camera -->
  <joint name="wide_stereo_l_stereo_camera_frame_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="wide_stereo_link"/>
    <child link="wide_stereo_l_stereo_camera_frame"/>
  </joint>
  <link name="wide_stereo_l_stereo_camera_frame">
    <inertial>
      <mass value="0.01"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
    </inertial>
  </link>
  <joint name="wide_stereo_l_stereo_camera_optical_frame_joint" type="fixed">
    <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
    <parent link="wide_stereo_l_stereo_camera_frame"/>
    <child link="wide_stereo_l_stereo_camera_optical_frame"/>
  </joint>
  <link name="wide_stereo_l_stereo_camera_optical_frame"/>
  <!-- extensions -->
  <gazebo reference="wide_stereo_l_stereo_camera_frame">
    <sensor name="wide_stereo_l_stereo_camera_sensor" type="camera">
      <always_on>true</always_on>
      <update_rate>25.0</update_rate>
      <camera>
        <horizontal_fov>1.57079632679</horizontal_fov>
        <image>
          <format>BAYER_BGGR8</format>
          <width>640</width>
          <height>480</height>
        </image>
        <clip>
          <near>0.1</near>
          <far>100</far>
        </clip>
      </camera>
      <plugin filename="libgazebo_ros_camera.so" name="wide_stereo_l_stereo_camera_controller">
        <alwaysOn>true</alwaysOn>
        <updateRate>25.0</updateRate>
        <cameraName>wide_stereo/left</cameraName>
        <imageTopicName>image_raw</imageTopicName>
        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
        <frameName>wide_stereo_optical_frame</frameName>
        <hackBaseline>0</hackBaseline>
        <CxPrime>320.5</CxPrime>
        <Cx>320.5</Cx>
        <Cy>240.5</Cy>
        <!-- image_width / (2*tan(hfov_radian /2)) -->
        <!-- 320 for wide and 772.55 for narrow stereo camera -->
        <focalLength>320.000105</focalLength>
        <distortionK1>0.0</distortionK1>
        <distortionK2>0.0</distortionK2>
        <distortionK3>0.0</distortionK3>
        <distortionT1>0.0</distortionT1>
        <distortionT2>0.0</distortionT2>
      </plugin>
    </sensor>
    <turnGravityOff>true</turnGravityOff>
    <material>PR2/Blue</material>
  </gazebo>
  <!-- stereo right camera -->
  <joint name="wide_stereo_r_stereo_camera_frame_joint" type="fixed">
    <origin rpy="0.0 0.0 0.0" xyz="0.0 -0.09 0.0"/>
    <parent link="wide_stereo_l_stereo_camera_frame"/>
    <child link="wide_stereo_r_stereo_camera_frame"/>
  </joint>
  <link name="wide_stereo_r_stereo_camera_frame">
    <inertial>
      <mass value="0.01"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
    </inertial>
  </link>
  <joint name="wide_stereo_r_stereo_camera_optical_frame_joint" type="fixed">
    <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
    <parent link="wide_stereo_r_stereo_camera_frame"/>
    <child link="wide_stereo_r_stereo_camera_optical_frame"/>
  </joint>
  <link name="wide_stereo_r_stereo_camera_optical_frame"/>
  <!-- extensions -->
  <gazebo reference="wide_stereo_r_stereo_camera_frame">
    <sensor name="wide_stereo_r_stereo_camera_sensor" type="camera">
      <always_on>true</always_on>
      <update_rate>25.0</update_rate>
      <camera>
        <horizontal_fov>1.57079632679</horizontal_fov>
        <image>
          <format>BAYER_BGGR8</format>
          <width>640</width>
          <height>480</height>
        </image>
        <clip>
          <near>0.1</near>
          <far>100</far>
        </clip>
      </camera>
      <plugin filename="libgazebo_ros_camera.so" name="wide_stereo_r_stereo_camera_controller">
        <alwaysOn>true</alwaysOn>
        <updateRate>25.0</updateRate>
        <cameraName>wide_stereo/right</cameraName>
        <imageTopicName>image_raw</imageTopicName>
        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
        <frameName>wide_stereo_optical_frame</frameName>
        <hackBaseline>0.09</hackBaseline>
        <CxPrime>320.5</CxPrime>
        <Cx>320.5</Cx>
        <Cy>240.5</Cy>
        <!-- image_width / (2*tan(hfov_radian /2)) -->
        <!-- 320 for wide and 772.55 for narrow stereo camera -->
        <focalLength>320.000105</focalLength>
        <distortionK1>0.0</distortionK1>
        <distortionK2>0.0</distortionK2>
        <distortionK3>0.0</distortionK3>
        <distortionT1>0.0</distortionT1>
        <distortionT2>0.0</distortionT2>
      </plugin>
    </sensor>
    <turnGravityOff>true</turnGravityOff>
    <material>PR2/Blue</material>
  </gazebo>
  <!-- extensions -->
  <gazebo reference="wide_stereo_link">
    <material value="PR2/Blue"/>
    <turnGravityOff value="true"/>
  </gazebo>
  <gazebo reference="wide_stereo_optical_frame">
    <material value="Gazebo/White"/>
    <turnGravityOff value="true"/>
  </gazebo>
  <joint name="narrow_stereo_frame_joint" type="fixed">
    <origin rpy="0.0   0.0   0.0" xyz="0.045 0.06 0.0501"/>
    <parent link="double_stereo_link"/>
    <child link="narrow_stereo_link"/>
  </joint>
  <!-- camera link is at center of the optical frame, but in x-forward notation -->
  <link name="narrow_stereo_link">
    <inertial>
      <mass value="0.1"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
      <!-- this inertia is made up for now. -->
    </inertial>
  </link>
  <!-- attach optical frame to the camera link -->
  <joint name="narrow_stereo_optical_frame_joint" type="fixed">
    <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
    <!-- rotate frame from x-forward to z-forward camera coords -->
    <parent link="narrow_stereo_link"/>
    <child link="narrow_stereo_optical_frame"/>
  </joint>
  <!-- optical frame for the stereo camera, with z-forward notation, this is the frame stereo camera images users should refer to -->
  <link name="narrow_stereo_optical_frame" type="camera"/>
  <!-- stereo left camera -->
  <joint name="narrow_stereo_l_stereo_camera_frame_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="narrow_stereo_link"/>
    <child link="narrow_stereo_l_stereo_camera_frame"/>
  </joint>
  <link name="narrow_stereo_l_stereo_camera_frame">
    <inertial>
      <mass value="0.01"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
    </inertial>
  </link>
  <joint name="narrow_stereo_l_stereo_camera_optical_frame_joint" type="fixed">
    <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
    <parent link="narrow_stereo_l_stereo_camera_frame"/>
    <child link="narrow_stereo_l_stereo_camera_optical_frame"/>
  </joint>
  <link name="narrow_stereo_l_stereo_camera_optical_frame"/>
  <!-- extensions -->
  <gazebo reference="narrow_stereo_l_stereo_camera_frame">
    <sensor name="narrow_stereo_l_stereo_camera_sensor" type="camera">
      <always_on>true</always_on>
      <update_rate>25.0</update_rate>
      <camera>
        <horizontal_fov>0.785398163397</horizontal_fov>
        <image>
          <format>L8</format>
          <width>640</width>
          <height>480</height>
        </image>
        <clip>
          <near>0.1</near>
          <far>100</far>
        </clip>
      </camera>
      <plugin filename="libgazebo_ros_camera.so" name="narrow_stereo_l_stereo_camera_controller">
        <alwaysOn>true</alwaysOn>
        <updateRate>25.0</updateRate>
        <cameraName>narrow_stereo/left</cameraName>
        <imageTopicName>image_raw</imageTopicName>
        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
        <frameName>narrow_stereo_optical_frame</frameName>
        <hackBaseline>0</hackBaseline>
        <CxPrime>320.5</CxPrime>
        <Cx>320.5</Cx>
        <Cy>240.5</Cy>
        <!-- image_width / (2*tan(hfov_radian /2)) -->
        <!-- 320 for wide and 772.55 for narrow stereo camera -->
        <focalLength>772.548518</focalLength>
        <distortionK1>0.0</distortionK1>
        <distortionK2>0.0</distortionK2>
        <distortionK3>0.0</distortionK3>
        <distortionT1>0.0</distortionT1>
        <distortionT2>0.0</distortionT2>
      </plugin>
    </sensor>
    <turnGravityOff>true</turnGravityOff>
    <material>PR2/Blue</material>
  </gazebo>
  <!-- stereo right camera -->
  <joint name="narrow_stereo_r_stereo_camera_frame_joint" type="fixed">
    <origin rpy="0.0 0.0 0.0" xyz="0.0 -0.09 0.0"/>
    <parent link="narrow_stereo_l_stereo_camera_frame"/>
    <child link="narrow_stereo_r_stereo_camera_frame"/>
  </joint>
  <link name="narrow_stereo_r_stereo_camera_frame">
    <inertial>
      <mass value="0.01"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
    </inertial>
  </link>
  <joint name="narrow_stereo_r_stereo_camera_optical_frame_joint" type="fixed">
    <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
    <parent link="narrow_stereo_r_stereo_camera_frame"/>
    <child link="narrow_stereo_r_stereo_camera_optical_frame"/>
  </joint>
  <link name="narrow_stereo_r_stereo_camera_optical_frame"/>
  <!-- extensions -->
  <gazebo reference="narrow_stereo_r_stereo_camera_frame">
    <sensor name="narrow_stereo_r_stereo_camera_sensor" type="camera">
      <always_on>true</always_on>
      <update_rate>25.0</update_rate>
      <camera>
        <horizontal_fov>0.785398163397</horizontal_fov>
        <image>
          <format>L8</format>
          <width>640</width>
          <height>480</height>
        </image>
        <clip>
          <near>0.1</near>
          <far>100</far>
        </clip>
      </camera>
      <plugin filename="libgazebo_ros_camera.so" name="narrow_stereo_r_stereo_camera_controller">
        <alwaysOn>true</alwaysOn>
        <updateRate>25.0</updateRate>
        <cameraName>narrow_stereo/right</cameraName>
        <imageTopicName>image_raw</imageTopicName>
        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
        <frameName>narrow_stereo_optical_frame</frameName>
        <hackBaseline>0.09</hackBaseline>
        <CxPrime>320.5</CxPrime>
        <Cx>320.5</Cx>
        <Cy>240.5</Cy>
        <!-- image_width / (2*tan(hfov_radian /2)) -->
        <!-- 320 for wide and 772.55 for narrow stereo camera -->
        <focalLength>772.548518</focalLength>
        <distortionK1>0.0</distortionK1>
        <distortionK2>0.0</distortionK2>
        <distortionK3>0.0</distortionK3>
        <distortionT1>0.0</distortionT1>
        <distortionT2>0.0</distortionT2>
      </plugin>
    </sensor>
    <turnGravityOff>true</turnGravityOff>
    <material>PR2/Blue</material>
  </gazebo>
  <!-- extensions -->
  <gazebo reference="narrow_stereo_link">
    <material value="PR2/Blue"/>
    <turnGravityOff value="true"/>
  </gazebo>
  <gazebo reference="narrow_stereo_optical_frame">
    <material value="Gazebo/White"/>
    <turnGravityOff value="true"/>
  </gazebo>
  <!-- extensions -->
  <!-- Define link to stereo cameras, set origin relative to that -->
  <gazebo reference="double_stereo_double_stereo_link">
    <material value="PR2/Blue"/>
  </gazebo>
  <!-- extensions -->
  <gazebo reference="sensor_mount_sensor_link">
    <material value="PR2/Blue"/>
  </gazebo>
  <!-- Projector -->
  <joint name="projector_wg6802418_frame_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0.110 0.0546"/>
    <parent link="head_plate_frame"/>
    <child link="projector_wg6802418_frame"/>
  </joint>
  <link name="projector_wg6802418_frame">
    <inertial>
      <mass value="0.01"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
    </inertial>
  </link>
  <joint name="projector_wg6802418_child_frame_joint" type="fixed">
    <origin rpy="0 -1.57079632679 0" xyz="0 0 0"/>
    <parent link="projector_wg6802418_frame"/>
    <child link="projector_wg6802418_child_frame"/>
  </joint>
  <link name="projector_wg6802418_child_frame">
    <inertial>
      <mass value="0.01"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
    </inertial>
  </link>
  <!-- extensions -->
  <gazebo reference="projector_wg6802418_child_frame">
    <projector name="projector_wg6802418_projector_wg6802418">
      <pose>0 0 0 0 -1.5707 0</pose>
      <texture>stereo_projection_pattern_high_res_red.png</texture>
      <fov>0.959931088597</fov>
      <near_clip>0.1</near_clip>
      <far_clip>10</far_clip>
    </projector>
  </gazebo>
  <gazebo>
    <plugin filename="libgazebo_ros_projector.so" name="projector_wg6802418_controller">
      <projector>projector_wg6802418_child_frame/projector_wg6802418_projector_wg6802418</projector>
      <alwaysOn>true</alwaysOn>
      <updateRate>15.0</updateRate>
      <textureName>stereo_projection_pattern_high_res_red.png</textureName>
      <filterTextureName>stereo_projection_pattern_filter.png</filterTextureName>
      <textureTopicName>projector_wg6802418_controller/image</textureTopicName>
      <projectorTopicName>projector_wg6802418_controller/projector</projectorTopicName>
    </plugin>
  </gazebo>
  <joint name="laser_tilt_mount_joint" type="revolute">
    <axis xyz="0 1 0"/>
    <limit effort="0.65" lower="-0.7854" upper="1.48353" velocity="10.0"/>
    <!-- alpha tested velocity and effort limits -->
    <safety_controller k_position="100" k_velocity="0.05" soft_lower_limit="-0.7354" soft_upper_limit="1.43353"/>
    <calibration falling="0.0"/>
    <dynamics damping="0.008"/>
    <origin rpy="0 0 0" xyz="0.09893 0 0.227"/>
    <parent link="torso_lift_link"/>
    <child link="laser_tilt_mount_link"/>
  </joint>
  <link name="laser_tilt_mount_link">
    <inertial>
      <mass value="0.591"/>
      <origin rpy="0 0 0" xyz="-0.001136 0.00167 -0.00713"/>
      <inertia ixx="0.001195273" ixy="0.000023087" ixz="0.000037467" iyy="0.001083956" iyz="0.000034906" izz="0.000795014"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/tilting_laser_v0/tilting_hokuyo.dae"/>
      </geometry>
      <material name="Red"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/tilting_laser_v0/tilting_hokuyo_L.stl"/>
      </geometry>
    </collision>
  </link>
  <joint name="laser_tilt_joint" type="fixed">
    <axis xyz="0 1 0"/>
    <origin rpy="0 0 0" xyz="0 0 0.03"/>
    <parent link="laser_tilt_mount_link"/>
    <child link="laser_tilt_link"/>
  </joint>
  <link name="laser_tilt_link" type="laser">
    <inertial>
      <mass value="0.001"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.0001"/>
    </inertial>
  </link>
  <!-- gazebo extensions -->
  <gazebo reference="laser_tilt_link">
    <sensor name="laser_tilt" type="ray">
      <always_on>true</always_on>
      <update_rate>40</update_rate>
      <pose>0 0 0 0 0 0</pose>
      <visualize>false</visualize>
      <ray>
        <scan>
          <horizontal>
            <samples>640</samples>
            <resolution>1</resolution>
            <min_angle>-1.3962634</min_angle>
            <max_angle>1.3962634</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.08</min>
          <max>10.0</max>
          <resolution>0.01</resolution>
        </range>
      </ray>
      <plugin filename="libgazebo_ros_laser.so" name="gazebo_ros_laser_tilt_controller">
        <gaussianNoise>0.005</gaussianNoise>
        <alwaysOn>true</alwaysOn>
        <updateRate>40</updateRate>
        <topicName>tilt_scan</topicName>
        <frameName>laser_tilt_link</frameName>
        <hokuyoMinIntensity>101</hokuyoMinIntensity>
      </plugin>
    </sensor>
  </gazebo>
  <!-- extensions -->
  <gazebo reference="laser_tilt_mount_link">
    </gazebo>
  <transmission name="laser_tilt_mount_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="laser_tilt_mount_motor"/>
    <joint name="laser_tilt_mount_joint"/>
    <mechanicalReduction>-6.05</mechanicalReduction>
  </transmission>
  <!-- This is a common convention, to use a reflect parameter that equals +-1 to distinguish left from right -->
  <!-- Shoulder pan -->
  <joint name="r_shoulder_pan_joint" type="revolute">
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 0" xyz="0.0 -0.188 0.0"/>
    <!-- transform from parent link to this joint frame -->
    <parent link="torso_lift_link"/>
    <child link="r_shoulder_pan_link"/>
    <limit effort="30" lower="-2.2853981634" upper="0.714601836603" velocity="2.088"/>
    <!-- alpha tested velocity and effort limits -->
    <dynamics damping="10.0"/>
    <safety_controller k_position="100" k_velocity="10" soft_lower_limit="-2.1353981634" soft_upper_limit="0.564601836603"/>
    <!-- joint angle when the rising or the falling flag is activated on PR2 -->
    <calibration rising="-0.785398163397"/>
  </joint>
  <link name="r_shoulder_pan_link">
    <inertial>
      <mass value="25.799322"/>
      <origin rpy="0 0 0" xyz="-0.001201 0.024513 -0.098231"/>
      <inertia ixx="0.866179142480" ixy="-0.06086507933" ixz="-0.12118061183" iyy="0.87421714893" iyz="-0.05886609911" izz="0.27353821674"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/shoulder_v0/shoulder_pan.dae"/>
      </geometry>
      <material name="Blue"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.0 0 0.0"/>
      <geometry>
        <mesh filename="meshes/shoulder_v0/shoulder_pan.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- Shoulder lift -->
  <joint name="r_shoulder_lift_joint" type="revolute">
    <axis xyz="0 1 0"/>
    <!-- Limits updated from Function's CAD values as of 2009_02_24 (link_data.xls) -->
    <limit effort="30" lower="-0.5236" upper="1.3963" velocity="2.082"/>
    <!-- alpha tested velocity and effort limits -->
    <safety_controller k_position="100" k_velocity="10" soft_lower_limit="-0.3536" soft_upper_limit="1.2963"/>
    <calibration falling="0.0"/>
    <dynamics damping="10.0"/>
    <origin rpy="0 0 0" xyz="0.1 0 0"/>
    <parent link="r_shoulder_pan_link"/>
    <child link="r_shoulder_lift_link"/>
  </joint>
  <link name="r_shoulder_lift_link">
    <inertial>
      <mass value="2.74988"/>
      <origin rpy="0 0 0" xyz="0.02195 -0.02664 -0.03127"/>
      <inertia ixx="0.02105584615" ixy="0.00496704022" ixz="-0.00194808955" iyy="0.02127223737" iyz="0.00110425490" izz="0.01975753814"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/shoulder_v0/shoulder_lift.dae"/>
      </geometry>
      <material name="Grey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/shoulder_v0/shoulder_lift.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- Upper arm roll is separate macro -->
  <joint name="r_upper_arm_roll_joint" type="revolute">
    <axis xyz="1 0 0"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="r_shoulder_lift_link"/>
    <child link="r_upper_arm_roll_link"/>
    <limit effort="30" lower="-3.9" upper="0.8" velocity="3.27"/>
    <!-- alpha tested velocity and effort limits -->
    <safety_controller k_position="100" k_velocity="2" soft_lower_limit="-3.75" soft_upper_limit="0.65"/>
    <calibration rising="-1.57079632679"/>
    <dynamics damping="0.1"/>
  </joint>
  <link name="r_upper_arm_roll_link">
    <inertial>
      <!-- dummy mass, to be removed -->
      <mass value="0.1"/>
      <origin rpy="0 0 0" xyz="0.0 0 0"/>
      <inertia ixx="0.01" ixy="0.00" ixz="0.00" iyy="0.01" iyz="0.00" izz="0.01"/>
    </inertial>
    <visual>
      <!-- TODO: This component doesn't actually have a mesh -->
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/shoulder_v0/upper_arm_roll.stl"/>
      </geometry>
      <material name="RollLinks"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/shoulder_v0/upper_arm_roll_L.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- extensions -->
  <gazebo reference="r_upper_arm_roll_link">
    <material value="PR2/RollLinks"/>
    <turnGravityOff>true</turnGravityOff>
  </gazebo>
  <gazebo reference="r_upper_arm_roll_joint">
    <stopKd value="1.0"/>
    <stopKp value="1000000.0"/>
    <fudgeFactor value="0.5"/>
  </gazebo>
  <!-- Upperarm roll: internal fixed attchment point for upper arm -->
  <transmission name="r_upper_arm_roll_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="r_upper_arm_roll_motor"/>
    <joint name="r_upper_arm_roll_joint"/>
    <mechanicalReduction>32.6525111499</mechanicalReduction>
  </transmission>
  <!-- extensions -->
  <!-- Shoulder pan -->
  <gazebo reference="r_shoulder_pan_link">
    <turnGravityOff>true</turnGravityOff>
  </gazebo>
  <gazebo reference="r_shoulder_pan_joint">
    <stopKd value="1.0"/>
    <stopKp value="1000000.0"/>
  </gazebo>
  <!-- Shoulder lift -->
  <gazebo reference="r_shoulder_lift_link">
    <turnGravityOff>true</turnGravityOff>
  </gazebo>
  <gazebo reference="r_shoulder_lift_joint">
    <stopKd value="1.0"/>
    <stopKp value="1000000.0"/>
  </gazebo>
  <!-- Shoulder pan -->
  <transmission name="r_shoulder_pan_trans" type="pr2_mechanism_model/SimpleTransmission">
    <joint name="r_shoulder_pan_joint"/>
    <actuator name="r_shoulder_pan_motor"/>
    <mechanicalReduction>63.1552452977</mechanicalReduction>
    <compensator k_belt="4000.0" kd_motor="15.0" lambda_combined="0.0" lambda_joint="40.0" lambda_motor="40.0" mass_motor="0.05"/>
  </transmission>
  <!-- Shoulder lift -->
  <transmission name="r_shoulder_lift_trans" type="pr2_mechanism_model/SimpleTransmission">
    <joint name="r_shoulder_lift_joint"/>
    <actuator name="r_shoulder_lift_motor"/>
    <mechanicalReduction>61.8948225713</mechanicalReduction>
    <compensator k_belt="4000.0" kd_motor="10.0" lambda_combined="0.0" lambda_joint="60.0" lambda_motor="60.0" mass_motor="0.05"/>
  </transmission>
  <joint name="r_upper_arm_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="r_upper_arm_roll_link"/>
    <child link="r_upper_arm_link"/>
  </joint>
  <link name="r_upper_arm_link">
    <inertial>
      <!-- NOTE:reflect==-1 for right side, reflect==1 for the left side -->
      <mass value="6.01769"/>
      <origin xyz="0.21398 -0.01621 -0.0002"/>
      <inertia ixx="0.01537748957" ixy="0.00375711247" ixz="-0.00070852914" iyy="0.0747367044" iyz="-0.0001793645" izz="0.07608763307"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/upper_arm_v0/upper_arm.dae"/>
      </geometry>
      <material name="Green"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/upper_arm_v0/upper_arm.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- extensions-->
  <gazebo reference="r_upper_arm_link">
    <turnGravityOff>true</turnGravityOff>
  </gazebo>
  <!-- Has upperarm link -->
  <!-- No transmission, since this a fixed joint w/o actuator -->
  <!-- Elbow flex, FA roll macros -->
  <!-- forearm_roll_link is a fictitious link internal to elbow_flex_link, provides an attachment point for the actual forearm -->
  <joint name="r_forearm_roll_joint" type="continuous">
    <axis xyz="1 0 0"/>
    <limit effort="30" velocity="3.6"/>
    <!-- alpha tested velocity and effort limits -->
    <safety_controller k_velocity="1"/>
    <calibration rising="0.0"/>
    <dynamics damping="0.1"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="r_elbow_flex_link"/>
    <child link="r_forearm_roll_link"/>
  </joint>
  <!-- TODO: inertial tag should be optional -->
  <link name="r_forearm_roll_link">
    <inertial>
      <!-- dummy masses, to be removed -->
      <mass value="0.1"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.01" ixy="0.00" ixz="0.00" iyy="0.01" iyz="0.00" izz="0.01"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/upper_arm_v0/forearm_roll.stl"/>
      </geometry>
      <material name="RollLinks"/>
    </visual>
    <!-- TODO: collision tag should be optional -->
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/upper_arm_v0/forearm_roll_L.stl"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="r_forearm_roll_link">
    <material value="PR2/RollLinks"/>
    <turnGravityOff>true</turnGravityOff>
  </gazebo>
  <gazebo reference="r_forearm_roll_joint">
    <fudgeFactor value="0.5"/>
  </gazebo>
  <transmission name="r_forearm_roll_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="r_forearm_roll_motor"/>
    <joint name="r_forearm_roll_joint"/>
    <mechanicalReduction>-90.5142857143</mechanicalReduction>
  </transmission>
  <!-- Elbow flex -->
  <joint name="r_elbow_flex_joint" type="revolute">
    <axis xyz="0 1 0"/>
    <!-- Note: Overtravel limits are 140, -7 degrees instead of 133, 0 -->
    <limit effort="30" lower="-2.3213" upper="0.00" velocity="3.3"/>
    <!-- alpha tested velocity and effort limits -->
    <safety_controller k_position="100" k_velocity="3" soft_lower_limit="-2.1213" soft_upper_limit="-0.15"/>
    <calibration falling="-1.1606"/>
    <dynamics damping="1.0"/>
    <origin rpy="0 0 0" xyz="0.4 0 0"/>
    <parent link="r_upper_arm_link"/>
    <child link="r_elbow_flex_link"/>
  </joint>
  <link name="r_elbow_flex_link">
    <inertial>
      <mass value="1.90327"/>
      <origin xyz="0.01014 0.00032 -0.01211"/>
      <inertia ixx="0.00346541989" ixy="0.00004066825" ixz="0.00043171614" iyy="0.00441606455" iyz="-0.00003968914" izz="0.00359156824"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/upper_arm_v0/elbow_flex.dae"/>
      </geometry>
      <material name="Grey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/upper_arm_v0/elbow_flex.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- testing initial joint position, to be parsed at urdf2gazebo parse time ->
    <gazebo reference="${side}_elbow_flex_joint">
      <initial_joint_position>-1.0</initial_joint_position>
    </gazebo>
    -->
  <gazebo reference="r_elbow_flex_link">
    <turnGravityOff>true</turnGravityOff>
  </gazebo>
  <gazebo reference="r_elbow_flex_joint">
    <stopKd value="1.0"/>
    <stopKp value="1000000.0"/>
  </gazebo>
  <!-- Elbow flex -->
  <transmission name="r_elbow_flex_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="r_elbow_flex_motor"/>
    <joint name="r_elbow_flex_joint"/>
    <mechanicalReduction>-36.167452007</mechanicalReduction>
  </transmission>
  <joint name="r_forearm_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <!-- transform from parent link to this joint frame -->
    <parent link="r_forearm_roll_link"/>
    <child link="r_forearm_link"/>
  </joint>
  <link name="r_forearm_link">
    <inertial>
      <mass value="2.57968"/>
      <origin xyz="0.18791 -0.00017 -0.00912"/>
      <inertia ixx="0.00364857222" ixy="0.00005215877" ixz="0.00071534842" iyy="0.01507736897" iyz="-0.00001310770" izz="0.01659310749"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/forearm_v0/forearm.dae"/>
      </geometry>
      <material name="Grey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/forearm_v0/forearm.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- Wrist flex -->
  <joint name="r_wrist_flex_joint" type="revolute">
    <axis xyz="0 1 0"/>
    <limit effort="10" lower="-2.18" upper="0.0" velocity="3.078"/>
    <!-- alpha tested velocity and effort limits -->
    <safety_controller k_position="20" k_velocity="4" soft_lower_limit="-2.0" soft_upper_limit="-0.1"/>
    <dynamics damping="0.1"/>
    <calibration falling="-0.5410521"/>
    <origin rpy="0 0 0" xyz="0.321 0 0"/>
    <parent link="r_forearm_link"/>
    <child link="r_wrist_flex_link"/>
  </joint>
  <link name="r_wrist_flex_link">
    <inertial>
      <mass value="0.61402"/>
      <origin xyz="-0.00157 0.0 -0.00075"/>
      <inertia ixx="0.00065165722" ixy="0.00000028864" ixz="0.00000303477" iyy="0.00019824443" iyz="-0.00000022645" izz="0.00064450498"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/forearm_v0/wrist_flex.dae"/>
      </geometry>
      <material name="Grey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/forearm_v0/wrist_flex.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- Wrist roll -->
  <joint name="r_wrist_roll_joint" type="continuous">
    <axis xyz="1 0 0"/>
    <limit effort="10" velocity="3.6"/>
    <!-- alpha tested velocity and effort limits -->
    <safety_controller k_velocity="2"/>
    <dynamics damping="0.1"/>
    <calibration rising="-1.57079632679"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="r_wrist_flex_link"/>
    <child link="r_wrist_roll_link"/>
  </joint>
  <link name="r_wrist_roll_link">
    <inertial>
      <!-- dummy masses, to be removed.  wrist roll masses are on "gripper_palm" -->
      <mass value="0.1"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/forearm_v0/wrist_roll.stl"/>
      </geometry>
      <material name="RollLinks"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/forearm_v0/wrist_roll_L.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- extensions-->
  <gazebo reference="r_forearm_link">
    <turnGravityOff>true</turnGravityOff>
  </gazebo>
  <!-- Wrist flex -->
  <gazebo reference="r_wrist_flex_link">
    <turnGravityOff>true</turnGravityOff>
  </gazebo>
  <gazebo reference="r_wrist_flex_joint">
    <stopKd value="1.0"/>
    <stopKp value="1000000.0"/>
  </gazebo>
  <!-- Wrist roll -->
  <gazebo reference="r_wrist_roll_link">
    <turnGravityOff>true</turnGravityOff>
    <material value="PR2/RollLinks"/>
  </gazebo>
  <gazebo reference="r_wrist_roll_joint">
    <fudgeFactor value="0.5"/>
  </gazebo>
  <!-- Wrist flex , Wrist roll -->
  <transmission name="r_wrist_trans" type="pr2_mechanism_model/WristTransmission">
    <rightActuator mechanicalReduction="60.1714285714" name="r_wrist_r_motor"/>
    <leftActuator mechanicalReduction="60.1714285714" name="r_wrist_l_motor"/>
    <flexJoint mechanicalReduction="-1.0" name="r_wrist_flex_joint"/>
    <rollJoint mechanicalReduction="1.0" name="r_wrist_roll_joint"/>
  </transmission>
  <joint name="r_gripper_palm_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="r_wrist_roll_link"/>
    <child link="r_gripper_palm_link"/>
  </joint>
  <link name="r_gripper_palm_link">
    <inertial>
      <mass value="0.58007"/>
      <origin rpy="0 0 0" xyz="0.06623 0.00053 -0.00119"/>
      <inertia ixx="0.00035223921" ixy="-0.00001580476" ixz="-0.00000091750" iyy="0.00067741312" iyz="-0.00000059554" izz="0.00086563316"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/gripper_v0/gripper_palm.dae"/>
      </geometry>
      <material name="Red"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/gripper_v0/gripper_palm.stl"/>
      </geometry>
    </collision>
  </link>
  <joint name="r_gripper_led_joint" type="fixed">
    <!--  Need to check if we need a positive or negative Z term -->
    <origin xyz="0.0513 0.0 .0244"/>
    <parent link="r_gripper_palm_link"/>
    <child link="r_gripper_led_frame"/>
  </joint>
  <link name="r_gripper_led_frame"/>
  <joint name="r_gripper_motor_accelerometer_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="r_gripper_palm_link"/>
    <child link="r_gripper_motor_accelerometer_link"/>
  </joint>
  <link name="r_gripper_motor_accelerometer_link">
    <inertial>
      <mass value="0.001"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.001 0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="r_gripper_tool_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.18 0 0"/>
    <parent link="r_gripper_palm_link"/>
    <child link="r_gripper_tool_frame"/>
  </joint>
  <link name="r_gripper_tool_frame"/>
  <!-- actuated motor screw joint -->
  <link name="r_gripper_motor_slider_link">
    <inertial>
      <mass value="0.01"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
    </inertial>
    <!-- for debugging only
      <visual>
        <origin xyz="0 0 0" rpy="1.5708 0 0" />
        <geometry>
          <cylinder length="0.002" radius="0.025"/>
        </geometry>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="1.5708 0 0" />
        <geometry>
          <cylinder length="0.002" radius="0.025"/>
        </geometry>
      </collision>
      -->
  </link>
  <joint name="r_gripper_motor_slider_joint" type="prismatic">
    <origin rpy="0 0 0" xyz="0.16828 0 0"/>
    <axis xyz="1 0 0"/>
    <parent link="r_gripper_palm_link"/>
    <child link="r_gripper_motor_slider_link"/>
    <limit effort="1000.0" lower="-0.1" upper="0.1" velocity="0.2"/>
  </joint>
  <link name="r_gripper_motor_screw_link">
    <inertial>
      <mass value="0.01"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
    </inertial>
    <!-- for debugging only
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <box size="0.05 0.001 0.05" />
        </geometry>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <box size="0.05 0.001 0.05" />
        </geometry>
      </collision>
      -->
  </link>
  <joint name="r_gripper_motor_screw_joint" type="continuous">
    <origin rpy="0 0 0" xyz="0.0 0 0"/>
    <axis xyz="0 1 0"/>
    <parent link="r_gripper_motor_slider_link"/>
    <child link="r_gripper_motor_screw_link"/>
    <dynamics damping="0.0001"/>
  </joint>
  <!-- pr2 fingers macro -->
  <!-- Finger proximal digit -->
  <joint name="r_gripper_l_finger_joint" type="revolute">
    <axis xyz="0 0 1"/>
    <!-- limits on passive finger and finger top joints without
         transmission are not enforced by safety controllers.
         The lower/upper limits and are enforced in PR2 simulation and
         effort and velocity limits are ignored. This is also needed because
         these joints are declared revolute rather than continuous.-->
    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
    <dynamics damping="0.02"/>
    <origin rpy="0 0 0" xyz="0.07691 0.01 0"/>
    <parent link="r_gripper_palm_link"/>
    <child link="r_gripper_l_finger_link"/>
  </joint>
  <link name="r_gripper_l_finger_link">
    <inertial>
      <mass value="0.17126"/>
      <origin rpy="0 0 0" xyz="0.03598 0.01730 -0.00164"/>
      <inertia ixx="0.00007756198" ixy="0.00000149095" ixz="-0.00000983385" iyy="0.00019708305" iyz="-0.00000306125" izz="0.00018105446"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/gripper_v0/l_finger.dae"/>
      </geometry>
      <material name="Grey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/gripper_v0/l_finger.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- Finger proximal digit -->
  <joint name="r_gripper_r_finger_joint" type="revolute">
    <axis xyz="0 0 -1"/>
    <origin rpy="0 0 0" xyz="0.07691 -0.01 0"/>
    <!-- limits on passive finger and finger top joints without
         transmission are not enforced by safety controllers.
         The lower/upper limits and are enforced in PR2 simulation and
         effort and velocity limits are ignored. This is also needed because
         these joints are declared revolute rather than continuous.-->
    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
    <dynamics damping="0.02"/>
    <mimic joint="r_gripper_l_finger_joint" multiplier="1" offset="0"/>
    <parent link="r_gripper_palm_link"/>
    <child link="r_gripper_r_finger_link"/>
  </joint>
  <link name="r_gripper_r_finger_link">
    <inertial>
      <mass value="0.17389"/>
      <origin rpy="0 0 0" xyz="0.03576 -0.01736 -0.00095"/>
      <inertia ixx="0.00007738410" ixy="-0.00000209309" ixz="-0.00000836228" iyy="0.00019847383" iyz="0.00000246110" izz="0.00018106988"/>
    </inertial>
    <visual>
      <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/gripper_v0/l_finger.dae"/>
      </geometry>
      <material name="Grey"/>
    </visual>
    <collision>
      <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/gripper_v0/l_finger.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- Finger tip -->
  <joint name="r_gripper_l_finger_tip_joint" type="revolute">
    <axis xyz="0 0 -1"/>
    <origin rpy="0 0 0" xyz="0.09137 0.00495 0"/>
    <!-- limits on passive finger and finger top joints without
         transmission are not enforced by safety controllers.
         The lower/upper limits and are enforced in PR2 simulation and
         effort and velocity limits are ignored. This is also needed because
         these joints are declared revolute rather than continuous.-->
    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
    <dynamics damping="0.001"/>
    <mimic joint="r_gripper_l_finger_joint" multiplier="1" offset="0"/>
    <parent link="r_gripper_l_finger_link"/>
    <child link="r_gripper_l_finger_tip_link"/>
  </joint>
  <link name="r_gripper_l_finger_tip_link">
    <inertial>
      <mass value="0.04419"/>
      <origin rpy="0 0 0" xyz="0.00423 0.00284 0.0"/>
      <inertia ixx="0.00000837047" ixy="0.00000583632" ixz="0.0" iyy="0.00000987067" iyz="0.0" izz="0.00001541768"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/gripper_v0/l_finger_tip.dae"/>
      </geometry>
      <material name="Green"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/gripper_v0/l_finger_tip.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- Finger tip -->
  <joint name="r_gripper_r_finger_tip_joint" type="revolute">
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 0" xyz="0.09137 -0.00495 0"/>
    <!-- limits on passive finger and finger top joints without
         transmission are not enforced by safety controllers.
         The lower/upper limits and are enforced in PR2 simulation and
         effort and velocity limits are ignored. This is also needed because
         these joints are declared revolute rather than continuous.-->
    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
    <dynamics damping="0.001"/>
    <mimic joint="r_gripper_l_finger_joint" multiplier="1" offset="0"/>
    <parent link="r_gripper_r_finger_link"/>
    <child link="r_gripper_r_finger_tip_link"/>
  </joint>
  <link name="r_gripper_r_finger_tip_link">
    <inertial>
      <mass value="0.04419"/>
      <origin rpy="0 0 0" xyz="0.00423 -0.00284 0.0"/>
      <inertia ixx="0.00000837047" ixy="-0.00000583632" ixz="0.0" iyy="0.00000987067" iyz="0.0" izz="0.00001541768"/>
    </inertial>
    <visual>
      <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/gripper_v0/l_finger_tip.dae"/>
      </geometry>
      <material name="Green"/>
    </visual>
    <collision>
      <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/gripper_v0/l_finger_tip.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- extensions -->
  <!-- Finger proximal digit -->
  <gazebo reference="r_gripper_l_finger_link">
    <turnGravityOff>true</turnGravityOff>
    <mu1 value="500.0"/>
    <mu2 value="500.0"/>
    <kp value="1000000.0"/>
    <kd value="1.0"/>
    <!-- for "${prefix}_l_finger_joint"-->
  </gazebo>
  <gazebo reference="r_gripper_l_finger_joint">
    <stopKd value="1.0"/>
    <stopKp value="10000000.0"/>
    <fudgeFactor value="1.0"/>
    <provideFeedback value="true"/>
  </gazebo>
  <!-- Finger proximal digit -->
  <gazebo reference="r_gripper_r_finger_link">
    <turnGravityOff>true</turnGravityOff>
    <mu1 value="500.0"/>
    <mu2 value="500.0"/>
    <kp value="1000000.0"/>
    <kd value="1.0"/>
  </gazebo>
  <gazebo reference="r_gripper_r_finger_joint">
    <stopKd value="1.0"/>
    <stopKp value="10000000.0"/>
    <fudgeFactor value="1.0"/>
    <provideFeedback value="true"/>
  </gazebo>
  <!-- Finger tip -->
  <gazebo reference="r_gripper_l_finger_tip_link">
    <turnGravityOff>true</turnGravityOff>
    <selfCollide>false</selfCollide>
    <sensor name="r_gripper_l_finger_tip_contact_sensor" type="contact">
      <update_rate>100.0</update_rate>
      <contact>
        <collision>r_gripper_l_finger_tip_link_collision</collision>
      </contact>
      <plugin filename="libgazebo_ros_bumper.so" name="r_gripper_l_finger_tip_gazebo_ros_bumper_controller">
        <alwaysOn>true</alwaysOn>
        <frameName>r_gripper_l_finger_tip_link</frameName>
        <updateRate>100.0</updateRate>
        <bumperTopicName>r_gripper_l_finger_tip_bumper</bumperTopicName>
      </plugin>
    </sensor>
    <mu1 value="500.0"/>
    <mu2 value="500.0"/>
    <kp value="10000000.0"/>
    <kd value="1.0"/>
  </gazebo>
  <gazebo reference="r_gripper_l_finger_tip_joint">
    <stopKd value="1.0"/>
    <stopKp value="10000000.0"/>
    <fudgeFactor value="1.0"/>
    <provideFeedback value="true"/>
  </gazebo>
  <!-- Finger tip -->
  <gazebo reference="r_gripper_r_finger_tip_link">
    <turnGravityOff>true</turnGravityOff>
    <selfCollide>false</selfCollide>
    <sensor name="r_gripper_r_finger_tip_contact_sensor" type="contact">
      <update_rate>100.0</update_rate>
      <contact>
        <collision>r_gripper_r_finger_tip_link_collision</collision>
      </contact>
      <plugin filename="libgazebo_ros_bumper.so" name="r_gripper_r_finger_tip_gazebo_ros_bumper_controller">
        <alwaysOn>true</alwaysOn>
        <frameName>r_gripper_r_finger_tip_link</frameName>
        <updateRate>100.0</updateRate>
        <bumperTopicName>r_gripper_r_finger_tip_bumper</bumperTopicName>
      </plugin>
    </sensor>
    <mu1 value="500.0"/>
    <mu2 value="500.0"/>
    <kp value="10000000.0"/>
    <kd value="1.0"/>
  </gazebo>
  <gazebo>
    <plugin filename="libgazebo_ros_p3d.so" name="p3d_r_gripper_l_finger_controller">
      <alwaysOn>true</alwaysOn>
      <updateRate>100.0</updateRate>
      <bodyName>r_gripper_l_finger_link</bodyName>
      <topicName>r_gripper_l_finger_pose_ground_truth</topicName>
      <gaussianNoise>0.0</gaussianNoise>
      <frameName>base_link</frameName>
    </plugin>
    <plugin filename="libgazebo_ros_f3d.so" name="f3d_r_gripper_l_finger_controller">
      <alwaysOn>true</alwaysOn>
      <updateRate>100.0</updateRate>
      <bodyName>r_gripper_l_finger_link</bodyName>
      <topicName>r_gripper_l_finger_force_ground_truth</topicName>
    </plugin>
  </gazebo>
  <gazebo reference="r_gripper_r_finger_tip_joint">
    <stopKd value="1.0"/>
    <stopKp value="10000000.0"/>
    <fudgeFactor value="1.0"/>
    <provideFeedback value="true"/>
  </gazebo>
  <!-- parallel link for simulating gripper constraints -->
  <gazebo>
    <link name="r_gripper_l_parallel_link">
      <inertial>
        <mass>0.17126</mass>
        <inertia>
          <ixx>7.7562e-05</ixx>
          <ixy>1.49095e-06</ixy>
          <ixz>-9.83385e-06</ixz>
          <iyy>0.000197083</iyy>
          <iyz>-3.06125e-06</iyz>
          <izz>0.000181054</izz>
        </inertia>
        <pose>0.03598 0.0173 -0.00164 0 0 0</pose>
      </inertial>
      <pose>0.82991 -0.157 0.790675 0 -0 0</pose>
      <gravity>false</gravity>
    </link>
    <link name="r_gripper_r_parallel_link">
      <inertial>
        <mass>0.17389</mass>
        <inertia>
          <ixx>7.73841e-05</ixx>
          <ixy>-2.09309e-06</ixy>
          <ixz>-8.36228e-06</ixz>
          <iyy>0.000198474</iyy>
          <iyz>2.4611e-06</iyz>
          <izz>0.00018107</izz>
        </inertia>
        <pose>0.03576 -0.01736 -0.00095 0 0 0</pose>
      </inertial>
      <pose>0.82991 -0.219 0.790675 0 0 0</pose>
      <gravity>false</gravity>
    </link>
  </gazebo>
  <gazebo>
    <joint name="r_gripper_r_screw_screw_joint" type="screw">
      <child>r_gripper_motor_screw_link</child>
      <parent>r_gripper_r_finger_tip_link</parent>
      <thread_pitch>-3141.6</thread_pitch>
      <axis>
        <xyz>0 1 0</xyz>
      </axis>
    </joint>
    <joint name="r_gripper_l_screw_screw_joint" type="screw">
      <parent>r_gripper_l_finger_tip_link</parent>
      <child>r_gripper_motor_screw_link</child>
      <thread_pitch>3141.6</thread_pitch>
      <axis>
        <xyz>0 1 0</xyz>
      </axis>
    </joint>
  </gazebo>
  <gazebo>
    <joint name="r_gripper_r_parallel_root_joint" type="revolute">
      <parent>r_gripper_r_parallel_link</parent>
      <child>r_gripper_palm_link</child>
      <axis>
        <xyz>0 0 -1</xyz>
        <dynamics>
          <damping>0.2</damping>
        </dynamics>
      </axis>
      <pose>0.05891 -0.031 0 0 0 0</pose>
    </joint>
    <joint name="r_gripper_l_parallel_root_joint" type="revolute">
      <parent>r_gripper_l_parallel_link</parent>
      <child>r_gripper_palm_link</child>
      <axis>
        <xyz>0 0 1</xyz>
        <dynamics>
          <damping>0.2</damping>
        </dynamics>
      </axis>
      <pose>0.05891 0.031 0 0 0 0</pose>
    </joint>
    <joint name="r_gripper_r_parallel_tip_joint" type="revolute">
      <parent>r_gripper_r_parallel_link</parent>
      <child>r_gripper_r_finger_tip_link</child>
      <axis>
        <xyz>0 0 1</xyz>
      </axis>
      <pose>-0.018 -0.021 0 0 0 0</pose>
    </joint>
    <joint name="r_gripper_l_parallel_tip_joint" type="revolute">
      <parent>r_gripper_l_parallel_link</parent>
      <child>r_gripper_l_finger_tip_link</child>
      <axis>
        <xyz>0 0 1</xyz>
      </axis>
      <pose>-0.018 0.021 0 0 0 0</pose>
    </joint>
    <joint name="r_gripper_joint" type="prismatic">
      <parent>r_gripper_r_finger_tip_link</parent>
      <child>r_gripper_l_finger_tip_link</child>
      <axis>
        <xyz>0 1 0</xyz>
      </axis>
    </joint>
  </gazebo>
  <gazebo reference="r_gripper_motor_slider_link">
    <turnGravityOff>true</turnGravityOff>
    <material value="PR2/Red"/>
  </gazebo>
  <gazebo reference="r_gripper_motor_screw_link">
    <turnGravityOff>true</turnGravityOff>
    <material value="PR2/Red"/>
  </gazebo>
  <gazebo reference="r_gripper_l_parallel_link">
    <turnGravityOff>true</turnGravityOff>
    <material value="PR2/Red"/>
  </gazebo>
  <gazebo reference="r_gripper_r_parallel_link">
    <turnGravityOff>true</turnGravityOff>
    <material value="PR2/Red"/>
  </gazebo>
  <!-- fictitous joint that represents the gripper gap -->
  <!-- effort is the linear force at the gripper gap
         velocity limit is the linear velocity limit at the gripper gap
         try and introduce a very stiff spring
         The velocity limits are alpha tested.
         The effort limits are somewhat inflated.
         k_velocity was recently raised from 500.0 to 5000.0.  Tested on beta
    -->
  <joint name="r_gripper_joint" type="prismatic">
    <parent link="r_gripper_r_finger_tip_link"/>
    <child link="r_gripper_l_finger_tip_frame"/>
    <axis xyz="0 1 0"/>
    <dynamics damping="10.0"/>
    <limit effort="1000.0" lower="0.0" upper="0.09" velocity="0.2"/>
    <safety_controller k_position="20.0" k_velocity="5000.0" soft_lower_limit="-0.01" soft_upper_limit="0.088"/>
  </joint>
  <!-- This link is the same as the l_finger_tip_link,
	 but because the urdf does not support graph structures,
	 this link exists twice -->
  <link name="r_gripper_l_finger_tip_frame"/>
  <!-- extensions -->
  <gazebo reference="r_gripper_palm_link">
    <turnGravityOff>true</turnGravityOff>
  </gazebo>
  <gazebo>
    <plugin filename="libgazebo_ros_p3d.so" name="p3d_r_gripper_palm_controller">
      <alwaysOn>true</alwaysOn>
      <updateRate>100.0</updateRate>
      <bodyName>r_gripper_palm_link</bodyName>
      <topicName>r_gripper_palm_pose_ground_truth</topicName>
      <xyzOffsets>0 0 0</xyzOffsets>
      <rpyOffsets>0 0 0</rpyOffsets>
      <gaussianNoise>0.0</gaussianNoise>
      <frameName>map</frameName>
    </plugin>
    <!-- a formal implementation of grasp hack in gazebo with fixed joint -->
    <gripper name="r_grasp_hack">
      <grasp_check>
        <attach_steps>20</attach_steps>
        <detach_steps>40</detach_steps>
        <min_contact_count>2</min_contact_count>
      </grasp_check>
      <gripper_link>r_gripper_r_finger_tip_link</gripper_link>
      <gripper_link>r_gripper_l_finger_tip_link</gripper_link>
      <palm_link>r_gripper_palm_link</palm_link>
    </gripper>
  </gazebo>
  <!-- [lr]_gripper_joint is a fictitious joint, used by transmission for controller gap   -->
  <!-- [lr]_gripper_joint is not attached to any link                                      -->
  <!-- [lr]_gripper_joint position is the gap_size                                         -->
  <!-- [lr]_gripper_joint velocity is the gap linear velocity                              -->
  <!-- [lr]_gripper_joint effort   is the gap linear force                                 -->
  <!-- Please refer to function engineering spreadsheet 090224_link_data.xls for           -->
  <!-- the transmission function.                                                          -->
  <!-- Please refer to mechanism_model/src/pr2_gripper_transmission.cpp for implementation.-->
  <!-- gazebo_mimic_pid is for sim only.                                                   -->
  <transmission name="r_gripper_trans" type="pr2_mechanism_model/PR2GripperTransmission">
    <actuator name="r_gripper_motor"/>
    <gap_joint L0="0.0375528" a="0.0683698" b="0.0433849" gear_ratio="40.095" h="0.0" mechanical_reduction="1.0" name="r_gripper_joint" phi0="0.518518122146" r="0.0915" screw_reduction="0.004" t0="-0.0001914" theta0="0.0628824676201"/>
    <!-- if a gazebo joint exists as [l|r]_gripper_joint, use this tag to have
           gripper transmission apply torque directly to prismatic joint
           this should be the default behavior in diamondback, deprecating this flag -->
    <use_simulated_gripper_joint/>
    <!-- set passive joint angles so things look nice in rviz -->
    <passive_joint name="r_gripper_l_finger_joint"/>
    <passive_joint name="r_gripper_r_finger_joint"/>
    <passive_joint name="r_gripper_r_finger_tip_joint"/>
    <passive_joint name="r_gripper_l_finger_tip_joint"/>
    <!-- screw joint to capture gripper "dynamics" -->
    <simulated_actuated_joint name="r_gripper_motor_screw_joint" passive_actuated_joint="r_gripper_motor_slider_joint" simulated_reduction="3141.6"/>
  </transmission>
  <!-- Shoulder pan -->
  <joint name="l_shoulder_pan_joint" type="revolute">
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 0" xyz="0.0 0.188 0.0"/>
    <!-- transform from parent link to this joint frame -->
    <parent link="torso_lift_link"/>
    <child link="l_shoulder_pan_link"/>
    <limit effort="30" lower="-0.714601836603" upper="2.2853981634" velocity="2.088"/>
    <!-- alpha tested velocity and effort limits -->
    <dynamics damping="10.0"/>
    <safety_controller k_position="100" k_velocity="10" soft_lower_limit="-0.564601836603" soft_upper_limit="2.1353981634"/>
    <!-- joint angle when the rising or the falling flag is activated on PR2 -->
    <calibration rising="0.785398163397"/>
  </joint>
  <link name="l_shoulder_pan_link">
    <inertial>
      <mass value="25.799322"/>
      <origin rpy="0 0 0" xyz="-0.001201 0.024513 -0.098231"/>
      <inertia ixx="0.866179142480" ixy="-0.06086507933" ixz="-0.12118061183" iyy="0.87421714893" iyz="-0.05886609911" izz="0.27353821674"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/shoulder_v0/shoulder_pan.dae"/>
      </geometry>
      <material name="Blue"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.0 0 0.0"/>
      <geometry>
        <mesh filename="meshes/shoulder_v0/shoulder_pan.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- Shoulder lift -->
  <joint name="l_shoulder_lift_joint" type="revolute">
    <axis xyz="0 1 0"/>
    <!-- Limits updated from Function's CAD values as of 2009_02_24 (link_data.xls) -->
    <limit effort="30" lower="-0.5236" upper="1.3963" velocity="2.082"/>
    <!-- alpha tested velocity and effort limits -->
    <safety_controller k_position="100" k_velocity="10" soft_lower_limit="-0.3536" soft_upper_limit="1.2963"/>
    <calibration falling="0.0"/>
    <dynamics damping="10.0"/>
    <origin rpy="0 0 0" xyz="0.1 0 0"/>
    <parent link="l_shoulder_pan_link"/>
    <child link="l_shoulder_lift_link"/>
  </joint>
  <link name="l_shoulder_lift_link">
    <inertial>
      <mass value="2.74988"/>
      <origin rpy="0 0 0" xyz="0.02195 -0.02664 -0.03127"/>
      <inertia ixx="0.02105584615" ixy="0.00496704022" ixz="-0.00194808955" iyy="0.02127223737" iyz="0.00110425490" izz="0.01975753814"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/shoulder_v0/shoulder_lift.dae"/>
      </geometry>
      <material name="Grey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/shoulder_v0/shoulder_lift.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- Upper arm roll is separate macro -->
  <joint name="l_upper_arm_roll_joint" type="revolute">
    <axis xyz="1 0 0"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="l_shoulder_lift_link"/>
    <child link="l_upper_arm_roll_link"/>
    <limit effort="30" lower="-0.8" upper="3.9" velocity="3.27"/>
    <!-- alpha tested velocity and effort limits -->
    <safety_controller k_position="100" k_velocity="2" soft_lower_limit="-0.65" soft_upper_limit="3.75"/>
    <calibration rising="1.57079632679"/>
    <dynamics damping="0.1"/>
  </joint>
  <link name="l_upper_arm_roll_link">
    <inertial>
      <!-- dummy mass, to be removed -->
      <mass value="0.1"/>
      <origin rpy="0 0 0" xyz="0.0 0 0"/>
      <inertia ixx="0.01" ixy="0.00" ixz="0.00" iyy="0.01" iyz="0.00" izz="0.01"/>
    </inertial>
    <visual>
      <!-- TODO: This component doesn't actually have a mesh -->
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/shoulder_v0/upper_arm_roll.stl"/>
      </geometry>
      <material name="RollLinks"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/shoulder_v0/upper_arm_roll_L.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- extensions -->
  <gazebo reference="l_upper_arm_roll_link">
    <material value="PR2/RollLinks"/>
    <turnGravityOff>true</turnGravityOff>
  </gazebo>
  <gazebo reference="l_upper_arm_roll_joint">
    <stopKd value="1.0"/>
    <stopKp value="1000000.0"/>
    <fudgeFactor value="0.5"/>
  </gazebo>
  <!-- Upperarm roll: internal fixed attchment point for upper arm -->
  <transmission name="l_upper_arm_roll_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="l_upper_arm_roll_motor"/>
    <joint name="l_upper_arm_roll_joint"/>
    <mechanicalReduction>32.6525111499</mechanicalReduction>
  </transmission>
  <!-- extensions -->
  <!-- Shoulder pan -->
  <gazebo reference="l_shoulder_pan_link">
    <turnGravityOff>true</turnGravityOff>
  </gazebo>
  <gazebo reference="l_shoulder_pan_joint">
    <stopKd value="1.0"/>
    <stopKp value="1000000.0"/>
  </gazebo>
  <!-- Shoulder lift -->
  <gazebo reference="l_shoulder_lift_link">
    <turnGravityOff>true</turnGravityOff>
  </gazebo>
  <gazebo reference="l_shoulder_lift_joint">
    <stopKd value="1.0"/>
    <stopKp value="1000000.0"/>
  </gazebo>
  <!-- Shoulder pan -->
  <transmission name="l_shoulder_pan_trans" type="pr2_mechanism_model/SimpleTransmission">
    <joint name="l_shoulder_pan_joint"/>
    <actuator name="l_shoulder_pan_motor"/>
    <mechanicalReduction>63.1552452977</mechanicalReduction>
    <compensator k_belt="4000.0" kd_motor="15.0" lambda_combined="0.0" lambda_joint="40.0" lambda_motor="40.0" mass_motor="0.05"/>
  </transmission>
  <!-- Shoulder lift -->
  <transmission name="l_shoulder_lift_trans" type="pr2_mechanism_model/SimpleTransmission">
    <joint name="l_shoulder_lift_joint"/>
    <actuator name="l_shoulder_lift_motor"/>
    <mechanicalReduction>61.8948225713</mechanicalReduction>
    <compensator k_belt="4000.0" kd_motor="10.0" lambda_combined="0.0" lambda_joint="60.0" lambda_motor="60.0" mass_motor="0.05"/>
  </transmission>
  <joint name="l_upper_arm_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="l_upper_arm_roll_link"/>
    <child link="l_upper_arm_link"/>
  </joint>
  <link name="l_upper_arm_link">
    <inertial>
      <!-- NOTE:reflect==-1 for right side, reflect==1 for the left side -->
      <mass value="6.01769"/>
      <origin xyz="0.21405 0.01658 -0.00057"/>
      <inertia ixx="0.01530603856" ixy="-0.00339324862" ixz="0.00060765455" iyy="0.07473694455" iyz="-0.00019953729" izz="0.07601594191"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/upper_arm_v0/upper_arm.dae"/>
      </geometry>
      <material name="Green"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/upper_arm_v0/upper_arm.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- extensions-->
  <gazebo reference="l_upper_arm_link">
    <turnGravityOff>true</turnGravityOff>
  </gazebo>
  <!-- Has upperarm link -->
  <!-- No transmission, since this a fixed joint w/o actuator -->
  <!-- Elbow flex, FA roll macros -->
  <!-- forearm_roll_link is a fictitious link internal to elbow_flex_link, provides an attachment point for the actual forearm -->
  <joint name="l_forearm_roll_joint" type="continuous">
    <axis xyz="1 0 0"/>
    <limit effort="30" velocity="3.6"/>
    <!-- alpha tested velocity and effort limits -->
    <safety_controller k_velocity="1"/>
    <calibration rising="0.0"/>
    <dynamics damping="0.1"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="l_elbow_flex_link"/>
    <child link="l_forearm_roll_link"/>
  </joint>
  <!-- TODO: inertial tag should be optional -->
  <link name="l_forearm_roll_link">
    <inertial>
      <!-- dummy masses, to be removed -->
      <mass value="0.1"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.01" ixy="0.00" ixz="0.00" iyy="0.01" iyz="0.00" izz="0.01"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/upper_arm_v0/forearm_roll.stl"/>
      </geometry>
      <material name="RollLinks"/>
    </visual>
    <!-- TODO: collision tag should be optional -->
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/upper_arm_v0/forearm_roll_L.stl"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="l_forearm_roll_link">
    <material value="PR2/RollLinks"/>
    <turnGravityOff>true</turnGravityOff>
  </gazebo>
  <gazebo reference="l_forearm_roll_joint">
    <fudgeFactor value="0.5"/>
  </gazebo>
  <transmission name="l_forearm_roll_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="l_forearm_roll_motor"/>
    <joint name="l_forearm_roll_joint"/>
    <mechanicalReduction>-90.5142857143</mechanicalReduction>
  </transmission>
  <!-- Elbow flex -->
  <joint name="l_elbow_flex_joint" type="revolute">
    <axis xyz="0 1 0"/>
    <!-- Note: Overtravel limits are 140, -7 degrees instead of 133, 0 -->
    <limit effort="30" lower="-2.3213" upper="0.00" velocity="3.3"/>
    <!-- alpha tested velocity and effort limits -->
    <safety_controller k_position="100" k_velocity="3" soft_lower_limit="-2.1213" soft_upper_limit="-0.15"/>
    <calibration falling="-1.1606"/>
    <dynamics damping="1.0"/>
    <origin rpy="0 0 0" xyz="0.4 0 0"/>
    <parent link="l_upper_arm_link"/>
    <child link="l_elbow_flex_link"/>
  </joint>
  <link name="l_elbow_flex_link">
    <inertial>
      <mass value="1.90327"/>
      <origin xyz="0.01014 0.00032 -0.01211"/>
      <inertia ixx="0.00346541989" ixy="0.00004066825" ixz="0.00043171614" iyy="0.00441606455" iyz="-0.00003968914" izz="0.00359156824"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/upper_arm_v0/elbow_flex.dae"/>
      </geometry>
      <material name="Grey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/upper_arm_v0/elbow_flex.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- testing initial joint position, to be parsed at urdf2gazebo parse time ->
    <gazebo reference="${side}_elbow_flex_joint">
      <initial_joint_position>-1.0</initial_joint_position>
    </gazebo>
    -->
  <gazebo reference="l_elbow_flex_link">
    <turnGravityOff>true</turnGravityOff>
  </gazebo>
  <gazebo reference="l_elbow_flex_joint">
    <stopKd value="1.0"/>
    <stopKp value="1000000.0"/>
  </gazebo>
  <!-- Elbow flex -->
  <transmission name="l_elbow_flex_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="l_elbow_flex_motor"/>
    <joint name="l_elbow_flex_joint"/>
    <mechanicalReduction>-36.167452007</mechanicalReduction>
  </transmission>
  <joint name="l_forearm_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <!-- transform from parent link to this joint frame -->
    <parent link="l_forearm_roll_link"/>
    <child link="l_forearm_link"/>
  </joint>
  <link name="l_forearm_link">
    <inertial>
      <mass value="2.57968"/>
      <origin xyz="0.18791 -0.00017 -0.00912"/>
      <inertia ixx="0.00364857222" ixy="0.00005215877" ixz="0.00071534842" iyy="0.01507736897" iyz="-0.00001310770" izz="0.01659310749"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/forearm_v0/forearm.dae"/>
      </geometry>
      <material name="Grey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/forearm_v0/forearm.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- Wrist flex -->
  <joint name="l_wrist_flex_joint" type="revolute">
    <axis xyz="0 1 0"/>
    <limit effort="10" lower="-2.18" upper="0.0" velocity="3.078"/>
    <!-- alpha tested velocity and effort limits -->
    <safety_controller k_position="20" k_velocity="4" soft_lower_limit="-2.0" soft_upper_limit="-0.1"/>
    <dynamics damping="0.1"/>
    <calibration falling="-0.5410521"/>
    <origin rpy="0 0 0" xyz="0.321 0 0"/>
    <parent link="l_forearm_link"/>
    <child link="l_wrist_flex_link"/>
  </joint>
  <link name="l_wrist_flex_link">
    <inertial>
      <mass value="0.61402"/>
      <origin xyz="-0.00157 0.0 -0.00075"/>
      <inertia ixx="0.00065165722" ixy="0.00000028864" ixz="0.00000303477" iyy="0.00019824443" iyz="-0.00000022645" izz="0.00064450498"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/forearm_v0/wrist_flex.dae"/>
      </geometry>
      <material name="Grey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/forearm_v0/wrist_flex.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- Wrist roll -->
  <joint name="l_wrist_roll_joint" type="continuous">
    <axis xyz="1 0 0"/>
    <limit effort="10" velocity="3.6"/>
    <!-- alpha tested velocity and effort limits -->
    <safety_controller k_velocity="2"/>
    <dynamics damping="0.1"/>
    <calibration rising="-1.57079632679"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="l_wrist_flex_link"/>
    <child link="l_wrist_roll_link"/>
  </joint>
  <link name="l_wrist_roll_link">
    <inertial>
      <!-- dummy masses, to be removed.  wrist roll masses are on "gripper_palm" -->
      <mass value="0.1"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/forearm_v0/wrist_roll.stl"/>
      </geometry>
      <material name="RollLinks"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/forearm_v0/wrist_roll_L.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- extensions-->
  <gazebo reference="l_forearm_link">
    <turnGravityOff>true</turnGravityOff>
  </gazebo>
  <!-- Wrist flex -->
  <gazebo reference="l_wrist_flex_link">
    <turnGravityOff>true</turnGravityOff>
  </gazebo>
  <gazebo reference="l_wrist_flex_joint">
    <stopKd value="1.0"/>
    <stopKp value="1000000.0"/>
  </gazebo>
  <!-- Wrist roll -->
  <gazebo reference="l_wrist_roll_link">
    <turnGravityOff>true</turnGravityOff>
    <material value="PR2/RollLinks"/>
  </gazebo>
  <gazebo reference="l_wrist_roll_joint">
    <fudgeFactor value="0.5"/>
  </gazebo>
  <!-- Wrist flex , Wrist roll -->
  <transmission name="l_wrist_trans" type="pr2_mechanism_model/WristTransmission">
    <rightActuator mechanicalReduction="60.1714285714" name="l_wrist_r_motor"/>
    <leftActuator mechanicalReduction="60.1714285714" name="l_wrist_l_motor"/>
    <flexJoint mechanicalReduction="-1.0" name="l_wrist_flex_joint"/>
    <rollJoint mechanicalReduction="1.0" name="l_wrist_roll_joint"/>
  </transmission>
  <joint name="l_gripper_palm_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="l_wrist_roll_link"/>
    <child link="l_gripper_palm_link"/>
  </joint>
  <link name="l_gripper_palm_link">
    <inertial>
      <mass value="0.58007"/>
      <origin rpy="0 0 0" xyz="0.06623 0.00053 -0.00119"/>
      <inertia ixx="0.00035223921" ixy="-0.00001580476" ixz="-0.00000091750" iyy="0.00067741312" iyz="-0.00000059554" izz="0.00086563316"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/gripper_v0/gripper_palm.dae"/>
      </geometry>
      <material name="Red"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/gripper_v0/gripper_palm.stl"/>
      </geometry>
    </collision>
  </link>
  <joint name="l_gripper_led_joint" type="fixed">
    <!--  Need to check if we need a positive or negative Z term -->
    <origin xyz="0.0513 0.0 .0244"/>
    <parent link="l_gripper_palm_link"/>
    <child link="l_gripper_led_frame"/>
  </joint>
  <link name="l_gripper_led_frame"/>
  <joint name="l_gripper_motor_accelerometer_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="l_gripper_palm_link"/>
    <child link="l_gripper_motor_accelerometer_link"/>
  </joint>
  <link name="l_gripper_motor_accelerometer_link">
    <inertial>
      <mass value="0.001"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.001 0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="l_gripper_tool_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.18 0 0"/>
    <parent link="l_gripper_palm_link"/>
    <child link="l_gripper_tool_frame"/>
  </joint>
  <link name="l_gripper_tool_frame"/>
  <!-- actuated motor screw joint -->
  <link name="l_gripper_motor_slider_link">
    <inertial>
      <mass value="0.01"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
    </inertial>
    <!-- for debugging only
      <visual>
        <origin xyz="0 0 0" rpy="1.5708 0 0" />
        <geometry>
          <cylinder length="0.002" radius="0.025"/>
        </geometry>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="1.5708 0 0" />
        <geometry>
          <cylinder length="0.002" radius="0.025"/>
        </geometry>
      </collision>
      -->
  </link>
  <joint name="l_gripper_motor_slider_joint" type="prismatic">
    <origin rpy="0 0 0" xyz="0.16828 0 0"/>
    <axis xyz="1 0 0"/>
    <parent link="l_gripper_palm_link"/>
    <child link="l_gripper_motor_slider_link"/>
    <limit effort="1000.0" lower="-0.1" upper="0.1" velocity="0.2"/>
  </joint>
  <link name="l_gripper_motor_screw_link">
    <inertial>
      <mass value="0.01"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
    </inertial>
    <!-- for debugging only
      <visual>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <box size="0.05 0.001 0.05" />
        </geometry>
      </visual>
      <collision>
        <origin xyz="0 0 0" rpy="0 0 0" />
        <geometry>
          <box size="0.05 0.001 0.05" />
        </geometry>
      </collision>
      -->
  </link>
  <joint name="l_gripper_motor_screw_joint" type="continuous">
    <origin rpy="0 0 0" xyz="0.0 0 0"/>
    <axis xyz="0 1 0"/>
    <parent link="l_gripper_motor_slider_link"/>
    <child link="l_gripper_motor_screw_link"/>
    <dynamics damping="0.0001"/>
  </joint>
  <!-- pr2 fingers macro -->
  <!-- Finger proximal digit -->
  <joint name="l_gripper_l_finger_joint" type="revolute">
    <axis xyz="0 0 1"/>
    <!-- limits on passive finger and finger top joints without
         transmission are not enforced by safety controllers.
         The lower/upper limits and are enforced in PR2 simulation and
         effort and velocity limits are ignored. This is also needed because
         these joints are declared revolute rather than continuous.-->
    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
    <dynamics damping="0.02"/>
    <origin rpy="0 0 0" xyz="0.07691 0.01 0"/>
    <parent link="l_gripper_palm_link"/>
    <child link="l_gripper_l_finger_link"/>
  </joint>
  <link name="l_gripper_l_finger_link">
    <inertial>
      <mass value="0.17126"/>
      <origin rpy="0 0 0" xyz="0.03598 0.01730 -0.00164"/>
      <inertia ixx="0.00007756198" ixy="0.00000149095" ixz="-0.00000983385" iyy="0.00019708305" iyz="-0.00000306125" izz="0.00018105446"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/gripper_v0/l_finger.dae"/>
      </geometry>
      <material name="Grey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/gripper_v0/l_finger.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- Finger proximal digit -->
  <joint name="l_gripper_r_finger_joint" type="revolute">
    <axis xyz="0 0 -1"/>
    <origin rpy="0 0 0" xyz="0.07691 -0.01 0"/>
    <!-- limits on passive finger and finger top joints without
         transmission are not enforced by safety controllers.
         The lower/upper limits and are enforced in PR2 simulation and
         effort and velocity limits are ignored. This is also needed because
         these joints are declared revolute rather than continuous.-->
    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
    <dynamics damping="0.02"/>
    <mimic joint="l_gripper_l_finger_joint" multiplier="1" offset="0"/>
    <parent link="l_gripper_palm_link"/>
    <child link="l_gripper_r_finger_link"/>
  </joint>
  <link name="l_gripper_r_finger_link">
    <inertial>
      <mass value="0.17389"/>
      <origin rpy="0 0 0" xyz="0.03576 -0.01736 -0.00095"/>
      <inertia ixx="0.00007738410" ixy="-0.00000209309" ixz="-0.00000836228" iyy="0.00019847383" iyz="0.00000246110" izz="0.00018106988"/>
    </inertial>
    <visual>
      <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/gripper_v0/l_finger.dae"/>
      </geometry>
      <material name="Grey"/>
    </visual>
    <collision>
      <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/gripper_v0/l_finger.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- Finger tip -->
  <joint name="l_gripper_l_finger_tip_joint" type="revolute">
    <axis xyz="0 0 -1"/>
    <origin rpy="0 0 0" xyz="0.09137 0.00495 0"/>
    <!-- limits on passive finger and finger top joints without
         transmission are not enforced by safety controllers.
         The lower/upper limits and are enforced in PR2 simulation and
         effort and velocity limits are ignored. This is also needed because
         these joints are declared revolute rather than continuous.-->
    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
    <dynamics damping="0.001"/>
    <mimic joint="l_gripper_l_finger_joint" multiplier="1" offset="0"/>
    <parent link="l_gripper_l_finger_link"/>
    <child link="l_gripper_l_finger_tip_link"/>
  </joint>
  <link name="l_gripper_l_finger_tip_link">
    <inertial>
      <mass value="0.04419"/>
      <origin rpy="0 0 0" xyz="0.00423 0.00284 0.0"/>
      <inertia ixx="0.00000837047" ixy="0.00000583632" ixz="0.0" iyy="0.00000987067" iyz="0.0" izz="0.00001541768"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/gripper_v0/l_finger_tip.dae"/>
      </geometry>
      <material name="Green"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/gripper_v0/l_finger_tip.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- Finger tip -->
  <joint name="l_gripper_r_finger_tip_joint" type="revolute">
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 0" xyz="0.09137 -0.00495 0"/>
    <!-- limits on passive finger and finger top joints without
         transmission are not enforced by safety controllers.
         The lower/upper limits and are enforced in PR2 simulation and
         effort and velocity limits are ignored. This is also needed because
         these joints are declared revolute rather than continuous.-->
    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
    <dynamics damping="0.001"/>
    <mimic joint="l_gripper_l_finger_joint" multiplier="1" offset="0"/>
    <parent link="l_gripper_r_finger_link"/>
    <child link="l_gripper_r_finger_tip_link"/>
  </joint>
  <link name="l_gripper_r_finger_tip_link">
    <inertial>
      <mass value="0.04419"/>
      <origin rpy="0 0 0" xyz="0.00423 -0.00284 0.0"/>
      <inertia ixx="0.00000837047" ixy="-0.00000583632" ixz="0.0" iyy="0.00000987067" iyz="0.0" izz="0.00001541768"/>
    </inertial>
    <visual>
      <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/gripper_v0/l_finger_tip.dae"/>
      </geometry>
      <material name="Green"/>
    </visual>
    <collision>
      <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="meshes/gripper_v0/l_finger_tip.stl"/>
      </geometry>
    </collision>
  </link>
  <!-- extensions -->
  <!-- Finger proximal digit -->
  <gazebo reference="l_gripper_l_finger_link">
    <turnGravityOff>true</turnGravityOff>
    <mu1 value="500.0"/>
    <mu2 value="500.0"/>
    <kp value="1000000.0"/>
    <kd value="1.0"/>
    <!-- for "${prefix}_l_finger_joint"-->
  </gazebo>
  <gazebo reference="l_gripper_l_finger_joint">
    <stopKd value="1.0"/>
    <stopKp value="10000000.0"/>
    <fudgeFactor value="1.0"/>
    <provideFeedback value="true"/>
  </gazebo>
  <!-- Finger proximal digit -->
  <gazebo reference="l_gripper_r_finger_link">
    <turnGravityOff>true</turnGravityOff>
    <mu1 value="500.0"/>
    <mu2 value="500.0"/>
    <kp value="1000000.0"/>
    <kd value="1.0"/>
  </gazebo>
  <gazebo reference="l_gripper_r_finger_joint">
    <stopKd value="1.0"/>
    <stopKp value="10000000.0"/>
    <fudgeFactor value="1.0"/>
    <provideFeedback value="true"/>
  </gazebo>
  <!-- Finger tip -->
  <gazebo reference="l_gripper_l_finger_tip_link">
    <turnGravityOff>true</turnGravityOff>
    <selfCollide>false</selfCollide>
    <sensor name="l_gripper_l_finger_tip_contact_sensor" type="contact">
      <update_rate>100.0</update_rate>
      <contact>
        <collision>l_gripper_l_finger_tip_link_collision</collision>
      </contact>
      <plugin filename="libgazebo_ros_bumper.so" name="l_gripper_l_finger_tip_gazebo_ros_bumper_controller">
        <alwaysOn>true</alwaysOn>
        <frameName>l_gripper_l_finger_tip_link</frameName>
        <updateRate>100.0</updateRate>
        <bumperTopicName>l_gripper_l_finger_tip_bumper</bumperTopicName>
      </plugin>
    </sensor>
    <mu1 value="500.0"/>
    <mu2 value="500.0"/>
    <kp value="10000000.0"/>
    <kd value="1.0"/>
  </gazebo>
  <gazebo reference="l_gripper_l_finger_tip_joint">
    <stopKd value="1.0"/>
    <stopKp value="10000000.0"/>
    <fudgeFactor value="1.0"/>
    <provideFeedback value="true"/>
  </gazebo>
  <!-- Finger tip -->
  <gazebo reference="l_gripper_r_finger_tip_link">
    <turnGravityOff>true</turnGravityOff>
    <selfCollide>false</selfCollide>
    <sensor name="l_gripper_r_finger_tip_contact_sensor" type="contact">
      <update_rate>100.0</update_rate>
      <contact>
        <collision>l_gripper_r_finger_tip_link_collision</collision>
      </contact>
      <plugin filename="libgazebo_ros_bumper.so" name="l_gripper_r_finger_tip_gazebo_ros_bumper_controller">
        <alwaysOn>true</alwaysOn>
        <frameName>l_gripper_r_finger_tip_link</frameName>
        <updateRate>100.0</updateRate>
        <bumperTopicName>l_gripper_r_finger_tip_bumper</bumperTopicName>
      </plugin>
    </sensor>
    <mu1 value="500.0"/>
    <mu2 value="500.0"/>
    <kp value="10000000.0"/>
    <kd value="1.0"/>
  </gazebo>
  <gazebo>
    <plugin filename="libgazebo_ros_p3d.so" name="p3d_l_gripper_l_finger_controller">
      <alwaysOn>true</alwaysOn>
      <updateRate>100.0</updateRate>
      <bodyName>l_gripper_l_finger_link</bodyName>
      <topicName>l_gripper_l_finger_pose_ground_truth</topicName>
      <gaussianNoise>0.0</gaussianNoise>
      <frameName>base_link</frameName>
    </plugin>
    <plugin filename="libgazebo_ros_f3d.so" name="f3d_l_gripper_l_finger_controller">
      <alwaysOn>true</alwaysOn>
      <updateRate>100.0</updateRate>
      <bodyName>l_gripper_l_finger_link</bodyName>
      <topicName>l_gripper_l_finger_force_ground_truth</topicName>
    </plugin>
  </gazebo>
  <gazebo reference="l_gripper_r_finger_tip_joint">
    <stopKd value="1.0"/>
    <stopKp value="10000000.0"/>
    <fudgeFactor value="1.0"/>
    <provideFeedback value="true"/>
  </gazebo>
  <!-- parallel link for simulating gripper constraints -->
  <gazebo>
    <link name="l_gripper_l_parallel_link">
      <inertial>
        <mass>0.17126</mass>
        <inertia>
          <ixx>7.7562e-05</ixx>
          <ixy>1.49095e-06</ixy>
          <ixz>-9.83385e-06</ixz>
          <iyy>0.000197083</iyy>
          <iyz>-3.06125e-06</iyz>
          <izz>0.000181054</izz>
        </inertia>
        <pose>0.03598 0.0173 -0.00164 0 0 0</pose>
      </inertial>
      <pose>0.82991 0.219 0.790675 0 -0 0</pose>
      <gravity>false</gravity>
    </link>
    <link name="l_gripper_r_parallel_link">
      <inertial>
        <mass>0.17389</mass>
        <inertia>
          <ixx>7.73841e-05</ixx>
          <ixy>-2.09309e-06</ixy>
          <ixz>-8.36228e-06</ixz>
          <iyy>0.000198474</iyy>
          <iyz>2.4611e-06</iyz>
          <izz>0.00018107</izz>
        </inertia>
        <pose>0.03576 -0.01736 -0.00095 0 0 0</pose>
      </inertial>
      <pose>0.82991 0.157 0.790675 0 0 0</pose>
      <gravity>false</gravity>
    </link>
  </gazebo>
  <gazebo>
    <joint name="l_gripper_r_screw_screw_joint" type="screw">
      <child>l_gripper_motor_screw_link</child>
      <parent>l_gripper_r_finger_tip_link</parent>
      <thread_pitch>-3141.6</thread_pitch>
      <axis>
        <xyz>0 1 0</xyz>
      </axis>
    </joint>
    <joint name="l_gripper_l_screw_screw_joint" type="screw">
      <parent>l_gripper_l_finger_tip_link</parent>
      <child>l_gripper_motor_screw_link</child>
      <thread_pitch>3141.6</thread_pitch>
      <axis>
        <xyz>0 1 0</xyz>
      </axis>
    </joint>
  </gazebo>
  <gazebo>
    <joint name="l_gripper_r_parallel_root_joint" type="revolute">
      <parent>l_gripper_r_parallel_link</parent>
      <child>l_gripper_palm_link</child>
      <axis>
        <xyz>0 0 -1</xyz>
        <dynamics>
          <damping>0.2</damping>
        </dynamics>
      </axis>
      <pose>0.05891 -0.031 0 0 0 0</pose>
    </joint>
    <joint name="l_gripper_l_parallel_root_joint" type="revolute">
      <parent>l_gripper_l_parallel_link</parent>
      <child>l_gripper_palm_link</child>
      <axis>
        <xyz>0 0 1</xyz>
        <dynamics>
          <damping>0.2</damping>
        </dynamics>
      </axis>
      <pose>0.05891 0.031 0 0 0 0</pose>
    </joint>
    <joint name="l_gripper_r_parallel_tip_joint" type="revolute">
      <parent>l_gripper_r_parallel_link</parent>
      <child>l_gripper_r_finger_tip_link</child>
      <axis>
        <xyz>0 0 1</xyz>
      </axis>
      <pose>-0.018 -0.021 0 0 0 0</pose>
    </joint>
    <joint name="l_gripper_l_parallel_tip_joint" type="revolute">
      <parent>l_gripper_l_parallel_link</parent>
      <child>l_gripper_l_finger_tip_link</child>
      <axis>
        <xyz>0 0 1</xyz>
      </axis>
      <pose>-0.018 0.021 0 0 0 0</pose>
    </joint>
    <joint name="l_gripper_joint" type="prismatic">
      <parent>l_gripper_r_finger_tip_link</parent>
      <child>l_gripper_l_finger_tip_link</child>
      <axis>
        <xyz>0 1 0</xyz>
      </axis>
    </joint>
  </gazebo>
  <gazebo reference="l_gripper_motor_slider_link">
    <turnGravityOff>true</turnGravityOff>
    <material value="PR2/Red"/>
  </gazebo>
  <gazebo reference="l_gripper_motor_screw_link">
    <turnGravityOff>true</turnGravityOff>
    <material value="PR2/Red"/>
  </gazebo>
  <gazebo reference="l_gripper_l_parallel_link">
    <turnGravityOff>true</turnGravityOff>
    <material value="PR2/Red"/>
  </gazebo>
  <gazebo reference="l_gripper_r_parallel_link">
    <turnGravityOff>true</turnGravityOff>
    <material value="PR2/Red"/>
  </gazebo>
  <!-- fictitous joint that represents the gripper gap -->
  <!-- effort is the linear force at the gripper gap
         velocity limit is the linear velocity limit at the gripper gap
         try and introduce a very stiff spring
         The velocity limits are alpha tested.
         The effort limits are somewhat inflated.
         k_velocity was recently raised from 500.0 to 5000.0.  Tested on beta
    -->
  <joint name="l_gripper_joint" type="prismatic">
    <parent link="l_gripper_r_finger_tip_link"/>
    <child link="l_gripper_l_finger_tip_frame"/>
    <axis xyz="0 1 0"/>
    <dynamics damping="10.0"/>
    <limit effort="1000.0" lower="0.0" upper="0.09" velocity="0.2"/>
    <safety_controller k_position="20.0" k_velocity="5000.0" soft_lower_limit="-0.01" soft_upper_limit="0.088"/>
  </joint>
  <!-- This link is the same as the l_finger_tip_link,
	 but because the urdf does not support graph structures,
	 this link exists twice -->
  <link name="l_gripper_l_finger_tip_frame"/>
  <!-- extensions -->
  <gazebo reference="l_gripper_palm_link">
    <turnGravityOff>true</turnGravityOff>
  </gazebo>
  <gazebo>
    <plugin filename="libgazebo_ros_p3d.so" name="p3d_l_gripper_palm_controller">
      <alwaysOn>true</alwaysOn>
      <updateRate>100.0</updateRate>
      <bodyName>l_gripper_palm_link</bodyName>
      <topicName>l_gripper_palm_pose_ground_truth</topicName>
      <xyzOffsets>0 0 0</xyzOffsets>
      <rpyOffsets>0 0 0</rpyOffsets>
      <gaussianNoise>0.0</gaussianNoise>
      <frameName>map</frameName>
    </plugin>
    <!-- a formal implementation of grasp hack in gazebo with fixed joint -->
    <gripper name="l_grasp_hack">
      <grasp_check>
        <attach_steps>20</attach_steps>
        <detach_steps>40</detach_steps>
        <min_contact_count>2</min_contact_count>
      </grasp_check>
      <gripper_link>l_gripper_r_finger_tip_link</gripper_link>
      <gripper_link>l_gripper_l_finger_tip_link</gripper_link>
      <palm_link>l_gripper_palm_link</palm_link>
    </gripper>
  </gazebo>
  <!-- [lr]_gripper_joint is a fictitious joint, used by transmission for controller gap   -->
  <!-- [lr]_gripper_joint is not attached to any link                                      -->
  <!-- [lr]_gripper_joint position is the gap_size                                         -->
  <!-- [lr]_gripper_joint velocity is the gap linear velocity                              -->
  <!-- [lr]_gripper_joint effort   is the gap linear force                                 -->
  <!-- Please refer to function engineering spreadsheet 090224_link_data.xls for           -->
  <!-- the transmission function.                                                          -->
  <!-- Please refer to mechanism_model/src/pr2_gripper_transmission.cpp for implementation.-->
  <!-- gazebo_mimic_pid is for sim only.                                                   -->
  <transmission name="l_gripper_trans" type="pr2_mechanism_model/PR2GripperTransmission">
    <actuator name="l_gripper_motor"/>
    <gap_joint L0="0.0375528" a="0.0683698" b="0.0433849" gear_ratio="40.095" h="0.0" mechanical_reduction="1.0" name="l_gripper_joint" phi0="0.518518122146" r="0.0915" screw_reduction="0.004" t0="-0.0001914" theta0="0.0628824676201"/>
    <!-- if a gazebo joint exists as [l|r]_gripper_joint, use this tag to have
           gripper transmission apply torque directly to prismatic joint
           this should be the default behavior in diamondback, deprecating this flag -->
    <use_simulated_gripper_joint/>
    <!-- set passive joint angles so things look nice in rviz -->
    <passive_joint name="l_gripper_l_finger_joint"/>
    <passive_joint name="l_gripper_r_finger_joint"/>
    <passive_joint name="l_gripper_r_finger_tip_joint"/>
    <passive_joint name="l_gripper_l_finger_tip_joint"/>
    <!-- screw joint to capture gripper "dynamics" -->
    <simulated_actuated_joint name="l_gripper_motor_screw_joint" passive_actuated_joint="l_gripper_motor_slider_joint" simulated_reduction="3141.6"/>
  </transmission>
  <!-- Forearm cam Position is a guess, based on full robot calibration -->
  <!-- Forearm cam Orientation is from Function -->
  <joint name="l_forearm_cam_frame_joint" type="fixed">
    <origin rpy="-1.57079632679 -0.562868683768 0" xyz=".135 0 .044"/>
    <parent link="l_forearm_roll_link"/>
    <child link="l_forearm_cam_frame"/>
  </joint>
  <link name="l_forearm_cam_frame">
    <inertial>
      <mass value="0.01"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
    </inertial>
  </link>
  <joint name="l_forearm_cam_optical_frame_joint" type="fixed">
    <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
    <parent link="l_forearm_cam_frame"/>
    <child link="l_forearm_cam_optical_frame"/>
  </joint>
  <link name="l_forearm_cam_optical_frame"/>
  <!-- extensions -->
  <gazebo reference="l_forearm_cam_frame">
    <sensor name="l_forearm_cam_sensor" type="camera">
      <always_on>true</always_on>
      <update_rate>25.0</update_rate>
      <camera>
        <horizontal_fov>1.57079632679</horizontal_fov>
        <image>
          <format>R8G8B8</format>
          <width>640</width>
          <height>480</height>
        </image>
        <clip>
          <near>0.1</near>
          <far>100</far>
        </clip>
      </camera>
      <plugin filename="libgazebo_ros_camera.so" name="l_forearm_cam_controller">
        <alwaysOn>true</alwaysOn>
        <updateRate>25.0</updateRate>
        <cameraName>l_forearm_cam</cameraName>
        <imageTopicName>image_raw</imageTopicName>
        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
        <frameName>l_forearm_cam_optical_frame</frameName>
        <hackBaseline>0</hackBaseline>
        <CxPrime>320.5</CxPrime>
        <Cx>320.5</Cx>
        <Cy>240.5</Cy>
        <!-- image_width / (2*tan(hfov_radian /2)) -->
        <!-- 320 for wide and 772.55 for narrow stereo camera -->
        <focalLength>320.000105</focalLength>
        <distortionK1>0.0</distortionK1>
        <distortionK2>0.0</distortionK2>
        <distortionK3>0.0</distortionK3>
        <distortionT1>0.0</distortionT1>
        <distortionT2>0.0</distortionT2>
      </plugin>
    </sensor>
    <turnGravityOff>true</turnGravityOff>
    <material>PR2/Blue</material>
  </gazebo>
  <joint name="r_forearm_cam_frame_joint" type="fixed">
    <origin rpy="1.57079632679 -0.562868683768 0" xyz=".135 0 .044"/>
    <parent link="r_forearm_roll_link"/>
    <child link="r_forearm_cam_frame"/>
  </joint>
  <link name="r_forearm_cam_frame">
    <inertial>
      <mass value="0.01"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
    </inertial>
  </link>
  <joint name="r_forearm_cam_optical_frame_joint" type="fixed">
    <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
    <parent link="r_forearm_cam_frame"/>
    <child link="r_forearm_cam_optical_frame"/>
  </joint>
  <link name="r_forearm_cam_optical_frame"/>
  <!-- extensions -->
  <gazebo reference="r_forearm_cam_frame">
    <sensor name="r_forearm_cam_sensor" type="camera">
      <always_on>true</always_on>
      <update_rate>25.0</update_rate>
      <camera>
        <horizontal_fov>1.57079632679</horizontal_fov>
        <image>
          <format>R8G8B8</format>
          <width>640</width>
          <height>480</height>
        </image>
        <clip>
          <near>0.1</near>
          <far>100</far>
        </clip>
      </camera>
      <plugin filename="libgazebo_ros_camera.so" name="r_forearm_cam_controller">
        <alwaysOn>true</alwaysOn>
        <updateRate>25.0</updateRate>
        <cameraName>r_forearm_cam</cameraName>
        <imageTopicName>image_raw</imageTopicName>
        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
        <frameName>r_forearm_cam_optical_frame</frameName>
        <hackBaseline>0</hackBaseline>
        <CxPrime>320.5</CxPrime>
        <Cx>320.5</Cx>
        <Cy>240.5</Cy>
        <!-- image_width / (2*tan(hfov_radian /2)) -->
        <!-- 320 for wide and 772.55 for narrow stereo camera -->
        <focalLength>320.000105</focalLength>
        <distortionK1>0.0</distortionK1>
        <distortionK2>0.0</distortionK2>
        <distortionK3>0.0</distortionK3>
        <distortionT1>0.0</distortionT1>
        <distortionT2>0.0</distortionT2>
      </plugin>
    </sensor>
    <turnGravityOff>true</turnGravityOff>
    <material>PR2/Blue</material>
  </gazebo>
  <!-- Kinect models -->
  <!-- Kinect2 xacro -->
  <!-- Kinect1 xacro -->
</robot>