From fdd1094772ef51a95bf558ca3e8a2b1a7931e88a Mon Sep 17 00:00:00 2001
From: wim <devnull@localhost>
Date: Tue, 22 Jun 2010 22:07:27 +0000
Subject: [PATCH] update kdl parser to comply with latest urdf bugfixes

---
 kdl_parser/test/pr2_desc.xml        | 6504 +++++++++++++--------------
 kdl_parser/test/test_kdl_parser.cpp |    6 +-
 2 files changed, 3219 insertions(+), 3291 deletions(-)

diff --git a/kdl_parser/test/pr2_desc.xml b/kdl_parser/test/pr2_desc.xml
index df794c6..07308a0 100644
--- a/kdl_parser/test/pr2_desc.xml
+++ b/kdl_parser/test/pr2_desc.xml
@@ -1,26 +1,19 @@
-<?xml version="1.0" ?><robot name="pr2" xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom" 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">
-  
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from /u/wim/cturtle_wg_all/stacks/pr2_common/pr2_description/robots/pr2.urdf.xacro | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="pr2" xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom" 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? -->
-  
   <!-- PR2 Arm -->
-  
-  
-  
   <!-- PR2 gripper -->
-  
   <!-- PR2 head -->
-  
   <!-- PR2 tilting laser mount -->
-  
   <!-- PR2 torso -->
-  
   <!-- PR2 base -->
-  
   <!-- Head sensors -->
-  
   <!-- Camera sensors -->
-  
   <!-- generic simulator_gazebo plugins for starting mechanism control, ros time, ros battery -->
   <gazebo>
     <controller:gazebo_ros_controller_manager name="gazebo_ros_controller_manager" plugin="libgazebo_ros_controller_manager.so">
@@ -28,46 +21,52 @@
       <updateRate>1000.0</updateRate>
       <interface:audio name="gazebo_ros_controller_manager_dummy_iface"/>
     </controller:gazebo_ros_controller_manager>
-
-    <controller:gazebo_ros_battery name="gazebo_ros_battery_controller" plugin="libgazebo_ros_battery.so">
+    <controller:gazebo_ros_power_monitor name="gazebo_ros_power_monitor_controller" plugin="libgazebo_ros_power_monitor.so">
       <alwaysOn>true</alwaysOn>
       <updateRate>1.0</updateRate>
       <timeout>5</timeout>
-      <interface:audio name="battery_dummy_interface"/>
+      <interface:audio name="power_monitor_dummy_interface"/>
       <powerStateTopic>power_state</powerStateTopic>
       <powerStateRate>10.0</powerStateRate>
-      <fullChargeCapacity>87.78</fullChargeCapacity>     
+      <fullChargeCapacity>87.78</fullChargeCapacity>
       <dischargeRate>-474</dischargeRate>
       <chargeRate>525</chargeRate>
       <dischargeVoltage>15.52</dischargeVoltage>
       <chargeVoltage>16.41</chargeVoltage>
-    </controller:gazebo_ros_battery>
+    </controller:gazebo_ros_power_monitor>
   </gazebo>
   <!-- materials for visualization -->
   <material name="Blue">
     <color rgba="0.0 0.0 0.8 1.0"/>
-  </material><material name="Green">
+  </material>
+  <material name="Green">
     <color rgba="0.0 0.8 0.0 1.0"/>
-  </material><material name="Grey">
+  </material>
+  <material name="Grey">
     <color rgba="0.7 0.7 0.7 1.0"/>
-  </material><material name="Grey2">
+  </material>
+  <material name="Grey2">
     <color rgba="0.9 0.9 0.9 1.0"/>
-  </material><material name="Red">
+  </material>
+  <material name="Red">
     <color rgba="0.8 0.0 0.0 1.0"/>
-  </material><material name="White">
+  </material>
+  <material name="White">
     <color rgba="1.0 1.0 1.0 1.0"/>
-  </material><material name="Caster">
+  </material>
+  <material name="Caster">
     <texture filename="package://pr2_ogre/Media/materials/textures/pr2_caster_texture.png"/>
-  </material><material name="Wheel_l">
-    <texture filename="package://pr2_ogre/Media/materials/textures/pr2_wheel_left.png"/>
-  </material><material name="Wheel_r">
-    <texture filename="package://pr2_ogre/Media/materials/textures/pr2_wheel_right.png"/>
-  </material><material name="RollLinks">
+  </material>
+  <material name="Wheel_l">
+    <texture filename="package://pr2_ogre/Media/materials/textures/pr2_wheel_left.png"/>
+  </material>
+  <material name="Wheel_r">
+    <texture filename="package://pr2_ogre/Media/materials/textures/pr2_wheel_right.png"/>
+  </material>
+  <material name="RollLinks">
     <texture filename="package://pr2_ogre/Media/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, 
@@ -75,819 +74,915 @@
        (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 -->
-  <joint name="base_joint" type="planar">
-      <origin rpy="0 0 0" xyz="0 0 0.051"/>
-      <parent link="world"/>
-      <child link="base_link"/>
-    </joint><link name="base_link">
-
-      <inertial>
-        <mass value="116.0"/>
-        <origin xyz="-0.061 0.0 0.293"/>
-        <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="package://pr2_description/meshes/base_v0/base.stl"/>
-        </geometry>
-        
-        <material name="Green"/>
-      </visual>
-
-      <collision>
-        <!-- represent base collision with a simple rectangular model, positioned by base_size_z s.t. top
+  <link name="base_link">
+    <inertial>
+      <mass value="116.0"/>
+      <origin xyz="-0.061 0.0 0.293"/>
+      <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="package://pr2_description/meshes/base_v0/base.stl"/>
+      </geometry>
+      <material name="White"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/base_v0/base_L.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <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.21"/>
-        <geometry>
-          <box size="0.65 0.65 0.04"/>
-        </geometry>
-      </collision>
-    </link><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>
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.001 0.001 0.001"/>
-        </geometry>
-        <material name="Red"/>
-      </visual>
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size=".001 .001 .001"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="base_laser_link">
-      <sensor:ray name="base_laser_link">
-        <rayCount>640</rayCount>
-        <rangeCount>640</rangeCount>
-        <laserCount>1</laserCount>
-
-        <origin>0.0 0.0 0.0</origin>
-        <displayRays>false</displayRays>
-
-        <minAngle>-80</minAngle>
-        <maxAngle> 80</maxAngle>
-
-        <minRange>0.05</minRange>
-        <maxRange>10.0</maxRange>
-        <resRange>0.01</resRange>
-        <updateRate>20.0</updateRate>
-        <controller:gazebo_ros_laser name="gazebo_ros_base_laser_link_controller" plugin="libgazebo_ros_laser.so">
-          <gaussianNoise>0.005</gaussianNoise>
-          <alwaysOn>true</alwaysOn>
-          <updateRate>20.0</updateRate>
-          <topicName>base_scan</topicName>
-          <frameName>base_laser_link</frameName>
-          <interface:laser name="gazebo_ros_base_laser_link_iface"/>
-        </controller:gazebo_ros_laser>
-      </sensor:ray>
-    </gazebo><joint name="fl_caster_rotation_joint" type="continuous">
-      <axis xyz="0 0 1"/>
-      <limit effort="100" velocity="100"/>
-      <safety_controller k_velocity="10"/>
-      <calibration reference_position="0.0"/>
-      <dynamics damping="0.0" friction="0.0"/>
-      <origin rpy="0 0 0" xyz="0.2225 0.2225 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="package://pr2_description/meshes/base_v0/caster.stl"/>
-        </geometry>
-        
-        <material name="Green"/>
-      </visual>
-
-      <collision>
-        <origin rpy="0.0 0.0 0.0" xyz="0 0 0.07"/>
-        <geometry>
-          <box size="0.192 0.164 0.013"/>
-        </geometry>
-      </collision>
-    </link><transmission name="fl_caster_rotation_trans" type="SimpleTransmission">
-      <actuator name="fl_caster_rotation_motor"/>
-      <joint name="fl_caster_rotation_joint"/>
-      <mechanicalReduction>-79.2380952381</mechanicalReduction>
-    </transmission><joint name="fl_caster_l_wheel_joint" type="continuous">
-      <axis xyz="0 1 0"/>
-      <limit effort="100" velocity="100"/>
-      <safety_controller k_velocity="10"/>
-      <dynamics damping="0.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="package://pr2_description/meshes/base_v0/pr2_wheel.stl"/>
-        </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><gazebo reference="fl_caster_l_wheel_link">
-      <mu1 value="100.0"/>
-      <mu2 value="100.0"/>
-      <kp value="1000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/wheel_l"/>
-    </gazebo><transmission name="fl_caster_l_wheel_trans" type="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="100" velocity="100"/>
-      <safety_controller k_velocity="10"/>
-      <dynamics damping="0.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="package://pr2_description/meshes/base_v0/pr2_wheel.stl"/>
-        </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><gazebo reference="fl_caster_r_wheel_link">
-      <mu1 value="100.0"/>
-      <mu2 value="100.0"/>
-      <kp value="1000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/wheel_r"/>
-    </gazebo><transmission name="fl_caster_r_wheel_trans" type="SimpleTransmission">
-      <actuator name="fl_caster_r_wheel_motor"/>
-      <joint name="fl_caster_r_wheel_joint"/>
-      <mechanicalReduction>-79.2380952381</mechanicalReduction>
-    </transmission><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="100" velocity="100"/>
-      <safety_controller k_velocity="10"/>
-      <calibration reference_position="0.0"/>
-      <dynamics damping="0.0" friction="0.0"/>
-      <origin rpy="0 0 0" xyz="0.2225 -0.2225 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="package://pr2_description/meshes/base_v0/caster.stl"/>
-        </geometry>
-        
-        <material name="Green"/>
-      </visual>
-
-      <collision>
-        <origin rpy="0.0 0.0 0.0" xyz="0 0 0.07"/>
-        <geometry>
-          <box size="0.192 0.164 0.013"/>
-        </geometry>
-      </collision>
-    </link><transmission name="fr_caster_rotation_trans" type="SimpleTransmission">
-      <actuator name="fr_caster_rotation_motor"/>
-      <joint name="fr_caster_rotation_joint"/>
-      <mechanicalReduction>-79.2380952381</mechanicalReduction>
-    </transmission><joint name="fr_caster_l_wheel_joint" type="continuous">
-      <axis xyz="0 1 0"/>
-      <limit effort="100" velocity="100"/>
-      <safety_controller k_velocity="10"/>
-      <dynamics damping="0.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="package://pr2_description/meshes/base_v0/pr2_wheel.stl"/>
-        </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><gazebo reference="fr_caster_l_wheel_link">
-      <mu1 value="100.0"/>
-      <mu2 value="100.0"/>
-      <kp value="1000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/wheel_l"/>
-    </gazebo><transmission name="fr_caster_l_wheel_trans" type="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="100" velocity="100"/>
-      <safety_controller k_velocity="10"/>
-      <dynamics damping="0.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="package://pr2_description/meshes/base_v0/pr2_wheel.stl"/>
-        </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><gazebo reference="fr_caster_r_wheel_link">
-      <mu1 value="100.0"/>
-      <mu2 value="100.0"/>
-      <kp value="1000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/wheel_r"/>
-    </gazebo><transmission name="fr_caster_r_wheel_trans" type="SimpleTransmission">
-      <actuator name="fr_caster_r_wheel_motor"/>
-      <joint name="fr_caster_r_wheel_joint"/>
-      <mechanicalReduction>-79.2380952381</mechanicalReduction>
-    </transmission><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="100" velocity="100"/>
-      <safety_controller k_velocity="10"/>
-      <calibration reference_position="0.0"/>
-      <dynamics damping="0.0" friction="0.0"/>
-      <origin rpy="0 0 0" xyz="-0.2225 0.2225 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="package://pr2_description/meshes/base_v0/caster.stl"/>
-        </geometry>
-        
-        <material name="Green"/>
-      </visual>
-
-      <collision>
-        <origin rpy="0.0 0.0 0.0" xyz="0 0 0.07"/>
-        <geometry>
-          <box size="0.192 0.164 0.013"/>
-        </geometry>
-      </collision>
-    </link><transmission name="bl_caster_rotation_trans" type="SimpleTransmission">
-      <actuator name="bl_caster_rotation_motor"/>
-      <joint name="bl_caster_rotation_joint"/>
-      <mechanicalReduction>-79.2380952381</mechanicalReduction>
-    </transmission><joint name="bl_caster_l_wheel_joint" type="continuous">
-      <axis xyz="0 1 0"/>
-      <limit effort="100" velocity="100"/>
-      <safety_controller k_velocity="10"/>
-      <dynamics damping="0.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="package://pr2_description/meshes/base_v0/pr2_wheel.stl"/>
-        </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><gazebo reference="bl_caster_l_wheel_link">
-      <mu1 value="100.0"/>
-      <mu2 value="100.0"/>
-      <kp value="1000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/wheel_l"/>
-    </gazebo><transmission name="bl_caster_l_wheel_trans" type="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="100" velocity="100"/>
-      <safety_controller k_velocity="10"/>
-      <dynamics damping="0.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="package://pr2_description/meshes/base_v0/pr2_wheel.stl"/>
-        </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><gazebo reference="bl_caster_r_wheel_link">
-      <mu1 value="100.0"/>
-      <mu2 value="100.0"/>
-      <kp value="1000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/wheel_r"/>
-    </gazebo><transmission name="bl_caster_r_wheel_trans" type="SimpleTransmission">
-      <actuator name="bl_caster_r_wheel_motor"/>
-      <joint name="bl_caster_r_wheel_joint"/>
-      <mechanicalReduction>-79.2380952381</mechanicalReduction>
-    </transmission><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="100" velocity="100"/>
-      <safety_controller k_velocity="10"/>
-      <calibration reference_position="0.0"/>
-      <dynamics damping="0.0" friction="0.0"/>
-      <origin rpy="0 0 0" xyz="-0.2225 -0.2225 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="package://pr2_description/meshes/base_v0/caster.stl"/>
-        </geometry>
-        
-        <material name="Green"/>
-      </visual>
-
-      <collision>
-        <origin rpy="0.0 0.0 0.0" xyz="0 0 0.07"/>
-        <geometry>
-          <box size="0.192 0.164 0.013"/>
-        </geometry>
-      </collision>
-    </link><transmission name="br_caster_rotation_trans" type="SimpleTransmission">
-      <actuator name="br_caster_rotation_motor"/>
-      <joint name="br_caster_rotation_joint"/>
-      <mechanicalReduction>-79.2380952381</mechanicalReduction>
-    </transmission><joint name="br_caster_l_wheel_joint" type="continuous">
-      <axis xyz="0 1 0"/>
-      <limit effort="100" velocity="100"/>
-      <safety_controller k_velocity="10"/>
-      <dynamics damping="0.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="package://pr2_description/meshes/base_v0/pr2_wheel.stl"/>
-        </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><gazebo reference="br_caster_l_wheel_link">
-      <mu1 value="100.0"/>
-      <mu2 value="100.0"/>
-      <kp value="1000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/wheel_l"/>
-    </gazebo><transmission name="br_caster_l_wheel_trans" type="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="100" velocity="100"/>
-      <safety_controller k_velocity="10"/>
-      <dynamics damping="0.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="package://pr2_description/meshes/base_v0/pr2_wheel.stl"/>
-        </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><gazebo reference="br_caster_r_wheel_link">
-      <mu1 value="100.0"/>
-      <mu2 value="100.0"/>
-      <kp value="1000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/wheel_r"/>
-    </gazebo><transmission name="br_caster_r_wheel_trans" type="SimpleTransmission">
-      <actuator name="br_caster_r_wheel_motor"/>
-      <joint name="br_caster_r_wheel_joint"/>
-      <mechanicalReduction>-79.2380952381</mechanicalReduction>
-    </transmission><gazebo reference="br_caster_rotation_link">
-      <material value="PR2/caster_texture"/>
-    </gazebo><gazebo reference="base_link">
-      <selfCollide>true</selfCollide>
-      <sensor:contact name="base_contact_sensor">
-        <geom>base_link_geom</geom>
-        <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="base_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>base_bumper</bumperTopicName>
-          <interface:bumper name="base_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <material value="PR2/Green"/>
-    </gazebo><gazebo>
-      <controller:gazebo_ros_p3d name="p3d_base_controller" plugin="libgazebo_ros_p3d.so">
+      <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>
+  <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>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="Red"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size=".001 .001 .001"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="base_laser_link">
+    <sensor:ray name="base_laser">
+      <rayCount>640</rayCount>
+      <rangeCount>640</rangeCount>
+      <laserCount>1</laserCount>
+      <origin>0.0 0.0 0.0</origin>
+      <displayRays>false</displayRays>
+      <minAngle>-129.998394137</minAngle>
+      <maxAngle>129.998394137</maxAngle>
+      <minRange>0.05</minRange>
+      <maxRange>10.0</maxRange>
+      <resRange>0.01</resRange>
+      <updateRate>20</updateRate>
+      <controller:gazebo_ros_laser name="gazebo_ros_base_laser_controller" plugin="libgazebo_ros_laser.so">
+        <gaussianNoise>0.005</gaussianNoise>
+        <alwaysOn>true</alwaysOn>
+        <updateRate>20</updateRate>
+        <topicName>base_scan</topicName>
+        <frameName>base_laser_link</frameName>
+        <interface:laser name="gazebo_ros_base_laser_iface"/>
+      </controller:gazebo_ros_laser>
+    </sensor:ray>
+  </gazebo>
+  <joint name="fl_caster_rotation_joint" type="continuous">
+    <axis xyz="0 0 1"/>
+    <limit effort="100" velocity="100"/>
+    <!-- alpha tested velocity and effort limits -->
+    <safety_controller k_velocity="10"/>
+    <calibration reference_position="-0.785398163397" rising="-0.785398163397"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <dynamics damping="0.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="package://pr2_description/meshes/base_v0/caster.stl"/>
+      </geometry>
+      <material name="Caster"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/base_v0/caster_L.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <transmission name="fl_caster_rotation_trans" type="SimpleTransmission">
+    <actuator name="fl_caster_rotation_motor"/>
+    <joint name="fl_caster_rotation_joint"/>
+    <mechanicalReduction>-79.2380952381</mechanicalReduction>
+  </transmission>
+  <joint name="fl_caster_l_wheel_joint" type="continuous">
+    <axis xyz="0 1 0"/>
+    <limit effort="100" velocity="100"/>
+    <!-- alpha tested effort and velocity limits -->
+    <safety_controller k_velocity="10"/>
+    <dynamics damping="0.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="package://pr2_description/meshes/base_v0/wheel.stl"/>
+      </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>
+  <gazebo reference="fl_caster_l_wheel_link">
+    <mu1 value="100.0"/>
+    <mu2 value="100.0"/>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+    <material value="PR2/wheel_l"/>
+  </gazebo>
+  <transmission name="fl_caster_l_wheel_trans" type="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="100" velocity="100"/>
+    <!-- alpha tested effort and velocity limits -->
+    <safety_controller k_velocity="10"/>
+    <dynamics damping="0.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="package://pr2_description/meshes/base_v0/wheel.stl"/>
+      </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>
+  <gazebo reference="fl_caster_r_wheel_link">
+    <mu1 value="100.0"/>
+    <mu2 value="100.0"/>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+    <material value="PR2/wheel_r"/>
+  </gazebo>
+  <transmission name="fl_caster_r_wheel_trans" type="SimpleTransmission">
+    <actuator name="fl_caster_r_wheel_motor"/>
+    <joint name="fl_caster_r_wheel_joint"/>
+    <mechanicalReduction>-79.2380952381</mechanicalReduction>
+  </transmission>
+  <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="100" velocity="100"/>
+    <!-- alpha tested velocity and effort limits -->
+    <safety_controller k_velocity="10"/>
+    <calibration reference_position="-0.785398163397" rising="-0.785398163397"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <dynamics damping="0.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="package://pr2_description/meshes/base_v0/caster.stl"/>
+      </geometry>
+      <material name="Caster"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/base_v0/caster_L.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <transmission name="fr_caster_rotation_trans" type="SimpleTransmission">
+    <actuator name="fr_caster_rotation_motor"/>
+    <joint name="fr_caster_rotation_joint"/>
+    <mechanicalReduction>-79.2380952381</mechanicalReduction>
+  </transmission>
+  <joint name="fr_caster_l_wheel_joint" type="continuous">
+    <axis xyz="0 1 0"/>
+    <limit effort="100" velocity="100"/>
+    <!-- alpha tested effort and velocity limits -->
+    <safety_controller k_velocity="10"/>
+    <dynamics damping="0.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="package://pr2_description/meshes/base_v0/wheel.stl"/>
+      </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>
+  <gazebo reference="fr_caster_l_wheel_link">
+    <mu1 value="100.0"/>
+    <mu2 value="100.0"/>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+    <material value="PR2/wheel_l"/>
+  </gazebo>
+  <transmission name="fr_caster_l_wheel_trans" type="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="100" velocity="100"/>
+    <!-- alpha tested effort and velocity limits -->
+    <safety_controller k_velocity="10"/>
+    <dynamics damping="0.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="package://pr2_description/meshes/base_v0/wheel.stl"/>
+      </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>
+  <gazebo reference="fr_caster_r_wheel_link">
+    <mu1 value="100.0"/>
+    <mu2 value="100.0"/>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+    <material value="PR2/wheel_r"/>
+  </gazebo>
+  <transmission name="fr_caster_r_wheel_trans" type="SimpleTransmission">
+    <actuator name="fr_caster_r_wheel_motor"/>
+    <joint name="fr_caster_r_wheel_joint"/>
+    <mechanicalReduction>-79.2380952381</mechanicalReduction>
+  </transmission>
+  <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="100" velocity="100"/>
+    <!-- alpha tested velocity and effort limits -->
+    <safety_controller k_velocity="10"/>
+    <calibration reference_position="2.35619449019" rising="2.35619449019"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <dynamics damping="0.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="package://pr2_description/meshes/base_v0/caster.stl"/>
+      </geometry>
+      <material name="Caster"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/base_v0/caster_L.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <transmission name="bl_caster_rotation_trans" type="SimpleTransmission">
+    <actuator name="bl_caster_rotation_motor"/>
+    <joint name="bl_caster_rotation_joint"/>
+    <mechanicalReduction>-79.2380952381</mechanicalReduction>
+  </transmission>
+  <joint name="bl_caster_l_wheel_joint" type="continuous">
+    <axis xyz="0 1 0"/>
+    <limit effort="100" velocity="100"/>
+    <!-- alpha tested effort and velocity limits -->
+    <safety_controller k_velocity="10"/>
+    <dynamics damping="0.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="package://pr2_description/meshes/base_v0/wheel.stl"/>
+      </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>
+  <gazebo reference="bl_caster_l_wheel_link">
+    <mu1 value="100.0"/>
+    <mu2 value="100.0"/>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+    <material value="PR2/wheel_l"/>
+  </gazebo>
+  <transmission name="bl_caster_l_wheel_trans" type="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="100" velocity="100"/>
+    <!-- alpha tested effort and velocity limits -->
+    <safety_controller k_velocity="10"/>
+    <dynamics damping="0.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="package://pr2_description/meshes/base_v0/wheel.stl"/>
+      </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>
+  <gazebo reference="bl_caster_r_wheel_link">
+    <mu1 value="100.0"/>
+    <mu2 value="100.0"/>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+    <material value="PR2/wheel_r"/>
+  </gazebo>
+  <transmission name="bl_caster_r_wheel_trans" type="SimpleTransmission">
+    <actuator name="bl_caster_r_wheel_motor"/>
+    <joint name="bl_caster_r_wheel_joint"/>
+    <mechanicalReduction>-79.2380952381</mechanicalReduction>
+  </transmission>
+  <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="100" velocity="100"/>
+    <!-- alpha tested velocity and effort limits -->
+    <safety_controller k_velocity="10"/>
+    <calibration reference_position="2.35619449019" rising="2.35619449019"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <dynamics damping="0.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="package://pr2_description/meshes/base_v0/caster.stl"/>
+      </geometry>
+      <material name="Caster"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/base_v0/caster_L.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <transmission name="br_caster_rotation_trans" type="SimpleTransmission">
+    <actuator name="br_caster_rotation_motor"/>
+    <joint name="br_caster_rotation_joint"/>
+    <mechanicalReduction>-79.2380952381</mechanicalReduction>
+  </transmission>
+  <joint name="br_caster_l_wheel_joint" type="continuous">
+    <axis xyz="0 1 0"/>
+    <limit effort="100" velocity="100"/>
+    <!-- alpha tested effort and velocity limits -->
+    <safety_controller k_velocity="10"/>
+    <dynamics damping="0.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="package://pr2_description/meshes/base_v0/wheel.stl"/>
+      </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>
+  <gazebo reference="br_caster_l_wheel_link">
+    <mu1 value="100.0"/>
+    <mu2 value="100.0"/>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+    <material value="PR2/wheel_l"/>
+  </gazebo>
+  <transmission name="br_caster_l_wheel_trans" type="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="100" velocity="100"/>
+    <!-- alpha tested effort and velocity limits -->
+    <safety_controller k_velocity="10"/>
+    <dynamics damping="0.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="package://pr2_description/meshes/base_v0/wheel.stl"/>
+      </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>
+  <gazebo reference="br_caster_r_wheel_link">
+    <mu1 value="100.0"/>
+    <mu2 value="100.0"/>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+    <material value="PR2/wheel_r"/>
+  </gazebo>
+  <transmission name="br_caster_r_wheel_trans" type="SimpleTransmission">
+    <actuator name="br_caster_r_wheel_motor"/>
+    <joint name="br_caster_r_wheel_joint"/>
+    <mechanicalReduction>-79.2380952381</mechanicalReduction>
+  </transmission>
+  <gazebo reference="br_caster_rotation_link">
+    <material value="PR2/caster_texture"/>
+  </gazebo>
+  <gazebo reference="base_link">
+    <selfCollide>true</selfCollide>
+    <sensor:contact name="base_contact_sensor">
+      <geom>base_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="base_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
         <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>25.7 25.7 0</xyzOffsets> <!-- initialize odometry for fake localization-->
-        <rpyOffsets>0 0 0</rpyOffsets>
-        <interface:position name="p3d_base_position"/>
-      </controller:gazebo_ros_p3d>
-    </gazebo>
-
+        <bumperTopicName>base_bumper</bumperTopicName>
+        <interface:bumper name="base_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <material value="PR2/caster_texture"/>
+  </gazebo>
+  <gazebo>
+    <controller:gazebo_ros_p3d name="p3d_base_controller" plugin="libgazebo_ros_p3d.so">
+      <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>25.7 25.7 0</xyzOffsets>
+      <!-- initialize odometry for fake localization-->
+      <rpyOffsets>0 0 0</rpyOffsets>
+      <interface:position name="p3d_base_position"/>
+    </controller:gazebo_ros_p3d>
+    <canonicalBody>base_footprint</canonicalBody>
+  </gazebo>
   <joint name="torso_lift_joint" type="prismatic">
-      <axis xyz="0 0 1"/>
-      <!-- KWatts: Max should be 0.31, changed to 0.20 to prevent jamming -->
-      <!-- HW ticket 481, will examine spine to determine failures -->
-      <limit effort="10000" lower="0.0" upper="0.20" velocity="0.015"/>
-      <safety_controller k_position="1000" k_velocity="400000" soft_lower_limit="0.005" soft_upper_limit="0.195"/>
-      <calibration reference_position="0.00536"/>
-      <dynamics damping="10.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 name="torso_lift_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="torso_lift_visual_geom">
-          <mesh filename="package://pr2_description/meshes/torso_v0/torso.stl"/>
-        </geometry>
-        
-        <material name="Grey2"/>
-      </visual>
-
-      <collision name="torso_lift_collision">
-        <origin rpy="0 0 0" xyz="0 0 0.16"/>
-        <geometry name="torso_lift_collision_geom">
-          <!-- arbitrarily chosen collision box by JH, torso collision is not important for now -->
-          <box size="0.30 0.620 0.02"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="torso_lift_link">
-      <sensor:contact name="torso_lift_contact_sensor">
-        <geom>torso_lift_link_geom</geom>
+    <axis xyz="0 0 1"/>
+    <limit effort="10000" lower="0.0" upper="0.31" 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.305"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <calibration falling="0.00536" reference_position="0.00536"/>
+    <dynamics damping="20000.0"/>
+    <!-- strictly tuned for sim only right now -->
+    <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="package://pr2_description/meshes/torso_v0/torso_lift.stl"/>
+      </geometry>
+      <material name="Grey2"/>
+    </visual>
+    <collision name="torso_lift_collision">
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/torso_v0/torso_lift_L.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="torso_lift_link">
+    <sensor:contact name="torso_lift_contact_sensor">
+      <geom>torso_lift_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="torso_lift_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
         <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="torso_lift_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>torso_lift_bumper</bumperTopicName>
-          <interface:bumper name="torso_lift_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <material value="PR2/Grey2"/>
-    </gazebo><transmission name="torso_lift_trans" type="SimpleTransmission">
-      <actuator name="torso_lift_motor"/>
-      <joint name="torso_lift_joint"/>
-      <mechanicalReduction>-52143.33</mechanicalReduction>
-    </transmission>
-
+        <bumperTopicName>torso_lift_bumper</bumperTopicName>
+        <interface:bumper name="torso_lift_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <material value="PR2/Grey2"/>
+  </gazebo>
+  <transmission name="torso_lift_trans" type="SimpleTransmission">
+    <actuator name="torso_lift_motor"/>
+    <joint name="torso_lift_joint"/>
+    <mechanicalReduction>-52143.33</mechanicalReduction>
+  </transmission>
+  <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>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="Red"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size=".001 .001 .001"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo>
+    <controller:gazebo_ros_imu name="imu_controller" plugin="libgazebo_ros_imu.so">
+      <alwaysOn>true</alwaysOn>
+      <updateRate>100.0</updateRate>
+      <bodyName>imu_link</bodyName>
+      <topicName>torso_lift_imu/data</topicName>
+      <gaussianNoise>2.89e-08</gaussianNoise>
+      <xyzOffsets>0 0 0</xyzOffsets>
+      <rpyOffsets>0 0 0</rpyOffsets>
+      <interface:position name="imu_position"/>
+    </controller:gazebo_ros_imu>
+  </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"/>
-      <safety_controller k_position="100" k_velocity="1.5" soft_lower_limit="-2.857" soft_upper_limit="2.857"/>
-      <calibration reference_position="0.0"/>
-      <dynamics damping="1.0"/>
-      <origin rpy="0.0 0.0 0.0" xyz="0.0    0.0     0.3915"/>
-      <parent link="torso_lift_link"/>
-      <child link="head_pan_link"/>
-    </joint><link name="head_pan_link">
-
-
-      <inertial>
-        <mass value="1.611118"/>
-        <origin rpy="0 0 0" xyz="-0.005717  0.010312 -0.029649"/>
-        <inertia ixx="0.00482611007" ixy="-0.000144683999" ixz="0.000110076136" iyy="0.005218991412" iyz="-0.000314239509" izz="0.008618784925"/>
-      </inertial>
-
-      <visual name="head_pan_visual">
-        <origin rpy="0 0 0 " xyz="0 0 0.0"/>
-        <geometry name="head_pan_visual_geom">
-          <mesh filename="package://pr2_description/meshes/head_v0/head_pan.stl"/>
-        </geometry>
-        
-        <material name="Blue"/>
-      </visual>
-
-      <collision name="head_pan_collision">
-        <origin rpy="0.0 0.0 0.0 " xyz="-0.005717  0.010312 -0.029649"/>
-        <geometry name="head_pan_collision_geom">
-          <box size="0.188 0.219 0.137"/>
-        </geometry>
-      </collision>
-
-    </link><gazebo reference="head_pan_link">
-      <material value="PR2/Blue"/>
-    </gazebo><transmission name="head_pan_trans" type="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="15" lower="-0.471238" upper="1.39626" velocity="5"/>
-      <safety_controller k_position="100" k_velocity="1.5" soft_lower_limit="-0.3712" soft_upper_limit="1.29626"/>
-      <calibration reference_position="0.0"/>
-      <dynamics damping="20.0"/>
-      <origin rpy="0 0 0" xyz="0.058 0 0"/>
-      <parent link="head_pan_link"/>
-      <child link="head_tilt_link"/>
-    </joint><link name="head_tilt_link">
-
-
-      <inertial>
-        <mass value="1.749727"/>
-        <origin rpy="0 0 0" xyz="0.041935 0.003569 0.028143"/>
-        <inertia ixx="0.010602303435" ixy="-0.000408814235" ixz="0.00198303894" iyy="0.011874383747" iyz="0.000197908779" izz="0.005516790626"/>
-      </inertial>
-
-      <visual name="head_tilt_visual">
-        <origin rpy="0.0 0.0 0.0 " xyz="0 0 0"/>
-        <geometry name="head_tilt_visual_geom">
-          <mesh filename="package://pr2_description/meshes/head_v0/head_tilt.stl"/>
-        </geometry>
-        
-        <material name="Green"/>
-      </visual>
-
-      <collision name="head_tilt_collision">
-        <origin rpy="0 0 0" xyz="0.041935 0.003569 0.028143"/>
-        <geometry name="head_tilt_collision_geom">
-          <box size="0.064 0.253 0.181"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="head_tilt_link">
-      <material value="PR2/Green"/>
-    </gazebo><transmission name="head_tilt_trans" type="SimpleTransmission">
-      <actuator name="head_tilt_motor"/>
-      <joint name="head_tilt_joint"/>
-      <mechanicalReduction>6.0</mechanicalReduction>
-    </transmission><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 name="head_plate_frame_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="head_plate_frame_visual_geom">
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-        
-        <material name="Blue"/>
-      </visual>
-
-      <collision name="head_plate_frame_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="head_plate_frame_collision_geom">
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="head_plate_frame">
-      <material value="PR2/Blue"/>
-    </gazebo>
-
+    <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"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <calibration reference_position="0.0" rising="0.0"/>
+    <dynamics damping="1.0"/>
+    <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="1.611118"/>
+      <origin rpy="0 0 0" xyz="-0.005717  0.010312 -0.029649"/>
+      <inertia ixx="0.00482611007" ixy="-0.000144683999" ixz="0.000110076136" iyy="0.005218991412" iyz="-0.000314239509" izz="0.008618784925"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0 " xyz="0 0 0.0"/>
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/head_v0/head_pan.stl"/>
+      </geometry>
+      <material name="Blue"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0 " xyz="0 0 0.0"/>
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/head_v0/head_pan_L.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="head_pan_link">
+    <material value="Gazebo/Black"/>
+  </gazebo>
+  <transmission name="head_pan_trans" type="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="15" lower="-0.471238" upper="1.39626" velocity="5"/>
+    <!-- alpha tested velocity and effort limits -->
+    <safety_controller k_position="100" k_velocity="1.5" soft_lower_limit="-0.3712" soft_upper_limit="1.29626"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <calibration falling="0.0" reference_position="0.0"/>
+    <dynamics damping="1.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="1.749727"/>
+      <origin rpy="0 0 0" xyz="0.041935 0.003569 0.028143"/>
+      <inertia ixx="0.010602303435" ixy="-0.000408814235" ixz="0.00198303894" iyy="0.011874383747" iyz="0.000197908779" izz="0.005516790626"/>
+    </inertial>
+    <visual>
+      <origin rpy="0.0 0.0 0.0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/head_v0/head_tilt.stl"/>
+      </geometry>
+      <material name="Green"/>
+    </visual>
+    <collision>
+      <origin rpy="0.0 0.0 0.0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/head_v0/head_tilt_L.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="head_tilt_link">
+    <material value="Gazebo/White"/>
+  </gazebo>
+  <transmission name="head_tilt_trans" type="SimpleTransmission">
+    <actuator name="head_tilt_motor"/>
+    <joint name="head_tilt_joint"/>
+    <mechanicalReduction>6.0</mechanicalReduction>
+  </transmission>
+  <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>
+  <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 name="sensor_mount_sensor_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="sensor_mount_sensor_visual_geom">
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-        
-        <material name="Blue"/>
-      </visual>
-
-      <collision name="sensor_mount_sensor_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="sensor_mount_sensor_collision_geom">
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </collision>
-    </link><joint name="high_def_frame_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="0 -0.109 0.035"/>
-      <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>
-
-      <visual>
-        <!-- bogus values for visual aesthetics -->
-        <origin rpy="0 0 0" xyz="-0.02 0 0"/>
-        <geometry>
-          <box size="0.04 0.04 0.04"/>
-        </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><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>
-
-      <visual>
-        <!-- bogus values for visual aesthetics -->
-        <origin rpy="0 0 1.57079632679" xyz="0 0 0"/>
-        <geometry>
-          <cylinder length="0.05" radius="0.02"/>
-        </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><gazebo reference="high_def_frame">
+    <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>
+    <collision name="sensor_mount_sensor_collision">
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="high_def_frame_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 -0.109 0.035"/>
+    <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>
+    <visual>
+      <!-- bogus values for visual aesthetics -->
+      <origin rpy="0 0 0" xyz="-0.02 0 0"/>
+      <geometry>
+        <box size="0.04 0.04 0.04"/>
+      </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>
+  <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>
+    <visual>
+      <!-- bogus values for visual aesthetics -->
+      <origin rpy="0 0 1.57079632679" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.05" radius="0.02"/>
+      </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>
+  <gazebo reference="high_def_frame">
     <sensor:camera name="high_def_sensor">
       <imageFormat>R8G8B8</imageFormat>
       <imageSize>2448 2050</imageSize>
@@ -898,2551 +993,2384 @@
       <controller:gazebo_ros_prosilica name="high_def_controller" plugin="libgazebo_ros_prosilica.so">
         <alwaysOn>true</alwaysOn>
         <updateRate>20.0</updateRate>
-        <camInfoTopicName>/prosilica/cam_info</camInfoTopicName>
-        <imageTopicName>/prosilica/image</imageTopicName>
-        <imageRectTopicName>/prosilica/image_rect</imageRectTopicName>
-        <camInfoServiceName>/prosilica/cam_info_service</camInfoServiceName>
-        <pollServiceName>/prosilica/poll</pollServiceName>
-        <frameName>hight_def_optical_frame</frameName>
+        <imageTopicName>/prosilica/image_raw</imageTopicName>
+        <cameraInfoTopicName>/prosilica/camera_info</cameraInfoTopicName>
+        <pollServiceName>/prosilica/request_image</pollServiceName>
+        <frameName>high_def_frame</frameName>
         <CxPrime>1224.5</CxPrime>
         <Cx>1224.5</Cx>
         <Cy>1025.5</Cy>
-        <focal_length>2955</focal_length> <!-- image_width / (2*tan(hfov_radian /2)) -->
-        <distortion_k1>0</distortion_k1>
-        <distortion_k2>0</distortion_k2>
-        <distortion_k3>0</distortion_k3>
-        <distortion_t1>0</distortion_t1>
-        <distortion_t2>0</distortion_t2>
+        <focal_length>2955</focal_length>
+        <!-- image_width / (2*tan(hfov_radian /2)) -->
+        <distortion_k1>0.00000001</distortion_k1>
+        <distortion_k2>0.00000001</distortion_k2>
+        <distortion_k3>0.00000001</distortion_k3>
+        <distortion_t1>0.00000001</distortion_t1>
+        <distortion_t2>0.00000001</distortion_t2>
         <interface:camera name="high_def_iface"/>
       </controller:gazebo_ros_prosilica>
     </sensor:camera>
-    <material value="PR2/Blue"/>
-  </gazebo><gazebo reference="high_def_optical_frame">
-    <material value="PR2/Blue"/>
-  </gazebo><joint name="double_stereo_frame_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.003"/>
-      <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 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><joint name="wide_stereo_frame_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="0 -0.015 0"/>
-      <parent link="double_stereo_link"/>
-      <child link="wide_stereo_link"/>
-    </joint><link name="wide_stereo_link">
-      
-      <inertial>
-        <mass value="0.1"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <inertia ixx="0.00482611007" ixy="-0.000144683999" ixz="0.000110076136" iyy="0.005218991412" iyz="-0.000314239509" izz="0.008618784925"/>
-      </inertial>
-
-      <visual>
-        <origin rpy="0 0 0" xyz="0.0 0 0.05"/>
-        <geometry>
-          <box size="0.005 0.01 0.005"/>
-        </geometry>
-        
-        <material name="Blue"/>
-      </visual>
-
-      <collision>
-        <origin rpy="0 0 0" xyz="0.0 0 0.05"/>
-        <geometry>
-          <box size="0.005 0.01 0.005"/>
-        </geometry>
-      </collision>
-      
-    </link><joint name="wide_stereo_optical_frame_joint" type="fixed">
-      <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0.0            0.045   0.0305"/>
-      <parent link="wide_stereo_link"/>
-      <child link="wide_stereo_optical_frame"/>
-    </joint><link name="wide_stereo_optical_frame" type="camera">
-
-      <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.001 0.001 0.001"/>
-        </geometry>
-        
-        <material name="Blue"/>
-      </visual>
-
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.0 0.0 0.0"/>
-        </geometry>
-      </collision>
-
-    </link><joint name="wide_stereo_l_stereo_camera_frame_joint" type="fixed">
-      <origin rpy="0 -1.57079632679 1.57079632679" xyz="0 0 0"/>
-      <parent link="wide_stereo_optical_frame"/>
-      <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>
-
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </visual>
-
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </collision>
-    </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">
-
-
-      <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>
-
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </visual>
-
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="wide_stereo_l_stereo_camera_frame">
-      <sensor:camera name="wide_stereo_l_stereo_camera_sensor">
-        <!--imageSize>${image_width} ${image_height}</imageSize-->
-        <imageSize>640 480</imageSize> <!-- FIXME: temporary fix because xacro expands above line to floats, not integers -->
-        <imageFormat>B8G8R8</imageFormat>
-        <hfov>90.0</hfov>
-        <nearClip>0.1</nearClip>
-        <farClip>100</farClip>
-        <updateRate>25.0</updateRate>
-        <controller:gazebo_ros_camera name="wide_stereo_l_stereo_camera_controller" plugin="libgazebo_ros_camera.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>25.0</updateRate>
-          <imageTopicName>wide_stereo/left/image_raw</imageTopicName>
-          <cameraInfoTopicName>wide_stereo/left/camera_info</cameraInfoTopicName>
-          <frameName>wide_stereo_optical_frame</frameName>
-          <hackBaseline>0.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 -->
-          <focal_length>320.0</focal_length>
-          <distortion_k1>0</distortion_k1>
-          <distortion_k2>0</distortion_k2>
-          <distortion_k3>0</distortion_k3>
-          <distortion_t1>0</distortion_t1>
-          <distortion_t2>0</distortion_t2>
-          <interface:camera name="wide_stereo_l_stereo_camera_iface"/>
-        </controller:gazebo_ros_camera>
-      </sensor:camera>
-      <turnGravityOff>true</turnGravityOff>
-      <material>PR2/Blue</material>
-    </gazebo><gazebo reference="wide_stereo_l_stereo_camera_optical_frame">
-      <turnGravityOff>true</turnGravityOff>
-    </gazebo><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>
-
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </visual>
-
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </collision>
-    </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">
-
-
-      <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>
-
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </visual>
-
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="wide_stereo_r_stereo_camera_frame">
-      <sensor:camera name="wide_stereo_r_stereo_camera_sensor">
-        <!--imageSize>${image_width} ${image_height}</imageSize-->
-        <imageSize>640 480</imageSize> <!-- FIXME: temporary fix because xacro expands above line to floats, not integers -->
-        <imageFormat>B8G8R8</imageFormat>
-        <hfov>90.0</hfov>
-        <nearClip>0.1</nearClip>
-        <farClip>100</farClip>
-        <updateRate>25.0</updateRate>
-        <controller:gazebo_ros_camera name="wide_stereo_r_stereo_camera_controller" plugin="libgazebo_ros_camera.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>25.0</updateRate>
-          <imageTopicName>wide_stereo/right/image_raw</imageTopicName>
-          <cameraInfoTopicName>wide_stereo/right/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 -->
-          <focal_length>320.0</focal_length>
-          <distortion_k1>0</distortion_k1>
-          <distortion_k2>0</distortion_k2>
-          <distortion_k3>0</distortion_k3>
-          <distortion_t1>0</distortion_t1>
-          <distortion_t2>0</distortion_t2>
-          <interface:camera name="wide_stereo_r_stereo_camera_iface"/>
-        </controller:gazebo_ros_camera>
-      </sensor:camera>
-      <turnGravityOff>true</turnGravityOff>
-      <material>PR2/Blue</material>
-    </gazebo><gazebo reference="wide_stereo_r_stereo_camera_optical_frame">
-      <turnGravityOff>true</turnGravityOff>
-    </gazebo><gazebo reference="wide_stereo_link">
-      <material value="PR2/Blue"/>
-    </gazebo><gazebo reference="wide_stereo_optical_frame">
-      <material value="PR2/Blue"/>
-    </gazebo><gazebo> <!-- this is not attached to any link, there no reference -->
-      <controller:gazebo_ros_stereo_camera name="wide_stereo_controller" plugin="libgazebo_ros_stereo_camera.so">
+    <material value="Gazebo/Black"/>
+  </gazebo>
+  <joint name="double_stereo_frame_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0.0 0.0 0.003"/>
+    <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>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.01 0 0.025"/>
+      <geometry>
+        <box size="0.02 0.12 0.05"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="wide_stereo_frame_joint" type="fixed">
+    <origin rpy="0.0   0.0   0.0" xyz="0.0 0.03 0.0305"/>
+    <parent link="double_stereo_link"/>
+    <child link="wide_stereo_link"/>
+  </joint>
+  <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>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="Blue"/>
+    </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="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>
+  <link name="wide_stereo_optical_frame" type="camera"/>
+  <joint name="wide_stereo_gazebo_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_gazebo_l_stereo_camera_frame"/>
+  </joint>
+  <link name="wide_stereo_gazebo_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>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="wide_stereo_gazebo_l_stereo_camera_optical_frame_joint" type="fixed">
+    <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
+    <parent link="wide_stereo_gazebo_l_stereo_camera_frame"/>
+    <child link="wide_stereo_gazebo_l_stereo_camera_optical_frame"/>
+  </joint>
+  <link name="wide_stereo_gazebo_l_stereo_camera_optical_frame"/>
+  <gazebo reference="wide_stereo_gazebo_l_stereo_camera_frame">
+    <sensor:camera name="wide_stereo_gazebo_l_stereo_camera_sensor">
+      <imageSize>640 480</imageSize>
+      <imageFormat>BAYER_BGGR8</imageFormat>
+      <hfov>90</hfov>
+      <nearClip>0.1</nearClip>
+      <farClip>100</farClip>
+      <updateRate>25.0</updateRate>
+      <controller:gazebo_ros_camera name="wide_stereo_gazebo_l_stereo_camera_controller" plugin="libgazebo_ros_camera.so">
         <alwaysOn>true</alwaysOn>
-        <updateRate>20.0</updateRate>
-        <leftCamera>wide_stereo_l_stereo_camera_sensor</leftCamera>
-        <rightCamera>wide_stereo_r_stereo_camera_sensor</rightCamera>
-        <topicName>wide_stereo/raw_stereo</topicName>
+        <updateRate>25.0</updateRate>
+        <imageTopicName>wide_stereo/left/image_raw</imageTopicName>
+        <cameraInfoTopicName>wide_stereo/left/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 -->
-        <focal_length>320.0</focal_length>
-        <distortion_k1>0</distortion_k1>
-        <distortion_k2>0</distortion_k2>
-        <distortion_k3>0</distortion_k3>
-        <distortion_t1>0</distortion_t1>
-        <distortion_t2>0</distortion_t2>
-        <baseline>-0.09</baseline> <!-- home pos. of robot has +x forward, +y left -->
-        <interface:stereocamera name="wide_stereo_iface"/>
-      </controller:gazebo_ros_stereo_camera>
-    </gazebo><joint name="narrow_stereo_frame_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="0 0.015 0"/>
-      <parent link="double_stereo_link"/>
-      <child link="narrow_stereo_link"/>
-    </joint><link name="narrow_stereo_link">
-      
-      <inertial>
-        <mass value="0.1"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <inertia ixx="0.00482611007" ixy="-0.000144683999" ixz="0.000110076136" iyy="0.005218991412" iyz="-0.000314239509" izz="0.008618784925"/>
-      </inertial>
-
-      <visual>
-        <origin rpy="0 0 0" xyz="0.0 0 0.05"/>
-        <geometry>
-          <box size="0.005 0.01 0.005"/>
-        </geometry>
-        
-        <material name="Blue"/>
-      </visual>
-
-      <collision>
-        <origin rpy="0 0 0" xyz="0.0 0 0.05"/>
-        <geometry>
-          <box size="0.005 0.01 0.005"/>
-        </geometry>
-      </collision>
-      
-    </link><joint name="narrow_stereo_optical_frame_joint" type="fixed">
-      <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0.0            0.045   0.0305"/>
-      <parent link="narrow_stereo_link"/>
-      <child link="narrow_stereo_optical_frame"/>
-    </joint><link name="narrow_stereo_optical_frame" type="camera">
-
-      <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.001 0.001 0.001"/>
-        </geometry>
-        
-        <material name="Blue"/>
-      </visual>
-
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.0 0.0 0.0"/>
-        </geometry>
-      </collision>
-
-    </link><joint name="narrow_stereo_l_stereo_camera_frame_joint" type="fixed">
-      <origin rpy="0 -1.57079632679 1.57079632679" xyz="0 0 0"/>
-      <parent link="narrow_stereo_optical_frame"/>
-      <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>
-
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </visual>
-
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </collision>
-    </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">
-
-
-      <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>
-
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </visual>
-
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="narrow_stereo_l_stereo_camera_frame">
-      <sensor:camera name="narrow_stereo_l_stereo_camera_sensor">
-        <!--imageSize>${image_width} ${image_height}</imageSize-->
-        <imageSize>640 480</imageSize> <!-- FIXME: temporary fix because xacro expands above line to floats, not integers -->
-        <imageFormat>L8</imageFormat>
-        <hfov>45.0</hfov>
-        <nearClip>0.1</nearClip>
-        <farClip>100</farClip>
-        <updateRate>25.0</updateRate>
-        <controller:gazebo_ros_camera name="narrow_stereo_l_stereo_camera_controller" plugin="libgazebo_ros_camera.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>25.0</updateRate>
-          <imageTopicName>narrow_stereo/left/image_raw</imageTopicName>
-          <cameraInfoTopicName>narrow_stereo/left/camera_info</cameraInfoTopicName>
-          <frameName>narrow_stereo_optical_frame</frameName>
-          <hackBaseline>0.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 -->
-          <focal_length>772.55</focal_length>
-          <distortion_k1>0</distortion_k1>
-          <distortion_k2>0</distortion_k2>
-          <distortion_k3>0</distortion_k3>
-          <distortion_t1>0</distortion_t1>
-          <distortion_t2>0</distortion_t2>
-          <interface:camera name="narrow_stereo_l_stereo_camera_iface"/>
-        </controller:gazebo_ros_camera>
-      </sensor:camera>
-      <turnGravityOff>true</turnGravityOff>
-      <material>PR2/Blue</material>
-    </gazebo><gazebo reference="narrow_stereo_l_stereo_camera_optical_frame">
-      <turnGravityOff>true</turnGravityOff>
-    </gazebo><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>
-
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </visual>
-
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </collision>
-    </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">
-
-
-      <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>
-
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </visual>
-
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="narrow_stereo_r_stereo_camera_frame">
-      <sensor:camera name="narrow_stereo_r_stereo_camera_sensor">
-        <!--imageSize>${image_width} ${image_height}</imageSize-->
-        <imageSize>640 480</imageSize> <!-- FIXME: temporary fix because xacro expands above line to floats, not integers -->
-        <imageFormat>L8</imageFormat>
-        <hfov>45.0</hfov>
-        <nearClip>0.1</nearClip>
-        <farClip>100</farClip>
-        <updateRate>25.0</updateRate>
-        <controller:gazebo_ros_camera name="narrow_stereo_r_stereo_camera_controller" plugin="libgazebo_ros_camera.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>25.0</updateRate>
-          <imageTopicName>narrow_stereo/right/image_raw</imageTopicName>
-          <cameraInfoTopicName>narrow_stereo/right/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 -->
-          <focal_length>772.55</focal_length>
-          <distortion_k1>0</distortion_k1>
-          <distortion_k2>0</distortion_k2>
-          <distortion_k3>0</distortion_k3>
-          <distortion_t1>0</distortion_t1>
-          <distortion_t2>0</distortion_t2>
-          <interface:camera name="narrow_stereo_r_stereo_camera_iface"/>
-        </controller:gazebo_ros_camera>
-      </sensor:camera>
-      <turnGravityOff>true</turnGravityOff>
-      <material>PR2/Blue</material>
-    </gazebo><gazebo reference="narrow_stereo_r_stereo_camera_optical_frame">
-      <turnGravityOff>true</turnGravityOff>
-    </gazebo><gazebo reference="narrow_stereo_link">
-      <material value="PR2/Blue"/>
-    </gazebo><gazebo reference="narrow_stereo_optical_frame">
-      <material value="PR2/Blue"/>
-    </gazebo><gazebo> <!-- this is not attached to any link, there no reference -->
-      <controller:gazebo_ros_stereo_camera name="narrow_stereo_controller" plugin="libgazebo_ros_stereo_camera.so">
+        <focal_length>320</focal_length>
+        <distortion_k1>0.00000001</distortion_k1>
+        <distortion_k2>0.00000001</distortion_k2>
+        <distortion_k3>0.00000001</distortion_k3>
+        <distortion_t1>0.00000001</distortion_t1>
+        <distortion_t2>0.00000001</distortion_t2>
+        <interface:camera name="wide_stereo_gazebo_l_stereo_camera_iface"/>
+      </controller:gazebo_ros_camera>
+    </sensor:camera>
+    <turnGravityOff>true</turnGravityOff>
+    <material>PR2/Blue</material>
+  </gazebo>
+  <joint name="wide_stereo_gazebo_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_gazebo_l_stereo_camera_frame"/>
+    <child link="wide_stereo_gazebo_r_stereo_camera_frame"/>
+  </joint>
+  <link name="wide_stereo_gazebo_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>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="wide_stereo_gazebo_r_stereo_camera_optical_frame_joint" type="fixed">
+    <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
+    <parent link="wide_stereo_gazebo_r_stereo_camera_frame"/>
+    <child link="wide_stereo_gazebo_r_stereo_camera_optical_frame"/>
+  </joint>
+  <link name="wide_stereo_gazebo_r_stereo_camera_optical_frame"/>
+  <gazebo reference="wide_stereo_gazebo_r_stereo_camera_frame">
+    <sensor:camera name="wide_stereo_gazebo_r_stereo_camera_sensor">
+      <imageSize>640 480</imageSize>
+      <imageFormat>BAYER_BGGR8</imageFormat>
+      <hfov>90</hfov>
+      <nearClip>0.1</nearClip>
+      <farClip>100</farClip>
+      <updateRate>25.0</updateRate>
+      <controller:gazebo_ros_camera name="wide_stereo_gazebo_r_stereo_camera_controller" plugin="libgazebo_ros_camera.so">
         <alwaysOn>true</alwaysOn>
-        <updateRate>20.0</updateRate>
-        <leftCamera>narrow_stereo_l_stereo_camera_sensor</leftCamera>
-        <rightCamera>narrow_stereo_r_stereo_camera_sensor</rightCamera>
-        <topicName>narrow_stereo/raw_stereo</topicName>
+        <updateRate>25.0</updateRate>
+        <imageTopicName>wide_stereo/right/image_raw</imageTopicName>
+        <cameraInfoTopicName>wide_stereo/right/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 -->
+        <focal_length>320</focal_length>
+        <distortion_k1>0.00000001</distortion_k1>
+        <distortion_k2>0.00000001</distortion_k2>
+        <distortion_k3>0.00000001</distortion_k3>
+        <distortion_t1>0.00000001</distortion_t1>
+        <distortion_t2>0.00000001</distortion_t2>
+        <interface:camera name="wide_stereo_gazebo_r_stereo_camera_iface"/>
+      </controller:gazebo_ros_camera>
+    </sensor:camera>
+    <turnGravityOff>true</turnGravityOff>
+    <material>PR2/Blue</material>
+  </gazebo>
+  <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.0 0.06 0.0305"/>
+    <parent link="double_stereo_link"/>
+    <child link="narrow_stereo_link"/>
+  </joint>
+  <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>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="Blue"/>
+    </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="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>
+  <link name="narrow_stereo_optical_frame" type="camera"/>
+  <joint name="narrow_stereo_gazebo_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_gazebo_l_stereo_camera_frame"/>
+  </joint>
+  <link name="narrow_stereo_gazebo_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>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="narrow_stereo_gazebo_l_stereo_camera_optical_frame_joint" type="fixed">
+    <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
+    <parent link="narrow_stereo_gazebo_l_stereo_camera_frame"/>
+    <child link="narrow_stereo_gazebo_l_stereo_camera_optical_frame"/>
+  </joint>
+  <link name="narrow_stereo_gazebo_l_stereo_camera_optical_frame"/>
+  <gazebo reference="narrow_stereo_gazebo_l_stereo_camera_frame">
+    <sensor:camera name="narrow_stereo_gazebo_l_stereo_camera_sensor">
+      <imageSize>640 480</imageSize>
+      <imageFormat>L8</imageFormat>
+      <hfov>45</hfov>
+      <nearClip>0.1</nearClip>
+      <farClip>100</farClip>
+      <updateRate>25.0</updateRate>
+      <controller:gazebo_ros_camera name="narrow_stereo_gazebo_l_stereo_camera_controller" plugin="libgazebo_ros_camera.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>25.0</updateRate>
+        <imageTopicName>narrow_stereo/left/image_raw</imageTopicName>
+        <cameraInfoTopicName>narrow_stereo/left/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 -->
         <focal_length>772.55</focal_length>
-        <distortion_k1>0</distortion_k1>
-        <distortion_k2>0</distortion_k2>
-        <distortion_k3>0</distortion_k3>
-        <distortion_t1>0</distortion_t1>
-        <distortion_t2>0</distortion_t2>
-        <baseline>-0.09</baseline> <!-- home pos. of robot has +x forward, +y left -->
-        <interface:stereocamera name="narrow_stereo_iface"/>
-      </controller:gazebo_ros_stereo_camera>
-    </gazebo><gazebo reference="double_stereo_double_stereo_link">
-      <material value="PR2/Blue"/>
-    </gazebo><gazebo reference="sensor_mount_sensor_link">
-      <material value="PR2/Blue"/>
-    </gazebo>
-
+        <distortion_k1>0.00000001</distortion_k1>
+        <distortion_k2>0.00000001</distortion_k2>
+        <distortion_k3>0.00000001</distortion_k3>
+        <distortion_t1>0.00000001</distortion_t1>
+        <distortion_t2>0.00000001</distortion_t2>
+        <interface:camera name="narrow_stereo_gazebo_l_stereo_camera_iface"/>
+      </controller:gazebo_ros_camera>
+    </sensor:camera>
+    <turnGravityOff>true</turnGravityOff>
+    <material>PR2/Blue</material>
+  </gazebo>
+  <joint name="narrow_stereo_gazebo_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_gazebo_l_stereo_camera_frame"/>
+    <child link="narrow_stereo_gazebo_r_stereo_camera_frame"/>
+  </joint>
+  <link name="narrow_stereo_gazebo_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>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="narrow_stereo_gazebo_r_stereo_camera_optical_frame_joint" type="fixed">
+    <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
+    <parent link="narrow_stereo_gazebo_r_stereo_camera_frame"/>
+    <child link="narrow_stereo_gazebo_r_stereo_camera_optical_frame"/>
+  </joint>
+  <link name="narrow_stereo_gazebo_r_stereo_camera_optical_frame"/>
+  <gazebo reference="narrow_stereo_gazebo_r_stereo_camera_frame">
+    <sensor:camera name="narrow_stereo_gazebo_r_stereo_camera_sensor">
+      <imageSize>640 480</imageSize>
+      <imageFormat>L8</imageFormat>
+      <hfov>45</hfov>
+      <nearClip>0.1</nearClip>
+      <farClip>100</farClip>
+      <updateRate>25.0</updateRate>
+      <controller:gazebo_ros_camera name="narrow_stereo_gazebo_r_stereo_camera_controller" plugin="libgazebo_ros_camera.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>25.0</updateRate>
+        <imageTopicName>narrow_stereo/right/image_raw</imageTopicName>
+        <cameraInfoTopicName>narrow_stereo/right/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 -->
+        <focal_length>772.55</focal_length>
+        <distortion_k1>0.00000001</distortion_k1>
+        <distortion_k2>0.00000001</distortion_k2>
+        <distortion_k3>0.00000001</distortion_k3>
+        <distortion_t1>0.00000001</distortion_t1>
+        <distortion_t2>0.00000001</distortion_t2>
+        <interface:camera name="narrow_stereo_gazebo_r_stereo_camera_iface"/>
+      </controller:gazebo_ros_camera>
+    </sensor:camera>
+    <turnGravityOff>true</turnGravityOff>
+    <material>PR2/Blue</material>
+  </gazebo>
+  <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>
+  <gazebo reference="double_stereo_double_stereo_link">
+    <material value="PR2/Blue"/>
+  </gazebo>
+  <gazebo reference="sensor_mount_sensor_link">
+    <material value="PR2/Blue"/>
+  </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"/>
-      <safety_controller k_position="100" k_velocity="0.05" soft_lower_limit="-0.7354" soft_upper_limit="1.43353"/>
-      <calibration reference_position="-0.35"/>
-      <dynamics damping="0.008"/>
-      <origin rpy="0 0 0" xyz="0.1 0 0.235"/>
-      <parent link="torso_lift_link"/>
-      <child link="laser_tilt_mount_link"/>
-    </joint><link name="laser_tilt_mount_link">
-
-      <inertial>
-        <mass value="0.05"/>
-        <origin rpy="0 0 0" xyz="-0.03 0 -0.03"/>
-        <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.0001"/>
-      </inertial>
-
-      <visual name="laser_tilt_mount_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="laser_tilt_mount_visual_geom">
-          <mesh filename="package://pr2_description/meshes/tilting_laser_v0/hok_tilt.stl"/>
-        </geometry>
-        
-        <material name="Red"/>
-      </visual>
-
-      <collision name="laser_tilt_mount_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="laser_tilt_mount_collision_geom">
-          <box size=".001 .001 .001"/>
-        </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>
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.001 0.001 0.001"/>
-        </geometry>
-        <material name="Red"/>
-      </visual>
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size=".001 .001 .001"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="laser_tilt_link">
-      <sensor:ray name="laser_tilt">
-        <rayCount>640</rayCount>
-        <rangeCount>640</rangeCount>
-        <laserCount>1</laserCount>
-
-        <origin>0.0 0.0 0.0</origin>
-        <displayRays>false</displayRays>
-
-        <minAngle>-80</minAngle>
-        <maxAngle> 80</maxAngle>
-
-        <minRange>0.05</minRange>
-        <maxRange>10.0</maxRange>
-        <resRange>0.01</resRange>
-        <updateRate>40.0</updateRate>
-        <controller:gazebo_ros_laser name="gazebo_ros_laser_tilt_controller" plugin="libgazebo_ros_laser.so">
-          <gaussianNoise>0.005</gaussianNoise>
-          <alwaysOn>true</alwaysOn>
-          <updateRate>40.0</updateRate>
-          <topicName>tilt_scan</topicName>
-          <frameName>laser_tilt_link</frameName>
-          <interface:laser name="gazebo_ros_laser_tilt_iface"/>
-        </controller:gazebo_ros_laser>
-      </sensor:ray>
-    </gazebo><gazebo reference="laser_tilt_mount_link">
-      <material value="PR2/Red"/>
-    </gazebo><transmission name="laser_tilt_mount_trans" type="SimpleTransmission">
-      <actuator name="laser_tilt_mount_motor"/>
-      <joint name="laser_tilt_mount_joint"/>
-      <mechanicalReduction>-6.05</mechanicalReduction>
-    </transmission>
-
+    <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"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <calibration falling="0.0" reference_position="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.05"/>
+      <origin rpy="0 0 0" xyz="-0.03 0 -0.03"/>
+      <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.0001"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/tilting_laser_v0/tilting_hokuyo.stl"/>
+      </geometry>
+      <material name="Red"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pr2_description/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>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.001 0.001 0.001"/>
+      </geometry>
+      <material name="Red"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size=".001 .001 .001"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="laser_tilt_link">
+    <sensor:ray name="laser_tilt">
+      <rayCount>640</rayCount>
+      <rangeCount>640</rangeCount>
+      <laserCount>1</laserCount>
+      <origin>0.0 0.0 0.0</origin>
+      <displayRays>false</displayRays>
+      <minAngle>-79.9999999086</minAngle>
+      <maxAngle>79.9999999086</maxAngle>
+      <minRange>0.05</minRange>
+      <maxRange>10.0</maxRange>
+      <resRange>0.01</resRange>
+      <updateRate>40</updateRate>
+      <controller:gazebo_ros_laser name="gazebo_ros_laser_tilt_controller" plugin="libgazebo_ros_laser.so">
+        <gaussianNoise>0.005</gaussianNoise>
+        <alwaysOn>true</alwaysOn>
+        <updateRate>40</updateRate>
+        <topicName>tilt_scan</topicName>
+        <frameName>laser_tilt_link</frameName>
+        <interface:laser name="gazebo_ros_laser_tilt_iface"/>
+      </controller:gazebo_ros_laser>
+    </sensor:ray>
+  </gazebo>
+  <gazebo reference="laser_tilt_mount_link">
+    <material value="Gazebo/Grey"/>
+  </gazebo>
+  <transmission name="laser_tilt_mount_trans" type="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 -->
   <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"/>
-
-      <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 reference_position="-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 name="r_shoulder_pan_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_shoulder_pan_visual_geom">
-          <mesh filename="package://pr2_description/meshes/shoulder_v0/shoulder_yaw.stl"/>
-        </geometry>
-        <material name="Blue"/>
-      </visual>
-
-      <collision name="r_shoulder_pan_collision">
-        <origin rpy="0 0 0" xyz="0.0 0 0.0"/>
-        <geometry name="r_shoulder_pan_collision_geom">
-          <mesh filename="package://pr2_description/meshes/shoulder_v0/convex/shoulder_yaw_convex.stlb"/>
-        </geometry>
-      </collision>
-    </link><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"/>
-
-      <safety_controller k_position="100" k_velocity="10" soft_lower_limit="-0.3536" soft_upper_limit="1.3463"/>
-      <calibration reference_position="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 name="r_shoulder_lift_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_shoulder_lift_visual_geom">
-          <mesh filename="package://pr2_description/meshes/shoulder_v0/shoulder_lift.stl"/>
-        </geometry>
-        <material name="Grey"/>
-      </visual>
-
-      <collision name="r_shoulder_lift_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_shoulder_lift_collision_geom">
-          <mesh filename="package://pr2_description/meshes/shoulder_v0/convex/shoulder_lift_convex.stlb"/>
-        </geometry>
-      </collision>
-    </link><joint name="r_upper_arm_roll_joint" type="revolute">
-      <axis xyz="1 0 0"/>
+    <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 -->
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <calibration reference_position="-0.785398163397" 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"/>
-      <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"/>
-      <safety_controller k_position="100" k_velocity="2" soft_lower_limit="-3.75" soft_upper_limit="0.65"/>
-      <calibration reference_position="-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 name="r_upper_arm_roll_visual">
-        <!-- TODO: This component doesn't actually have a mesh -->
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_upper_arm_roll_visual_geom">
-          <mesh filename="package://pr2_description/meshes/shoulder_v0/upper_arm_roll.stl"/>
-        </geometry>
-
-        <material name="RollLinks"/>
-      </visual>
-      <collision name="r_upper_arm_roll_collision">
-        <!-- TODO: collision tag should be optional -->
-        <origin rpy="0 0 0" xyz="0.0 0 0"/>
-        <geometry name="r_upper_arm_roll_collision_geom">
-          <box size="0.1 0.1 0.1"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="r_shoulder_pan_link">
-      <sensor:contact name="r_shoulder_pan_contact_sensor">
-        <geom>r_shoulder_pan_link_geom</geom>
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/shoulder_v0/shoulder_pan.stl"/>
+      </geometry>
+      <material name="Blue"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.0 0 0.0"/>
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/shoulder_v0/shoulder_pan.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <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"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <calibration falling="0.0" reference_position="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="package://pr2_description/meshes/shoulder_v0/shoulder_lift.stl"/>
+      </geometry>
+      <material name="Grey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/shoulder_v0/shoulder_lift.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <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"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <calibration reference_position="-1.57079632679" 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="package://pr2_description/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="package://pr2_description/meshes/shoulder_v0/upper_arm_roll_L.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <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>
+  <transmission name="r_upper_arm_roll_trans" type="SimpleTransmission">
+    <actuator name="r_upper_arm_roll_motor"/>
+    <joint name="r_upper_arm_roll_joint"/>
+    <mechanicalReduction>32.6525111499</mechanicalReduction>
+  </transmission>
+  <gazebo reference="r_shoulder_pan_link">
+    <sensor:contact name="r_shoulder_pan_contact_sensor">
+      <geom>r_shoulder_pan_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="r_shoulder_pan_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
         <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="r_shoulder_pan_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>r_shoulder_pan_bumper</bumperTopicName>
-          <interface:bumper name="r_shoulder_pan_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <material value="PR2/Blue"/>
-      <turnGravityOff>true</turnGravityOff>
-    </gazebo><gazebo reference="r_shoulder_pan_joint">
-      <stopKd value="1.0"/>
-      <stopKp value="1000000.0"/>
-    </gazebo><gazebo reference="r_shoulder_lift_link">
-      <sensor:contact name="r_shoulder_lift_contact_sensor">
-        <geom>r_shoulder_lift_link_geom</geom>
+        <bumperTopicName>r_shoulder_pan_bumper</bumperTopicName>
+        <interface:bumper name="r_shoulder_pan_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <material value="PR2/Grey"/>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="r_shoulder_pan_joint">
+    <stopKd value="1.0"/>
+    <stopKp value="1000000.0"/>
+  </gazebo>
+  <gazebo reference="r_shoulder_lift_link">
+    <sensor:contact name="r_shoulder_lift_contact_sensor">
+      <geom>r_shoulder_lift_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="r_shoulder_lift_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
         <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="r_shoulder_lift_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>r_r_shoulder_lift_bumper</bumperTopicName>
-          <interface:bumper name="r_shoulder_lift_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <turnGravityOff>true</turnGravityOff>
-    </gazebo><gazebo reference="r_shoulder_lift_joint">
-      <stopKd value="1.0"/>
-      <stopKp value="1000000.0"/>
-    </gazebo><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><transmission name="r_shoulder_pan_trans" type="SimpleTransmission">
-      <actuator name="r_shoulder_pan_motor"/>
-      <joint name="r_shoulder_pan_joint"/>
-      <mechanicalReduction>63.16</mechanicalReduction>
-    </transmission><transmission name="r_shoulder_lift_trans" type="SimpleTransmission">
-      <actuator name="r_shoulder_lift_motor"/>
-      <joint name="r_shoulder_lift_joint"/>
-      <mechanicalReduction>61.89</mechanicalReduction>
-    </transmission><transmission name="r_upper_arm_roll_trans" type="SimpleTransmission">
-      <actuator name="r_upper_arm_roll_motor"/>
-      <joint name="r_upper_arm_roll_joint"/>
-      <mechanicalReduction>32.65</mechanicalReduction>
-    </transmission>
+        <bumperTopicName>r_r_shoulder_lift_bumper</bumperTopicName>
+        <interface:bumper name="r_shoulder_lift_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="r_shoulder_lift_joint">
+    <stopKd value="1.0"/>
+    <stopKp value="1000000.0"/>
+  </gazebo>
+  <transmission name="r_shoulder_pan_trans" type="SimpleTransmission">
+    <actuator name="r_shoulder_pan_motor"/>
+    <joint name="r_shoulder_pan_joint"/>
+    <mechanicalReduction>63.1552452977</mechanicalReduction>
+  </transmission>
+  <transmission name="r_shoulder_lift_trans" type="SimpleTransmission">
+    <actuator name="r_shoulder_lift_motor"/>
+    <joint name="r_shoulder_lift_joint"/>
+    <mechanicalReduction>61.8948225713</mechanicalReduction>
+  </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"/>
-      <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 name="{side}_upper_arm_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_upper_arm_visual_geom">
-          <mesh filename="package://pr2_description/meshes/upper_arm_v0/upper_arm.stl"/>
-        </geometry>
-
-        <material name="Green"/>
-      </visual>
-
-      <collision name="r_upper_arm_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_upper_arm_collision_geom">
-          <mesh filename="package://pr2_description/meshes/upper_arm_v0/convex/upper_arm_convex.stlb"/>
-        </geometry>
-      </collision>
-
-    </link><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"/>
-      <safety_controller k_position="100" k_velocity="3" soft_lower_limit="-2.1213" soft_upper_limit="-0.15"/>
-      <calibration reference_position="-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 name="r_elbow_flex_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_elbow_flex_visual_geom">
-          <mesh filename="package://pr2_description/meshes/upper_arm_v0/elbow_flex.stl"/>
-        </geometry>
-
-        <material name="Grey"/>
-      </visual>
-
-      <collision name="r_elbow_flex_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_elbow_flex_collision_geom">
-          <mesh filename="package://pr2_description/meshes/upper_arm_v0/convex/elbow_flex_convex.stlb"/>
-        </geometry>
-      </collision>
-    </link><joint name="r_forearm_roll_joint" type="continuous">
-      <axis xyz="-1 0 0"/>
-      <limit effort="30" velocity="3.6"/>
-      <safety_controller k_velocity="1"/>
-      <calibration reference_position="0.0"/>
-      <dynamics damping="0.1"/>
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/upper_arm_v0/upper_arm.stl"/>
+      </geometry>
+      <material name="Green"/>
+    </visual>
+    <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
-      <parent link="r_elbow_flex_link"/>
-      <child link="r_forearm_roll_link"/>
-    </joint><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 name="r_forearm_roll_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_forearm_roll_visual_geom">
-          <mesh filename="package://pr2_description/meshes/upper_arm_v0/forearm_roll.stl"/>
-        </geometry>
-
-        <material name="RollLinks"/>
-      </visual>
-      <!-- TODO: collision tag should be optional -->
-      <collision name="r_forearm_roll_collision">
-        <!-- dummy geom, to be removed -->
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_forearm_roll_collision_geom">
-          <box size="0.1 0.1 0.1"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="r_upper_arm_link">
-      <sensor:contact name="r_upper_arm_contact_sensor">
-        <geom>r_upper_arm_link_geom</geom><!-- TODO -->
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/upper_arm_v0/upper_arm.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="r_upper_arm_link">
+    <sensor:contact name="r_upper_arm_contact_sensor">
+      <geom>r_upper_arm_link_geom</geom>
+      <!-- TODO -->
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="r_upper_arm_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
         <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="r_upper_arm_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>r_upper_arm_bumper</bumperTopicName>
-          <interface:bumper name="r_upper_arm_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <turnGravityOff>true</turnGravityOff>
-      <material value="PR2/Green"/>
-    </gazebo><gazebo reference="r_elbow_flex_joint">
-      <initial_joint_position>-1.0</initial_joint_position>
-    </gazebo><gazebo reference="r_elbow_flex_link">
-      <sensor:contact name="r_elbow_flex_contact_sensor">
-        <geom>r_elbow_flex_link_geom</geom>
+        <bumperTopicName>r_upper_arm_bumper</bumperTopicName>
+        <interface:bumper name="r_upper_arm_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <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"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <calibration reference_position="0.0" 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>
+  <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="package://pr2_description/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="package://pr2_description/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="SimpleTransmission">
+    <actuator name="r_forearm_roll_motor"/>
+    <joint name="r_forearm_roll_joint"/>
+    <mechanicalReduction>-90.5142857143</mechanicalReduction>
+  </transmission>
+  <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"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <calibration falling="-1.1606" reference_position="-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="package://pr2_description/meshes/upper_arm_v0/elbow_flex.stl"/>
+      </geometry>
+      <material name="Grey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/upper_arm_v0/elbow_flex.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="r_elbow_flex_joint">
+    <initial_joint_position>-1.0</initial_joint_position>
+  </gazebo>
+  <gazebo reference="r_elbow_flex_link">
+    <sensor:contact name="r_elbow_flex_contact_sensor">
+      <geom>r_elbow_flex_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="r_elbow_flex_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
         <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="r_elbow_flex_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>r_elbow_flex_bumper</bumperTopicName>
-          <interface:bumper name="r_elbow_flex_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <material value="PR2/Grey"/>
-      <turnGravityOff>true</turnGravityOff>
-    </gazebo><gazebo reference="r_elbow_flex_joint">
-      <stopKd value="1.0"/>
-      <stopKp value="1000000.0"/>
-    </gazebo><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_elbow_flex_trans" type="SimpleTransmission">
-      <actuator name="r_elbow_flex_motor"/>
-      <joint name="r_elbow_flex_joint"/>
-      <mechanicalReduction>-36.17</mechanicalReduction>
-    </transmission><transmission name="r_forearm_roll_trans" type="SimpleTransmission">
-      <actuator name="r_forearm_roll_motor"/>
-      <joint name="r_forearm_roll_joint"/>
-      <mechanicalReduction>90.5142857143</mechanicalReduction>
-    </transmission>
+        <bumperTopicName>r_elbow_flex_bumper</bumperTopicName>
+        <interface:bumper name="r_elbow_flex_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <material value="PR2/Grey"/>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="r_elbow_flex_joint">
+    <stopKd value="1.0"/>
+    <stopKp value="1000000.0"/>
+  </gazebo>
+  <transmission name="r_elbow_flex_trans" type="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"/>
-      <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 name="r_forearm_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_forearm_visual_geom">
-          <mesh filename="package://pr2_description/meshes/forearm_v0/forearm.stl"/>
-        </geometry>
-
-        <material name="Blue"/>
-      </visual>
-
-      <collision name="r_forearm_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_forearm_collision_geom">
-          <mesh filename="package://pr2_description/meshes/forearm_v0/convex/forearm_convex.stlb"/>
-        </geometry>
-      </collision>
-    </link><joint name="r_wrist_flex_joint" type="revolute">
-      <axis xyz="0 -1 0"/>
-      <limit effort="10" lower="0.0" upper="2.094" velocity="3.078"/>
-      <safety_controller k_position="20" k_velocity="4" soft_lower_limit="0.15" soft_upper_limit="1.944"/>
-      <dynamics damping="0.1"/>
-      <calibration reference_position="0.5410521"/>
-      <origin rpy="0 0 0" xyz="0.32025 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 name="r_wrist_flex_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_wrist_flex_visual_geom">
-          <mesh filename="package://pr2_description/meshes/forearm_v0/wrist_flex.stl"/>
-        </geometry>
-
-        <material name="Grey"/>
-      </visual>
-
-      <collision name="r_wrist_flex_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_wrist_flex_collision_geom">
-          <mesh filename="package://pr2_description/meshes/forearm_v0/convex/wrist_flex_convex.stlb"/>
-        </geometry>
-      </collision>
-    </link><joint name="r_wrist_roll_joint" type="continuous">
-      <axis xyz="1 0 0"/>
-      <limit effort="10" velocity="3.6"/>
-      <safety_controller k_velocity="2"/>
-      <dynamics damping="0.1"/>
-      <calibration reference_position="1.57079632679"/>
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/forearm_v0/forearm.stl"/>
+      </geometry>
+      <material name="Grey"/>
+    </visual>
+    <collision>
       <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 name="r_wrist_roll_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_wrist_roll_visual_geom">
-          <mesh filename="package://pr2_description/meshes/forearm_v0/wrist_roll.stl"/>
-        </geometry>
-
-        <material name="RollLinks"/>
-      </visual>
-      <collision name="r_wrist_roll_collision">
-        <origin rpy="0 0 0" xyz="0.0 0 0"/>
-        <geometry name="r_wrist_roll_collision_geom">
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-        <verbose value="Yes"/>
-      </collision>
-    </link><gazebo reference="r_forearm_link">
-      <turnGravityOff>true</turnGravityOff>
-      <sensor:contact name="r_forearm_contact_sensor">
-        <geom>r_forearm_link_geom</geom>
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/forearm_v0/forearm.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="r_wrist_flex_joint" type="revolute">
+    <axis xyz="0 1 0"/>
+    <limit effort="10" lower="-2.094" upper="0.0" velocity="3.078"/>
+    <!-- alpha tested velocity and effort limits -->
+    <safety_controller k_position="20" k_velocity="4" soft_lower_limit="-1.994" soft_upper_limit="-0.1"/>
+    <dynamics damping="0.1"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <calibration falling="-0.5410521" reference_position="-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="package://pr2_description/meshes/forearm_v0/wrist_flex.stl"/>
+      </geometry>
+      <material name="Grey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/forearm_v0/wrist_flex.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <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"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <calibration reference_position="-1.57079632679" 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="package://pr2_description/meshes/forearm_v0/wrist_roll.stl"/>
+      </geometry>
+      <material name="RollLinks"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/forearm_v0/wrist_roll_L.stl"/>
+      </geometry>
+      <verbose value="Yes"/>
+    </collision>
+  </link>
+  <gazebo reference="r_forearm_link">
+    <turnGravityOff>true</turnGravityOff>
+    <sensor:contact name="r_forearm_contact_sensor">
+      <geom>r_forearm_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="r_forearm_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
         <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="r_forearm_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>r_forearm_bumper</bumperTopicName>
-          <interface:bumper name="r_forearm_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <material value="PR2/Blue"/>
-    </gazebo><gazebo reference="r_wrist_flex_link">
-      <turnGravityOff>true</turnGravityOff>
-      <sensor:contact name="r_wrist_flex_contact_sensor">
-        <geom>r_wrist_flex_link_geom</geom>
+        <bumperTopicName>r_forearm_bumper</bumperTopicName>
+        <interface:bumper name="r_forearm_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+  </gazebo>
+  <gazebo reference="r_wrist_flex_link">
+    <turnGravityOff>true</turnGravityOff>
+    <sensor:contact name="r_wrist_flex_contact_sensor">
+      <geom>r_wrist_flex_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="r_wrist_flex_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
         <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="r_wrist_flex_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>r_wrist_flex_bumper</bumperTopicName>
-          <interface:bumper name="r_wrist_flex_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <material value="PR2/Grey"/>
-    </gazebo><gazebo reference="r_wrist_flex_joint">
-      <stopKd value="1.0"/>
-      <stopKp value="1000000.0"/>
-    </gazebo><gazebo reference="r_wrist_roll_link">
-      <turnGravityOff>true</turnGravityOff>
-      <sensor:contact name="r_wrist_roll_contact_sensor">
-        <geom>r_wrist_roll_link_geom</geom>
+        <bumperTopicName>r_wrist_flex_bumper</bumperTopicName>
+        <interface:bumper name="r_wrist_flex_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <material value="PR2/Grey"/>
+  </gazebo>
+  <gazebo reference="r_wrist_flex_joint">
+    <stopKd value="1.0"/>
+    <stopKp value="1000000.0"/>
+  </gazebo>
+  <gazebo reference="r_wrist_roll_link">
+    <turnGravityOff>true</turnGravityOff>
+    <sensor:contact name="r_wrist_roll_contact_sensor">
+      <geom>r_wrist_roll_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="r_wrist_roll_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
         <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="r_wrist_roll_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>r_wrist_roll_bumper</bumperTopicName>
-          <interface:bumper name="r_wrist_roll_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <material value="PR2/RollLinks"/>
-    </gazebo><gazebo reference="r_wrist_roll_joint">
-      <fudgeFactor value="0.5"/>
-    </gazebo><transmission name="r_wrist_trans" type="WristTransmission">
-      <rightActuator name="r_wrist_r_motor"/>
-      <leftActuator name="r_wrist_l_motor"/>
-      <flexJoint mechanicalReduction="60.1714285714" name="r_wrist_flex_joint"/>
-      <rollJoint mechanicalReduction="60.1714285714" name="r_wrist_roll_joint"/>
-    </transmission>
-
+        <bumperTopicName>r_wrist_roll_bumper</bumperTopicName>
+        <interface:bumper name="r_wrist_roll_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <material value="PR2/RollLinks"/>
+  </gazebo>
+  <gazebo reference="r_wrist_roll_joint">
+    <fudgeFactor value="0.5"/>
+  </gazebo>
+  <transmission name="r_wrist_trans" type="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"/>
-      <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 name="r_gripper_palm_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_gripper_palm_visual_geom">
-          <mesh filename="package://pr2_defs/meshes/gripper_palm.stl"/>
-        </geometry>
-        
-        <material name="Red"/>
-      </visual>
-      <collision name="r_gripper_palm_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_gripper_palm_collision_geom">
-          <mesh filename="package://pr2_defs/meshes/convex/gripper_palm_convex.stlb"/>
-        </geometry>
-        <verbose value="Yes"/>
-      </collision>
-    </link><joint name="r_gripper_motor_accelerometer_joint" type="fixed">
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/gripper_v0/gripper_palm.stl"/>
+      </geometry>
+      <material name="Red"/>
+    </visual>
+    <collision>
       <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>
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/gripper_v0/gripper_palm.stl"/>
+      </geometry>
+      <verbose value="Yes"/>
+    </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"/>
+  <joint name="r_gripper_l_finger_joint" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
+    <dynamics damping="0.2"/>
+    <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="package://pr2_description/meshes/gripper_v0/l_finger.stl"/>
+      </geometry>
+      <material name="Grey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.stl"/>
+      </geometry>
+      <verbose value="Yes"/>
+    </collision>
+  </link>
+  <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"/>
+    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
+    <dynamics damping="0.2"/>
+    <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="package://pr2_description/meshes/gripper_v0/l_finger.stl"/>
+      </geometry>
+      <material name="Grey"/>
+    </visual>
+    <collision>
+      <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.stl"/>
+      </geometry>
+      <verbose value="Yes"/>
+    </collision>
+  </link>
+  <joint name="r_gripper_l_parallel_root_joint" type="revolute">
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="0.05891 0.031 0"/>
+    <dynamics damping="0.2"/>
+    <mimic joint="r_gripper_l_finger_joint" multiplier="1" offset="0"/>
+    <parent link="r_gripper_palm_link"/>
+    <child link="r_gripper_l_parallel_link"/>
+    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
+  </joint>
+  <joint name="r_gripper_r_parallel_root_joint" type="revolute">
+    <axis xyz="0 0 -1"/>
+    <origin rpy="0 0 0" xyz="0.05891 -0.031 0"/>
+    <dynamics damping="0.2"/>
+    <mimic joint="r_gripper_l_finger_joint" multiplier="-1" offset="0"/>
+    <parent link="r_gripper_palm_link"/>
+    <child link="r_gripper_r_parallel_link"/>
+    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
+  </joint>
+  <link name="r_gripper_l_parallel_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"/>
+        <origin xyz="${0.0914942479/2.0} 0 0" rpy="0 0 0.0541224233" />
         <geometry>
-          <box size="0.001 0.001 0.001"/>
+          <box size="${0.0914942479} 0.005 0.005" />
         </geometry>
+        <material name="Green" />
       </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">
-      <!-- TODO: all that follows is made-up and useless, but the old parser crashes without it -->
-      <inertial>
-        <mass value="0.01"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
-      </inertial>
+-->
+    <collision>
+      <origin rpy="0 0 0.0541224233" xyz="0.04574712395 0 0"/>
+      <geometry>
+        <box size="0.0914942479 0.005 0.005"/>
+      </geometry>
+      <verbose value="Yes"/>
+    </collision>
+  </link>
+  <link name="r_gripper_r_parallel_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="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_gripper_tool_visual_geom">
-          <box size="0.001 0.001 0.001"/>
+        <origin xyz="${0.0914942479/2.0} 0 0" rpy="0 0 -0.0541224233" />
+        <geometry>
+          <box size="${0.0914942479} 0.005 0.005" />
         </geometry>
-        <material name="Grey"/>
+        <material name="Green" />
       </visual>
-      <collision>
-        <origin rpy="0 0 0" xyz="-0.1 0 0"/> <!-- move it out of the way -->
-        <geometry name="r_gripper_tool_collision_geom">
-          <box size="0.001 0.001 0.001"/>
-        </geometry>
-      </collision>
-    </link><joint name="r_gripper_l_finger_joint" type="revolute">
-      <axis xyz="0 0 1"/>
-
-      <limit effort="100.0" lower="0.0" upper="0.548" velocity="0.5"/><safety_controller k_position="100.0" k_velocity="100.0" soft_lower_limit="0.0" soft_upper_limit="0.548"/>
-
-      <dynamics damping="0.2"/>
-      <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 name="r_gripper_l_finger_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_gripper_l_finger_visual_geom">
-          <mesh filename="package://pr2_defs/meshes/upper_finger_l.stl"/>
-        </geometry>
-        
-        <material name="Grey"/>
-      </visual>
-      <collision name="r_gripper_l_finger_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_gripper_l_finger_collision_geom">
-          <mesh filename="package://pr2_defs/meshes/convex/upper_finger_l_convex.stlb"/>
-        </geometry>
-        <verbose value="Yes"/>
-      </collision>
-    </link><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"/>
-      <limit effort="100.0" lower="0.0" upper="0.548" velocity="0.5"/><safety_controller k_position="100.0" k_velocity="100.0" soft_lower_limit="0.0" soft_upper_limit="0.548"/>
-      <dynamics damping="0.2"/>
-      <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 name="r_gripper_r_finger_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_gripper_r_finger_visual_geom">
-          <mesh filename="package://pr2_defs/meshes/upper_finger_r.stl"/>
-        </geometry>
-        
-        <material name="Grey"/>
-      </visual>
-      <collision name="r_gripper_r_finger_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_gripper_r_finger_collision_geom">
-          <mesh filename="package://pr2_defs/meshes/convex/upper_finger_r_convex.stlb"/>
-        </geometry>
-        <verbose value="Yes"/>
-      </collision>
-    </link><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"/>
-      <limit effort="100.0" lower="0.0" upper="0.548" velocity="0.5"/><safety_controller k_position="100.0" k_velocity="100.0" soft_lower_limit="0.0" soft_upper_limit="0.548"/>
-      <dynamics damping="0.01"/>
-      <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 name="r_gripper_l_finger_tip_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_gripper_l_finger_tip_visual_geom">
-          <mesh filename="package://pr2_defs/meshes/finger_tip_l.stl"/>
-        </geometry>
-        
-        <material name="Green"/>
-      </visual>
-      <collision name="r_gripper_l_finger_tip_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_gripper_l_finger_tip_collision_geom">
-          <mesh filename="package://pr2_defs/meshes/finger_tip_pad2_l.stl"/>
-        </geometry>
-        <verbose value="Yes"/>
-      </collision>
-    </link><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"/>
-      <limit effort="100.0" lower="0.0" upper="0.548" velocity="0.5"/><safety_controller k_position="100.0" k_velocity="100.0" soft_lower_limit="0.0" soft_upper_limit="0.548"/>
-      <dynamics damping="0.01"/>
-      <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 name="r_gripper_r_finger_tip_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_gripper_r_finger_tip_visual_geom">
-          <mesh filename="package://pr2_defs/meshes/finger_tip_r.stl"/>
-        </geometry>
-        
-        <material name="Green"/>
-      </visual>
-      <collision name="r_gripper_r_finger_tip_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_gripper_r_finger_tip_collision_geom">
-          <mesh filename="package://pr2_defs/meshes/finger_tip_pad2_r.stl"/>
-        </geometry>
-        <verbose value="Yes"/>
-      </collision>
-    </link><gazebo reference="r_gripper_l_finger_link">
-      <turnGravityOff>true</turnGravityOff>
-      <sensor:contact name="r_gripper_l_finger_contact_sensor">
-        <geom>r_gripper_l_finger_link_geom</geom>
-        <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="r_gripper_l_finger_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>r_gripper_l_finger_bumper</bumperTopicName>
-          <interface:bumper name="r_gripper_l_finger_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <mu1 value="500.0"/>
-      <mu2 value="500.0"/>
-      <kp value="1000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/Grey"/>
-      <!-- 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><gazebo reference="r_gripper_r_finger_link">
-      <turnGravityOff>true</turnGravityOff>
-      <sensor:contact name="r_gripper_r_finger_contact_sensor">
-        <geom>r_gripper_r_finger_link_geom</geom>
-        <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="r_gripper_r_finger_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>r_gripper_r_finger_bumper</bumperTopicName>
-          <interface:bumper name="r_gripper_r_finger_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <mu1 value="500.0"/>
-      <mu2 value="500.0"/>
-      <kp value="1000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/Grey"/>
-    </gazebo><gazebo reference="r_gripper_r_finger_joint">
-      <stopKd value="1.0"/>
-      <stopKp value="10000000.0"/>
-      <fudgeFactor value="1.0"/>
-      <provideFeedback value="true"/>
-    </gazebo><gazebo reference="r_gripper_l_finger_tip_link">
-      <turnGravityOff>true</turnGravityOff>
-      <selfCollide>false</selfCollide>
-      <sensor:contact name="r_gripper_l_finger_tip_contact_sensor">
-        <geom>r_gripper_l_finger_tip_link_geom</geom>
-        <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="r_gripper_l_finger_tip_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>r_gripper_l_finger_tip_bumper</bumperTopicName>
-          <interface:bumper name="r_gripper_l_finger_tip_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <mu1 value="500.0"/>
-      <mu2 value="500.0"/>
-      <kp value="10000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/Red"/>
-    </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><gazebo reference="r_gripper_r_finger_tip_link">
-      <turnGravityOff>true</turnGravityOff>
-      <selfCollide>false</selfCollide>
-      <sensor:contact name="r_gripper_r_finger_tip_contact_sensor">
-        <geom>r_gripper_r_finger_tip_link_geom</geom>
-        <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="r_gripper_r_finger_tip_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>r_gripper_r_finger_tip_bumper</bumperTopicName>
-          <interface:bumper name="r_gripper_r_finger_tip_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <mu1 value="500.0"/>
-      <mu2 value="500.0"/>
-      <kp value="10000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/Red"/>
-    </gazebo><gazebo>
-      <controller:gazebo_ros_p3d name="p3d_r_gripper_l_finger_controller" plugin="libgazebo_ros_p3d.so">
+-->
+    <collision>
+      <origin rpy="0 0 -0.0541224233" xyz="0.04574712395 0 0"/>
+      <geometry>
+        <box size="0.0914942479 0.005 0.005"/>
+      </geometry>
+      <verbose value="Yes"/>
+    </collision>
+  </link>
+  <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"/>
+    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
+    <dynamics damping="0.01"/>
+    <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="package://pr2_description/meshes/gripper_v0/l_finger_tip.stl"/>
+      </geometry>
+      <material name="Green"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.stl"/>
+      </geometry>
+      <verbose value="Yes"/>
+    </collision>
+  </link>
+  <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"/>
+    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
+    <dynamics damping="0.01"/>
+    <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="package://pr2_description/meshes/gripper_v0/l_finger_tip.stl"/>
+      </geometry>
+      <material name="Green"/>
+    </visual>
+    <collision>
+      <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.stl"/>
+      </geometry>
+      <verbose value="Yes"/>
+    </collision>
+  </link>
+  <gazebo reference="r_gripper_l_finger_link">
+    <turnGravityOff>true</turnGravityOff>
+    <sensor:contact name="r_gripper_l_finger_contact_sensor">
+      <geom>r_gripper_l_finger_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="r_gripper_l_finger_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
         <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>map</frameName>
-        <interface:position name="p3d_r_gripper_l_finger_position_iface"/>
-      </controller:gazebo_ros_p3d>
-      <controller:gazebo_ros_f3d name="f3d_r_gripper_l_finger_controller" plugin="libgazebo_ros_f3d.so">
+        <bumperTopicName>r_gripper_l_finger_bumper</bumperTopicName>
+        <interface:bumper name="r_gripper_l_finger_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <mu1 value="500.0"/>
+    <mu2 value="500.0"/>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+    <material value="PR2/Grey"/>
+    <!-- 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>
+  <gazebo reference="r_gripper_r_finger_link">
+    <turnGravityOff>true</turnGravityOff>
+    <sensor:contact name="r_gripper_r_finger_contact_sensor">
+      <geom>r_gripper_r_finger_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="r_gripper_r_finger_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
         <alwaysOn>true</alwaysOn>
         <updateRate>100.0</updateRate>
-        <bodyName>r_gripper_l_finger_link</bodyName>
-        <topicName>r_gripper_l_finger_force_ground_truth</topicName>
-        <frameName>r_gripper_l_finger_link</frameName>
-        <interface:position name="f3d_r_gripper_l_finger_force_iface"/>
-      </controller:gazebo_ros_f3d>
-    </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><joint name="r_gripper_joint" type="prismatic">
-      <axis xyz="0 0 1"/>
-      <limit effort="100.0" lower="0.0" upper="0.09" velocity="0.2"/>
-      <safety_controller k_position="20.0" k_velocity="500.0" soft_lower_limit="-0.01" soft_upper_limit="0.088"/>
-    </joint><gazebo reference="r_gripper_palm_link">
-      <turnGravityOff>true</turnGravityOff>
-      <sensor:contact name="r_gripper_palm_contact_sensor">
-        <geom>r_gripper_palm_link_geom</geom>
-        <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="r_gripper_palm_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>r_gripper_palm_bumper</bumperTopicName>
-          <interface:bumper name="r_gripper_palm_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <material value="PR2/Red"/>
-    </gazebo><gazebo reference="r_gripper_float_link">
-      <turnGravityOff>true</turnGravityOff>
-      <material value="PR2/White"/>
-    </gazebo><gazebo>
-      <!-- floating link for enforcing gripper constraints -->
-      <body:box name="r_gripper_float_link">
-          <massMatrix>true</massMatrix>
-          <mass>0.01</mass>
-          <ixx>0.0001</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.0001</iyy>
-          <iyz>0</iyz>
-          <izz>0.0001</izz>
-          <cx>0</cx>
-          <cy>0</cy>
-          <cz>0</cz>
-          <xyz>0.82025 0.188 0.790675</xyz>
-          <rpy>0 -0 0</rpy>
-          <geom:box name="r_gripper_float_link_geom">
-              <xyz>0 0 0</xyz>
-              <rpy>0 -0 0</rpy>
-              <size>0.0 0.0 0.0</size>
-              <visual>
-                  <xyz>0 0 0</xyz>
-                  <rpy>0 -0 0</rpy>
-                  <scale>0.0 0.0 0.0</scale>
-                  <mesh>unit_box</mesh>
-                  <material>PR2/White</material>
-              </visual>
-          </geom:box>
-          <turnGravityOff>true</turnGravityOff>
-          <selfCollide>false</selfCollide>
-      </body:box>
-      <joint:slider name="r_gripper_float_joint">
-          <body1>r_gripper_float_link</body1>
-          <body2>r_gripper_palm_link</body2>
-          <anchor>r_gripper_float_link</anchor>
-          <axis>1 0 0</axis>
-          <lowStop>-0.05</lowStop>
-          <highStop>0.001</highStop>
-      </joint:slider>
-      <joint:slider name="r_gripper_l_finger_tip_slider_joint">
-          <body1>r_gripper_l_finger_tip_link</body1>
-          <body2>r_gripper_float_link</body2>
-          <anchor>r_gripper_l_finger_tip_link</anchor>
-          <axis>0 1 0</axis>
-          <anchorOffset>0 0 0</anchorOffset>
-      </joint:slider>
-      <joint:slider name="r_gripper_r_finger_tip_slider_joint">
-          <body1>r_gripper_r_finger_tip_link</body1>
-          <body2>r_gripper_float_link</body2>
-          <anchor>r_gripper_r_finger_tip_link</anchor>
-          <axis>0 1 0</axis>
-          <anchorOffset>0 0 0</anchorOffset>
-      </joint:slider>
-      <!--
-      <joint:slider name="${side}_gripper_finger_tip_slider_joint">
-          <body1>${side}_gripper_l_finger_tip_link</body1>
-          <body2>${side}_gripper_r_finger_tip_link</body2>
-          <anchor>${side}_gripper_l_finger_tip_link</anchor>
-          <axis>0 1 0</axis>
-          <anchorOffset>0 0 0</anchorOffset>
-          <lowStop>0.0</lowStop>
-          <highStop>0.08</highStop>
-      </joint:slider>
-      -->
-    </gazebo><gazebo>
-      <controller:gazebo_ros_p3d name="p3d_r_gripper_palm_controller" plugin="libgazebo_ros_p3d.so">
+        <bumperTopicName>r_gripper_r_finger_bumper</bumperTopicName>
+        <interface:bumper name="r_gripper_r_finger_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <mu1 value="500.0"/>
+    <mu2 value="500.0"/>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+    <material value="PR2/Grey"/>
+  </gazebo>
+  <gazebo reference="r_gripper_r_finger_joint">
+    <stopKd value="1.0"/>
+    <stopKp value="10000000.0"/>
+    <fudgeFactor value="1.0"/>
+    <provideFeedback value="true"/>
+  </gazebo>
+  <gazebo reference="r_gripper_l_finger_tip_link">
+    <turnGravityOff>true</turnGravityOff>
+    <selfCollide>false</selfCollide>
+    <sensor:contact name="r_gripper_l_finger_tip_contact_sensor">
+      <geom>r_gripper_l_finger_tip_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="r_gripper_l_finger_tip_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
         <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>
-        <interface:position name="p3d_r_gripper_palm_position_iface"/>
-      </controller:gazebo_ros_p3d>
-      <controller:gazebo_ros_p3d name="p3d_r_gripper_tool_frame" plugin="libgazebo_ros_p3d.so">
+        <bumperTopicName>r_gripper_l_finger_tip_bumper</bumperTopicName>
+        <interface:bumper name="r_gripper_l_finger_tip_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <mu1 value="500.0"/>
+    <mu2 value="500.0"/>
+    <kp value="10000000.0"/>
+    <kd value="1.0"/>
+    <material value="PR2/Grey"/>
+  </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>
+  <gazebo reference="r_gripper_r_finger_tip_link">
+    <turnGravityOff>true</turnGravityOff>
+    <selfCollide>false</selfCollide>
+    <sensor:contact name="r_gripper_r_finger_tip_contact_sensor">
+      <geom>r_gripper_r_finger_tip_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="r_gripper_r_finger_tip_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
         <alwaysOn>true</alwaysOn>
         <updateRate>100.0</updateRate>
-        <bodyName>r_gripper_tool_frame</bodyName>
-        <topicName>r_gripper_tool_frame_pose_ground_truth</topicName>
-        <xyzOffsets>0 0 0</xyzOffsets>
-        <rpyOffsets>0 0 0</rpyOffsets>
-        <gaussianNoise>0.0</gaussianNoise>
-        <frameName>/map</frameName>
-        <interface:position name="p3d_r_gripper_tool_frame_position_iface"/>
-      </controller:gazebo_ros_p3d>
-    </gazebo><gazebo reference="r_gripper_tool_frame">
-      <turnGravityOff>true</turnGravityOff>
-      <material value="PR2/Grey"/>
-    </gazebo><transmission name="r_gripper_trans" type="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"/>
-      <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"/>
-    </transmission>
-
+        <bumperTopicName>r_gripper_r_finger_tip_bumper</bumperTopicName>
+        <interface:bumper name="r_gripper_r_finger_tip_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <mu1 value="500.0"/>
+    <mu2 value="500.0"/>
+    <kp value="10000000.0"/>
+    <kd value="1.0"/>
+    <material value="PR2/Grey"/>
+  </gazebo>
+  <gazebo>
+    <controller:gazebo_ros_p3d name="p3d_r_gripper_l_finger_controller" plugin="libgazebo_ros_p3d.so">
+      <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>
+      <interface:position name="p3d_r_gripper_l_finger_position_iface"/>
+    </controller:gazebo_ros_p3d>
+    <controller:gazebo_ros_f3d name="f3d_r_gripper_l_finger_controller" plugin="libgazebo_ros_f3d.so">
+      <alwaysOn>true</alwaysOn>
+      <updateRate>100.0</updateRate>
+      <bodyName>r_gripper_l_finger_link</bodyName>
+      <topicName>r_gripper_l_finger_force_ground_truth</topicName>
+      <frameName>r_gripper_l_finger_link</frameName>
+      <interface:position name="f3d_r_gripper_l_finger_force_iface"/>
+    </controller:gazebo_ros_f3d>
+  </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>
+  <gazebo>
+    <joint:hinge name="r_gripper_r_parallel_tip_joint">
+      <body1>r_gripper_r_parallel_link</body1>
+      <body2>r_gripper_r_finger_tip_link</body2>
+      <anchor>r_gripper_r_finger_tip_link</anchor>
+      <axis>0 0 1</axis>
+      <anchorOffset>-0.018 -0.021 0</anchorOffset>
+    </joint:hinge>
+    <joint:hinge name="r_gripper_l_parallel_tip_joint">
+      <body1>r_gripper_l_parallel_link</body1>
+      <body2>r_gripper_l_finger_tip_link</body2>
+      <anchor>r_gripper_l_finger_tip_link</anchor>
+      <axis>0 0 1</axis>
+      <anchorOffset>-0.018 0.021 0</anchorOffset>
+    </joint:hinge>
+    <joint:slider name="r_gripper_joint">
+      <body1>r_gripper_l_finger_tip_link</body1>
+      <body2>r_gripper_r_finger_tip_link</body2>
+      <anchor>r_gripper_r_finger_tip_link</anchor>
+      <axis>0 1 0</axis>
+    </joint:slider>
+  </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>
+  <joint name="r_gripper_joint" type="prismatic">
+    <axis xyz="0 1 0"/>
+    <dynamics damping="100.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>
+  <gazebo reference="r_gripper_palm_link">
+    <turnGravityOff>true</turnGravityOff>
+    <sensor:contact name="r_gripper_palm_contact_sensor">
+      <geom>r_gripper_palm_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="r_gripper_palm_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>100.0</updateRate>
+        <bumperTopicName>r_gripper_palm_bumper</bumperTopicName>
+        <interface:bumper name="r_gripper_palm_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <material value="PR2/Grey"/>
+  </gazebo>
+  <gazebo>
+    <controller:gazebo_ros_p3d name="p3d_r_gripper_palm_controller" plugin="libgazebo_ros_p3d.so">
+      <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>
+      <interface:position name="p3d_r_gripper_palm_position_iface"/>
+    </controller:gazebo_ros_p3d>
+  </gazebo>
+  <transmission name="r_gripper_trans" type="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 -->
+    <use_simulated_gripper_joint/>
+    <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"/>
+    <passive_joint name="r_gripper_r_parallel_root_joint"/>
+    <passive_joint name="r_gripper_l_parallel_root_joint"/>
+  </transmission>
   <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"/>
-
-      <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 reference_position="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 name="l_shoulder_pan_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_shoulder_pan_visual_geom">
-          <mesh filename="package://pr2_description/meshes/shoulder_v0/shoulder_yaw.stl"/>
-        </geometry>
-        <material name="Blue"/>
-      </visual>
-
-      <collision name="l_shoulder_pan_collision">
-        <origin rpy="0 0 0" xyz="0.0 0 0.0"/>
-        <geometry name="l_shoulder_pan_collision_geom">
-          <mesh filename="package://pr2_description/meshes/shoulder_v0/convex/shoulder_yaw_convex.stlb"/>
-        </geometry>
-      </collision>
-    </link><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"/>
-
-      <safety_controller k_position="100" k_velocity="10" soft_lower_limit="-0.3536" soft_upper_limit="1.3463"/>
-      <calibration reference_position="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 name="l_shoulder_lift_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_shoulder_lift_visual_geom">
-          <mesh filename="package://pr2_description/meshes/shoulder_v0/shoulder_lift.stl"/>
-        </geometry>
-        <material name="Grey"/>
-      </visual>
-
-      <collision name="l_shoulder_lift_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_shoulder_lift_collision_geom">
-          <mesh filename="package://pr2_description/meshes/shoulder_v0/convex/shoulder_lift_convex.stlb"/>
-        </geometry>
-      </collision>
-    </link><joint name="l_upper_arm_roll_joint" type="revolute">
-      <axis xyz="1 0 0"/>
+    <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 -->
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <calibration reference_position="0.785398163397" 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"/>
-      <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"/>
-      <safety_controller k_position="100" k_velocity="2" soft_lower_limit="-0.65" soft_upper_limit="3.75"/>
-      <calibration reference_position="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 name="l_upper_arm_roll_visual">
-        <!-- TODO: This component doesn't actually have a mesh -->
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_upper_arm_roll_visual_geom">
-          <mesh filename="package://pr2_description/meshes/shoulder_v0/upper_arm_roll.stl"/>
-        </geometry>
-
-        <material name="RollLinks"/>
-      </visual>
-      <collision name="l_upper_arm_roll_collision">
-        <!-- TODO: collision tag should be optional -->
-        <origin rpy="0 0 0" xyz="0.0 0 0"/>
-        <geometry name="l_upper_arm_roll_collision_geom">
-          <box size="0.1 0.1 0.1"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="l_shoulder_pan_link">
-      <sensor:contact name="l_shoulder_pan_contact_sensor">
-        <geom>l_shoulder_pan_link_geom</geom>
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/shoulder_v0/shoulder_pan.stl"/>
+      </geometry>
+      <material name="Blue"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.0 0 0.0"/>
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/shoulder_v0/shoulder_pan.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <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"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <calibration falling="0.0" reference_position="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="package://pr2_description/meshes/shoulder_v0/shoulder_lift.stl"/>
+      </geometry>
+      <material name="Grey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/shoulder_v0/shoulder_lift.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <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"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <calibration reference_position="1.57079632679" 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="package://pr2_description/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="package://pr2_description/meshes/shoulder_v0/upper_arm_roll_L.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <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>
+  <transmission name="l_upper_arm_roll_trans" type="SimpleTransmission">
+    <actuator name="l_upper_arm_roll_motor"/>
+    <joint name="l_upper_arm_roll_joint"/>
+    <mechanicalReduction>32.6525111499</mechanicalReduction>
+  </transmission>
+  <gazebo reference="l_shoulder_pan_link">
+    <sensor:contact name="l_shoulder_pan_contact_sensor">
+      <geom>l_shoulder_pan_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="l_shoulder_pan_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
         <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="l_shoulder_pan_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>l_shoulder_pan_bumper</bumperTopicName>
-          <interface:bumper name="l_shoulder_pan_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <material value="PR2/Blue"/>
-      <turnGravityOff>true</turnGravityOff>
-    </gazebo><gazebo reference="l_shoulder_pan_joint">
-      <stopKd value="1.0"/>
-      <stopKp value="1000000.0"/>
-    </gazebo><gazebo reference="l_shoulder_lift_link">
-      <sensor:contact name="l_shoulder_lift_contact_sensor">
-        <geom>l_shoulder_lift_link_geom</geom>
+        <bumperTopicName>l_shoulder_pan_bumper</bumperTopicName>
+        <interface:bumper name="l_shoulder_pan_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <material value="PR2/Grey"/>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="l_shoulder_pan_joint">
+    <stopKd value="1.0"/>
+    <stopKp value="1000000.0"/>
+  </gazebo>
+  <gazebo reference="l_shoulder_lift_link">
+    <sensor:contact name="l_shoulder_lift_contact_sensor">
+      <geom>l_shoulder_lift_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="l_shoulder_lift_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
         <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="l_shoulder_lift_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>l_r_shoulder_lift_bumper</bumperTopicName>
-          <interface:bumper name="l_shoulder_lift_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <turnGravityOff>true</turnGravityOff>
-    </gazebo><gazebo reference="l_shoulder_lift_joint">
-      <stopKd value="1.0"/>
-      <stopKp value="1000000.0"/>
-    </gazebo><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><transmission name="l_shoulder_pan_trans" type="SimpleTransmission">
-      <actuator name="l_shoulder_pan_motor"/>
-      <joint name="l_shoulder_pan_joint"/>
-      <mechanicalReduction>63.16</mechanicalReduction>
-    </transmission><transmission name="l_shoulder_lift_trans" type="SimpleTransmission">
-      <actuator name="l_shoulder_lift_motor"/>
-      <joint name="l_shoulder_lift_joint"/>
-      <mechanicalReduction>61.89</mechanicalReduction>
-    </transmission><transmission name="l_upper_arm_roll_trans" type="SimpleTransmission">
-      <actuator name="l_upper_arm_roll_motor"/>
-      <joint name="l_upper_arm_roll_joint"/>
-      <mechanicalReduction>32.65</mechanicalReduction>
-    </transmission>
+        <bumperTopicName>l_r_shoulder_lift_bumper</bumperTopicName>
+        <interface:bumper name="l_shoulder_lift_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="l_shoulder_lift_joint">
+    <stopKd value="1.0"/>
+    <stopKp value="1000000.0"/>
+  </gazebo>
+  <transmission name="l_shoulder_pan_trans" type="SimpleTransmission">
+    <actuator name="l_shoulder_pan_motor"/>
+    <joint name="l_shoulder_pan_joint"/>
+    <mechanicalReduction>63.1552452977</mechanicalReduction>
+  </transmission>
+  <transmission name="l_shoulder_lift_trans" type="SimpleTransmission">
+    <actuator name="l_shoulder_lift_motor"/>
+    <joint name="l_shoulder_lift_joint"/>
+    <mechanicalReduction>61.8948225713</mechanicalReduction>
+  </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"/>
-      <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 name="{side}_upper_arm_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_upper_arm_visual_geom">
-          <mesh filename="package://pr2_description/meshes/upper_arm_v0/upper_arm.stl"/>
-        </geometry>
-
-        <material name="Green"/>
-      </visual>
-
-      <collision name="l_upper_arm_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_upper_arm_collision_geom">
-          <mesh filename="package://pr2_description/meshes/upper_arm_v0/convex/upper_arm_convex.stlb"/>
-        </geometry>
-      </collision>
-
-    </link><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"/>
-      <safety_controller k_position="100" k_velocity="3" soft_lower_limit="-2.1213" soft_upper_limit="-0.15"/>
-      <calibration reference_position="-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 name="l_elbow_flex_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_elbow_flex_visual_geom">
-          <mesh filename="package://pr2_description/meshes/upper_arm_v0/elbow_flex.stl"/>
-        </geometry>
-
-        <material name="Grey"/>
-      </visual>
-
-      <collision name="l_elbow_flex_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_elbow_flex_collision_geom">
-          <mesh filename="package://pr2_description/meshes/upper_arm_v0/convex/elbow_flex_convex.stlb"/>
-        </geometry>
-      </collision>
-    </link><joint name="l_forearm_roll_joint" type="continuous">
-      <axis xyz="-1 0 0"/>
-      <limit effort="30" velocity="3.6"/>
-      <safety_controller k_velocity="1"/>
-      <calibration reference_position="0.0"/>
-      <dynamics damping="0.1"/>
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/upper_arm_v0/upper_arm.stl"/>
+      </geometry>
+      <material name="Green"/>
+    </visual>
+    <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
-      <parent link="l_elbow_flex_link"/>
-      <child link="l_forearm_roll_link"/>
-    </joint><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 name="l_forearm_roll_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_forearm_roll_visual_geom">
-          <mesh filename="package://pr2_description/meshes/upper_arm_v0/forearm_roll.stl"/>
-        </geometry>
-
-        <material name="RollLinks"/>
-      </visual>
-      <!-- TODO: collision tag should be optional -->
-      <collision name="l_forearm_roll_collision">
-        <!-- dummy geom, to be removed -->
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_forearm_roll_collision_geom">
-          <box size="0.1 0.1 0.1"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="l_upper_arm_link">
-      <sensor:contact name="l_upper_arm_contact_sensor">
-        <geom>l_upper_arm_link_geom</geom><!-- TODO -->
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/upper_arm_v0/upper_arm.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="l_upper_arm_link">
+    <sensor:contact name="l_upper_arm_contact_sensor">
+      <geom>l_upper_arm_link_geom</geom>
+      <!-- TODO -->
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="l_upper_arm_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
         <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="l_upper_arm_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>l_upper_arm_bumper</bumperTopicName>
-          <interface:bumper name="l_upper_arm_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <turnGravityOff>true</turnGravityOff>
-      <material value="PR2/Green"/>
-    </gazebo><gazebo reference="l_elbow_flex_joint">
-      <initial_joint_position>-1.0</initial_joint_position>
-    </gazebo><gazebo reference="l_elbow_flex_link">
-      <sensor:contact name="l_elbow_flex_contact_sensor">
-        <geom>l_elbow_flex_link_geom</geom>
+        <bumperTopicName>l_upper_arm_bumper</bumperTopicName>
+        <interface:bumper name="l_upper_arm_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <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"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <calibration reference_position="0.0" 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>
+  <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="package://pr2_description/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="package://pr2_description/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="SimpleTransmission">
+    <actuator name="l_forearm_roll_motor"/>
+    <joint name="l_forearm_roll_joint"/>
+    <mechanicalReduction>-90.5142857143</mechanicalReduction>
+  </transmission>
+  <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"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <calibration falling="-1.1606" reference_position="-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="package://pr2_description/meshes/upper_arm_v0/elbow_flex.stl"/>
+      </geometry>
+      <material name="Grey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/upper_arm_v0/elbow_flex.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <gazebo reference="l_elbow_flex_joint">
+    <initial_joint_position>-1.0</initial_joint_position>
+  </gazebo>
+  <gazebo reference="l_elbow_flex_link">
+    <sensor:contact name="l_elbow_flex_contact_sensor">
+      <geom>l_elbow_flex_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="l_elbow_flex_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
         <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="l_elbow_flex_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>l_elbow_flex_bumper</bumperTopicName>
-          <interface:bumper name="l_elbow_flex_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <material value="PR2/Grey"/>
-      <turnGravityOff>true</turnGravityOff>
-    </gazebo><gazebo reference="l_elbow_flex_joint">
-      <stopKd value="1.0"/>
-      <stopKp value="1000000.0"/>
-    </gazebo><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_elbow_flex_trans" type="SimpleTransmission">
-      <actuator name="l_elbow_flex_motor"/>
-      <joint name="l_elbow_flex_joint"/>
-      <mechanicalReduction>-36.17</mechanicalReduction>
-    </transmission><transmission name="l_forearm_roll_trans" type="SimpleTransmission">
-      <actuator name="l_forearm_roll_motor"/>
-      <joint name="l_forearm_roll_joint"/>
-      <mechanicalReduction>90.5142857143</mechanicalReduction>
-    </transmission>
+        <bumperTopicName>l_elbow_flex_bumper</bumperTopicName>
+        <interface:bumper name="l_elbow_flex_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <material value="PR2/Grey"/>
+    <turnGravityOff>true</turnGravityOff>
+  </gazebo>
+  <gazebo reference="l_elbow_flex_joint">
+    <stopKd value="1.0"/>
+    <stopKp value="1000000.0"/>
+  </gazebo>
+  <transmission name="l_elbow_flex_trans" type="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"/>
-      <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 name="l_forearm_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_forearm_visual_geom">
-          <mesh filename="package://pr2_description/meshes/forearm_v0/forearm.stl"/>
-        </geometry>
-
-        <material name="Blue"/>
-      </visual>
-
-      <collision name="l_forearm_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_forearm_collision_geom">
-          <mesh filename="package://pr2_description/meshes/forearm_v0/convex/forearm_convex.stlb"/>
-        </geometry>
-      </collision>
-    </link><joint name="l_wrist_flex_joint" type="revolute">
-      <axis xyz="0 -1 0"/>
-      <limit effort="10" lower="0.0" upper="2.094" velocity="3.078"/>
-      <safety_controller k_position="20" k_velocity="4" soft_lower_limit="0.15" soft_upper_limit="1.944"/>
-      <dynamics damping="0.1"/>
-      <calibration reference_position="0.5410521"/>
-      <origin rpy="0 0 0" xyz="0.32025 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 name="l_wrist_flex_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_wrist_flex_visual_geom">
-          <mesh filename="package://pr2_description/meshes/forearm_v0/wrist_flex.stl"/>
-        </geometry>
-
-        <material name="Grey"/>
-      </visual>
-
-      <collision name="l_wrist_flex_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_wrist_flex_collision_geom">
-          <mesh filename="package://pr2_description/meshes/forearm_v0/convex/wrist_flex_convex.stlb"/>
-        </geometry>
-      </collision>
-    </link><joint name="l_wrist_roll_joint" type="continuous">
-      <axis xyz="1 0 0"/>
-      <limit effort="10" velocity="3.6"/>
-      <safety_controller k_velocity="2"/>
-      <dynamics damping="0.1"/>
-      <calibration reference_position="1.57079632679"/>
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/forearm_v0/forearm.stl"/>
+      </geometry>
+      <material name="Grey"/>
+    </visual>
+    <collision>
       <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 name="l_wrist_roll_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_wrist_roll_visual_geom">
-          <mesh filename="package://pr2_description/meshes/forearm_v0/wrist_roll.stl"/>
-        </geometry>
-
-        <material name="RollLinks"/>
-      </visual>
-      <collision name="l_wrist_roll_collision">
-        <origin rpy="0 0 0" xyz="0.0 0 0"/>
-        <geometry name="l_wrist_roll_collision_geom">
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-        <verbose value="Yes"/>
-      </collision>
-    </link><gazebo reference="l_forearm_link">
-      <turnGravityOff>true</turnGravityOff>
-      <sensor:contact name="l_forearm_contact_sensor">
-        <geom>l_forearm_link_geom</geom>
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/forearm_v0/forearm.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="l_wrist_flex_joint" type="revolute">
+    <axis xyz="0 1 0"/>
+    <limit effort="10" lower="-2.094" upper="0.0" velocity="3.078"/>
+    <!-- alpha tested velocity and effort limits -->
+    <safety_controller k_position="20" k_velocity="4" soft_lower_limit="-1.994" soft_upper_limit="-0.1"/>
+    <dynamics damping="0.1"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <calibration falling="-0.5410521" reference_position="-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="package://pr2_description/meshes/forearm_v0/wrist_flex.stl"/>
+      </geometry>
+      <material name="Grey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/forearm_v0/wrist_flex.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <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"/>
+    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
+    <calibration reference_position="-1.57079632679" 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="package://pr2_description/meshes/forearm_v0/wrist_roll.stl"/>
+      </geometry>
+      <material name="RollLinks"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/forearm_v0/wrist_roll_L.stl"/>
+      </geometry>
+      <verbose value="Yes"/>
+    </collision>
+  </link>
+  <gazebo reference="l_forearm_link">
+    <turnGravityOff>true</turnGravityOff>
+    <sensor:contact name="l_forearm_contact_sensor">
+      <geom>l_forearm_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="l_forearm_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
         <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="l_forearm_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>l_forearm_bumper</bumperTopicName>
-          <interface:bumper name="l_forearm_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <material value="PR2/Blue"/>
-    </gazebo><gazebo reference="l_wrist_flex_link">
-      <turnGravityOff>true</turnGravityOff>
-      <sensor:contact name="l_wrist_flex_contact_sensor">
-        <geom>l_wrist_flex_link_geom</geom>
+        <bumperTopicName>l_forearm_bumper</bumperTopicName>
+        <interface:bumper name="l_forearm_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+  </gazebo>
+  <gazebo reference="l_wrist_flex_link">
+    <turnGravityOff>true</turnGravityOff>
+    <sensor:contact name="l_wrist_flex_contact_sensor">
+      <geom>l_wrist_flex_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="l_wrist_flex_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
         <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="l_wrist_flex_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>l_wrist_flex_bumper</bumperTopicName>
-          <interface:bumper name="l_wrist_flex_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <material value="PR2/Grey"/>
-    </gazebo><gazebo reference="l_wrist_flex_joint">
-      <stopKd value="1.0"/>
-      <stopKp value="1000000.0"/>
-    </gazebo><gazebo reference="l_wrist_roll_link">
-      <turnGravityOff>true</turnGravityOff>
-      <sensor:contact name="l_wrist_roll_contact_sensor">
-        <geom>l_wrist_roll_link_geom</geom>
+        <bumperTopicName>l_wrist_flex_bumper</bumperTopicName>
+        <interface:bumper name="l_wrist_flex_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <material value="PR2/Grey"/>
+  </gazebo>
+  <gazebo reference="l_wrist_flex_joint">
+    <stopKd value="1.0"/>
+    <stopKp value="1000000.0"/>
+  </gazebo>
+  <gazebo reference="l_wrist_roll_link">
+    <turnGravityOff>true</turnGravityOff>
+    <sensor:contact name="l_wrist_roll_contact_sensor">
+      <geom>l_wrist_roll_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="l_wrist_roll_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
         <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="l_wrist_roll_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>l_wrist_roll_bumper</bumperTopicName>
-          <interface:bumper name="l_wrist_roll_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <material value="PR2/RollLinks"/>
-    </gazebo><gazebo reference="l_wrist_roll_joint">
-      <fudgeFactor value="0.5"/>
-    </gazebo><transmission name="l_wrist_trans" type="WristTransmission">
-      <rightActuator name="l_wrist_r_motor"/>
-      <leftActuator name="l_wrist_l_motor"/>
-      <flexJoint mechanicalReduction="60.1714285714" name="l_wrist_flex_joint"/>
-      <rollJoint mechanicalReduction="60.1714285714" name="l_wrist_roll_joint"/>
-    </transmission>
-
+        <bumperTopicName>l_wrist_roll_bumper</bumperTopicName>
+        <interface:bumper name="l_wrist_roll_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <material value="PR2/RollLinks"/>
+  </gazebo>
+  <gazebo reference="l_wrist_roll_joint">
+    <fudgeFactor value="0.5"/>
+  </gazebo>
+  <transmission name="l_wrist_trans" type="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"/>
-      <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 name="l_gripper_palm_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_gripper_palm_visual_geom">
-          <mesh filename="package://pr2_defs/meshes/gripper_palm.stl"/>
-        </geometry>
-        
-        <material name="Red"/>
-      </visual>
-      <collision name="l_gripper_palm_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_gripper_palm_collision_geom">
-          <mesh filename="package://pr2_defs/meshes/convex/gripper_palm_convex.stlb"/>
-        </geometry>
-        <verbose value="Yes"/>
-      </collision>
-    </link><joint name="l_gripper_motor_accelerometer_joint" type="fixed">
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/gripper_v0/gripper_palm.stl"/>
+      </geometry>
+      <material name="Red"/>
+    </visual>
+    <collision>
       <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>
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/gripper_v0/gripper_palm.stl"/>
+      </geometry>
+      <verbose value="Yes"/>
+    </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"/>
+  <joint name="l_gripper_l_finger_joint" type="revolute">
+    <axis xyz="0 0 1"/>
+    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
+    <dynamics damping="0.2"/>
+    <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="package://pr2_description/meshes/gripper_v0/l_finger.stl"/>
+      </geometry>
+      <material name="Grey"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.stl"/>
+      </geometry>
+      <verbose value="Yes"/>
+    </collision>
+  </link>
+  <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"/>
+    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
+    <dynamics damping="0.2"/>
+    <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="package://pr2_description/meshes/gripper_v0/l_finger.stl"/>
+      </geometry>
+      <material name="Grey"/>
+    </visual>
+    <collision>
+      <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.stl"/>
+      </geometry>
+      <verbose value="Yes"/>
+    </collision>
+  </link>
+  <joint name="l_gripper_l_parallel_root_joint" type="revolute">
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="0.05891 0.031 0"/>
+    <dynamics damping="0.2"/>
+    <mimic joint="l_gripper_l_finger_joint" multiplier="1" offset="0"/>
+    <parent link="l_gripper_palm_link"/>
+    <child link="l_gripper_l_parallel_link"/>
+    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
+  </joint>
+  <joint name="l_gripper_r_parallel_root_joint" type="revolute">
+    <axis xyz="0 0 -1"/>
+    <origin rpy="0 0 0" xyz="0.05891 -0.031 0"/>
+    <dynamics damping="0.2"/>
+    <mimic joint="l_gripper_l_finger_joint" multiplier="-1" offset="0"/>
+    <parent link="l_gripper_palm_link"/>
+    <child link="l_gripper_r_parallel_link"/>
+    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
+  </joint>
+  <link name="l_gripper_l_parallel_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"/>
+        <origin xyz="${0.0914942479/2.0} 0 0" rpy="0 0 0.0541224233" />
         <geometry>
-          <box size="0.001 0.001 0.001"/>
+          <box size="${0.0914942479} 0.005 0.005" />
         </geometry>
+        <material name="Green" />
       </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">
-      <!-- TODO: all that follows is made-up and useless, but the old parser crashes without it -->
-      <inertial>
-        <mass value="0.01"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
-      </inertial>
+-->
+    <collision>
+      <origin rpy="0 0 0.0541224233" xyz="0.04574712395 0 0"/>
+      <geometry>
+        <box size="0.0914942479 0.005 0.005"/>
+      </geometry>
+      <verbose value="Yes"/>
+    </collision>
+  </link>
+  <link name="l_gripper_r_parallel_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="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_gripper_tool_visual_geom">
-          <box size="0.001 0.001 0.001"/>
+        <origin xyz="${0.0914942479/2.0} 0 0" rpy="0 0 -0.0541224233" />
+        <geometry>
+          <box size="${0.0914942479} 0.005 0.005" />
         </geometry>
-        <material name="Grey"/>
+        <material name="Green" />
       </visual>
-      <collision>
-        <origin rpy="0 0 0" xyz="-0.1 0 0"/> <!-- move it out of the way -->
-        <geometry name="l_gripper_tool_collision_geom">
-          <box size="0.001 0.001 0.001"/>
-        </geometry>
-      </collision>
-    </link><joint name="l_gripper_l_finger_joint" type="revolute">
-      <axis xyz="0 0 1"/>
-
-      <limit effort="100.0" lower="0.0" upper="0.548" velocity="0.5"/><safety_controller k_position="100.0" k_velocity="100.0" soft_lower_limit="0.0" soft_upper_limit="0.548"/>
-
-      <dynamics damping="0.2"/>
-      <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 name="l_gripper_l_finger_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_gripper_l_finger_visual_geom">
-          <mesh filename="package://pr2_defs/meshes/upper_finger_l.stl"/>
-        </geometry>
-        
-        <material name="Grey"/>
-      </visual>
-      <collision name="l_gripper_l_finger_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_gripper_l_finger_collision_geom">
-          <mesh filename="package://pr2_defs/meshes/convex/upper_finger_l_convex.stlb"/>
-        </geometry>
-        <verbose value="Yes"/>
-      </collision>
-    </link><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"/>
-      <limit effort="100.0" lower="0.0" upper="0.548" velocity="0.5"/><safety_controller k_position="100.0" k_velocity="100.0" soft_lower_limit="0.0" soft_upper_limit="0.548"/>
-      <dynamics damping="0.2"/>
-      <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 name="l_gripper_r_finger_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_gripper_r_finger_visual_geom">
-          <mesh filename="package://pr2_defs/meshes/upper_finger_r.stl"/>
-        </geometry>
-        
-        <material name="Grey"/>
-      </visual>
-      <collision name="l_gripper_r_finger_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_gripper_r_finger_collision_geom">
-          <mesh filename="package://pr2_defs/meshes/convex/upper_finger_r_convex.stlb"/>
-        </geometry>
-        <verbose value="Yes"/>
-      </collision>
-    </link><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"/>
-      <limit effort="100.0" lower="0.0" upper="0.548" velocity="0.5"/><safety_controller k_position="100.0" k_velocity="100.0" soft_lower_limit="0.0" soft_upper_limit="0.548"/>
-      <dynamics damping="0.01"/>
-      <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 name="l_gripper_l_finger_tip_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_gripper_l_finger_tip_visual_geom">
-          <mesh filename="package://pr2_defs/meshes/finger_tip_l.stl"/>
-        </geometry>
-        
-        <material name="Green"/>
-      </visual>
-      <collision name="l_gripper_l_finger_tip_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_gripper_l_finger_tip_collision_geom">
-          <mesh filename="package://pr2_defs/meshes/finger_tip_pad2_l.stl"/>
-        </geometry>
-        <verbose value="Yes"/>
-      </collision>
-    </link><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"/>
-      <limit effort="100.0" lower="0.0" upper="0.548" velocity="0.5"/><safety_controller k_position="100.0" k_velocity="100.0" soft_lower_limit="0.0" soft_upper_limit="0.548"/>
-      <dynamics damping="0.01"/>
-      <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 name="l_gripper_r_finger_tip_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_gripper_r_finger_tip_visual_geom">
-          <mesh filename="package://pr2_defs/meshes/finger_tip_r.stl"/>
-        </geometry>
-        
-        <material name="Green"/>
-      </visual>
-      <collision name="l_gripper_r_finger_tip_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_gripper_r_finger_tip_collision_geom">
-          <mesh filename="package://pr2_defs/meshes/finger_tip_pad2_r.stl"/>
-        </geometry>
-        <verbose value="Yes"/>
-      </collision>
-    </link><gazebo reference="l_gripper_l_finger_link">
-      <turnGravityOff>true</turnGravityOff>
-      <sensor:contact name="l_gripper_l_finger_contact_sensor">
-        <geom>l_gripper_l_finger_link_geom</geom>
-        <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="l_gripper_l_finger_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>l_gripper_l_finger_bumper</bumperTopicName>
-          <interface:bumper name="l_gripper_l_finger_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <mu1 value="500.0"/>
-      <mu2 value="500.0"/>
-      <kp value="1000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/Grey"/>
-      <!-- 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><gazebo reference="l_gripper_r_finger_link">
-      <turnGravityOff>true</turnGravityOff>
-      <sensor:contact name="l_gripper_r_finger_contact_sensor">
-        <geom>l_gripper_r_finger_link_geom</geom>
-        <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="l_gripper_r_finger_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>l_gripper_r_finger_bumper</bumperTopicName>
-          <interface:bumper name="l_gripper_r_finger_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <mu1 value="500.0"/>
-      <mu2 value="500.0"/>
-      <kp value="1000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/Grey"/>
-    </gazebo><gazebo reference="l_gripper_r_finger_joint">
-      <stopKd value="1.0"/>
-      <stopKp value="10000000.0"/>
-      <fudgeFactor value="1.0"/>
-      <provideFeedback value="true"/>
-    </gazebo><gazebo reference="l_gripper_l_finger_tip_link">
-      <turnGravityOff>true</turnGravityOff>
-      <selfCollide>false</selfCollide>
-      <sensor:contact name="l_gripper_l_finger_tip_contact_sensor">
-        <geom>l_gripper_l_finger_tip_link_geom</geom>
-        <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="l_gripper_l_finger_tip_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>l_gripper_l_finger_tip_bumper</bumperTopicName>
-          <interface:bumper name="l_gripper_l_finger_tip_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <mu1 value="500.0"/>
-      <mu2 value="500.0"/>
-      <kp value="10000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/Red"/>
-    </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><gazebo reference="l_gripper_r_finger_tip_link">
-      <turnGravityOff>true</turnGravityOff>
-      <selfCollide>false</selfCollide>
-      <sensor:contact name="l_gripper_r_finger_tip_contact_sensor">
-        <geom>l_gripper_r_finger_tip_link_geom</geom>
-        <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="l_gripper_r_finger_tip_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>l_gripper_r_finger_tip_bumper</bumperTopicName>
-          <interface:bumper name="l_gripper_r_finger_tip_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <mu1 value="500.0"/>
-      <mu2 value="500.0"/>
-      <kp value="10000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/Red"/>
-    </gazebo><gazebo>
-      <controller:gazebo_ros_p3d name="p3d_l_gripper_l_finger_controller" plugin="libgazebo_ros_p3d.so">
+-->
+    <collision>
+      <origin rpy="0 0 -0.0541224233" xyz="0.04574712395 0 0"/>
+      <geometry>
+        <box size="0.0914942479 0.005 0.005"/>
+      </geometry>
+      <verbose value="Yes"/>
+    </collision>
+  </link>
+  <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"/>
+    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
+    <dynamics damping="0.01"/>
+    <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="package://pr2_description/meshes/gripper_v0/l_finger_tip.stl"/>
+      </geometry>
+      <material name="Green"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.stl"/>
+      </geometry>
+      <verbose value="Yes"/>
+    </collision>
+  </link>
+  <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"/>
+    <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
+    <dynamics damping="0.01"/>
+    <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="package://pr2_description/meshes/gripper_v0/l_finger_tip.stl"/>
+      </geometry>
+      <material name="Green"/>
+    </visual>
+    <collision>
+      <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.stl"/>
+      </geometry>
+      <verbose value="Yes"/>
+    </collision>
+  </link>
+  <gazebo reference="l_gripper_l_finger_link">
+    <turnGravityOff>true</turnGravityOff>
+    <sensor:contact name="l_gripper_l_finger_contact_sensor">
+      <geom>l_gripper_l_finger_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="l_gripper_l_finger_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
         <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>map</frameName>
-        <interface:position name="p3d_l_gripper_l_finger_position_iface"/>
-      </controller:gazebo_ros_p3d>
-      <controller:gazebo_ros_f3d name="f3d_l_gripper_l_finger_controller" plugin="libgazebo_ros_f3d.so">
+        <bumperTopicName>l_gripper_l_finger_bumper</bumperTopicName>
+        <interface:bumper name="l_gripper_l_finger_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <mu1 value="500.0"/>
+    <mu2 value="500.0"/>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+    <material value="PR2/Grey"/>
+    <!-- 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>
+  <gazebo reference="l_gripper_r_finger_link">
+    <turnGravityOff>true</turnGravityOff>
+    <sensor:contact name="l_gripper_r_finger_contact_sensor">
+      <geom>l_gripper_r_finger_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="l_gripper_r_finger_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
         <alwaysOn>true</alwaysOn>
         <updateRate>100.0</updateRate>
-        <bodyName>l_gripper_l_finger_link</bodyName>
-        <topicName>l_gripper_l_finger_force_ground_truth</topicName>
-        <frameName>l_gripper_l_finger_link</frameName>
-        <interface:position name="f3d_l_gripper_l_finger_force_iface"/>
-      </controller:gazebo_ros_f3d>
-    </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><joint name="l_gripper_joint" type="prismatic">
-      <axis xyz="0 0 1"/>
-      <limit effort="100.0" lower="0.0" upper="0.09" velocity="0.2"/>
-      <safety_controller k_position="20.0" k_velocity="500.0" soft_lower_limit="-0.01" soft_upper_limit="0.088"/>
-    </joint><gazebo reference="l_gripper_palm_link">
-      <turnGravityOff>true</turnGravityOff>
-      <sensor:contact name="l_gripper_palm_contact_sensor">
-        <geom>l_gripper_palm_link_geom</geom>
-        <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="l_gripper_palm_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>l_gripper_palm_bumper</bumperTopicName>
-          <interface:bumper name="l_gripper_palm_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <material value="PR2/Red"/>
-    </gazebo><gazebo reference="l_gripper_float_link">
-      <turnGravityOff>true</turnGravityOff>
-      <material value="PR2/White"/>
-    </gazebo><gazebo>
-      <!-- floating link for enforcing gripper constraints -->
-      <body:box name="l_gripper_float_link">
-          <massMatrix>true</massMatrix>
-          <mass>0.01</mass>
-          <ixx>0.0001</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.0001</iyy>
-          <iyz>0</iyz>
-          <izz>0.0001</izz>
-          <cx>0</cx>
-          <cy>0</cy>
-          <cz>0</cz>
-          <xyz>0.82025 0.188 0.790675</xyz>
-          <rpy>0 -0 0</rpy>
-          <geom:box name="l_gripper_float_link_geom">
-              <xyz>0 0 0</xyz>
-              <rpy>0 -0 0</rpy>
-              <size>0.0 0.0 0.0</size>
-              <visual>
-                  <xyz>0 0 0</xyz>
-                  <rpy>0 -0 0</rpy>
-                  <scale>0.0 0.0 0.0</scale>
-                  <mesh>unit_box</mesh>
-                  <material>PR2/White</material>
-              </visual>
-          </geom:box>
-          <turnGravityOff>true</turnGravityOff>
-          <selfCollide>false</selfCollide>
-      </body:box>
-      <joint:slider name="l_gripper_float_joint">
-          <body1>l_gripper_float_link</body1>
-          <body2>l_gripper_palm_link</body2>
-          <anchor>l_gripper_float_link</anchor>
-          <axis>1 0 0</axis>
-          <lowStop>-0.05</lowStop>
-          <highStop>0.001</highStop>
-      </joint:slider>
-      <joint:slider name="l_gripper_l_finger_tip_slider_joint">
-          <body1>l_gripper_l_finger_tip_link</body1>
-          <body2>l_gripper_float_link</body2>
-          <anchor>l_gripper_l_finger_tip_link</anchor>
-          <axis>0 1 0</axis>
-          <anchorOffset>0 0 0</anchorOffset>
-      </joint:slider>
-      <joint:slider name="l_gripper_r_finger_tip_slider_joint">
-          <body1>l_gripper_r_finger_tip_link</body1>
-          <body2>l_gripper_float_link</body2>
-          <anchor>l_gripper_r_finger_tip_link</anchor>
-          <axis>0 1 0</axis>
-          <anchorOffset>0 0 0</anchorOffset>
-      </joint:slider>
-      <!--
-      <joint:slider name="${side}_gripper_finger_tip_slider_joint">
-          <body1>${side}_gripper_l_finger_tip_link</body1>
-          <body2>${side}_gripper_r_finger_tip_link</body2>
-          <anchor>${side}_gripper_l_finger_tip_link</anchor>
-          <axis>0 1 0</axis>
-          <anchorOffset>0 0 0</anchorOffset>
-          <lowStop>0.0</lowStop>
-          <highStop>0.08</highStop>
-      </joint:slider>
-      -->
-    </gazebo><gazebo>
-      <controller:gazebo_ros_p3d name="p3d_l_gripper_palm_controller" plugin="libgazebo_ros_p3d.so">
+        <bumperTopicName>l_gripper_r_finger_bumper</bumperTopicName>
+        <interface:bumper name="l_gripper_r_finger_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <mu1 value="500.0"/>
+    <mu2 value="500.0"/>
+    <kp value="1000000.0"/>
+    <kd value="1.0"/>
+    <material value="PR2/Grey"/>
+  </gazebo>
+  <gazebo reference="l_gripper_r_finger_joint">
+    <stopKd value="1.0"/>
+    <stopKp value="10000000.0"/>
+    <fudgeFactor value="1.0"/>
+    <provideFeedback value="true"/>
+  </gazebo>
+  <gazebo reference="l_gripper_l_finger_tip_link">
+    <turnGravityOff>true</turnGravityOff>
+    <selfCollide>false</selfCollide>
+    <sensor:contact name="l_gripper_l_finger_tip_contact_sensor">
+      <geom>l_gripper_l_finger_tip_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="l_gripper_l_finger_tip_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
         <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>
-        <interface:position name="p3d_l_gripper_palm_position_iface"/>
-      </controller:gazebo_ros_p3d>
-      <controller:gazebo_ros_p3d name="p3d_l_gripper_tool_frame" plugin="libgazebo_ros_p3d.so">
+        <bumperTopicName>l_gripper_l_finger_tip_bumper</bumperTopicName>
+        <interface:bumper name="l_gripper_l_finger_tip_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <mu1 value="500.0"/>
+    <mu2 value="500.0"/>
+    <kp value="10000000.0"/>
+    <kd value="1.0"/>
+    <material value="PR2/Grey"/>
+  </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>
+  <gazebo reference="l_gripper_r_finger_tip_link">
+    <turnGravityOff>true</turnGravityOff>
+    <selfCollide>false</selfCollide>
+    <sensor:contact name="l_gripper_r_finger_tip_contact_sensor">
+      <geom>l_gripper_r_finger_tip_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="l_gripper_r_finger_tip_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
         <alwaysOn>true</alwaysOn>
         <updateRate>100.0</updateRate>
-        <bodyName>l_gripper_tool_frame</bodyName>
-        <topicName>l_gripper_tool_frame_pose_ground_truth</topicName>
-        <xyzOffsets>0 0 0</xyzOffsets>
-        <rpyOffsets>0 0 0</rpyOffsets>
-        <gaussianNoise>0.0</gaussianNoise>
-        <frameName>/map</frameName>
-        <interface:position name="p3d_l_gripper_tool_frame_position_iface"/>
-      </controller:gazebo_ros_p3d>
-    </gazebo><gazebo reference="l_gripper_tool_frame">
-      <turnGravityOff>true</turnGravityOff>
-      <material value="PR2/Grey"/>
-    </gazebo><transmission name="l_gripper_trans" type="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"/>
-      <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"/>
-    </transmission>
-
-  <!-- Forearm Cam (Hand approximated values) -->
+        <bumperTopicName>l_gripper_r_finger_tip_bumper</bumperTopicName>
+        <interface:bumper name="l_gripper_r_finger_tip_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <mu1 value="500.0"/>
+    <mu2 value="500.0"/>
+    <kp value="10000000.0"/>
+    <kd value="1.0"/>
+    <material value="PR2/Grey"/>
+  </gazebo>
+  <gazebo>
+    <controller:gazebo_ros_p3d name="p3d_l_gripper_l_finger_controller" plugin="libgazebo_ros_p3d.so">
+      <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>
+      <interface:position name="p3d_l_gripper_l_finger_position_iface"/>
+    </controller:gazebo_ros_p3d>
+    <controller:gazebo_ros_f3d name="f3d_l_gripper_l_finger_controller" plugin="libgazebo_ros_f3d.so">
+      <alwaysOn>true</alwaysOn>
+      <updateRate>100.0</updateRate>
+      <bodyName>l_gripper_l_finger_link</bodyName>
+      <topicName>l_gripper_l_finger_force_ground_truth</topicName>
+      <frameName>l_gripper_l_finger_link</frameName>
+      <interface:position name="f3d_l_gripper_l_finger_force_iface"/>
+    </controller:gazebo_ros_f3d>
+  </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>
+  <gazebo>
+    <joint:hinge name="l_gripper_r_parallel_tip_joint">
+      <body1>l_gripper_r_parallel_link</body1>
+      <body2>l_gripper_r_finger_tip_link</body2>
+      <anchor>l_gripper_r_finger_tip_link</anchor>
+      <axis>0 0 1</axis>
+      <anchorOffset>-0.018 -0.021 0</anchorOffset>
+    </joint:hinge>
+    <joint:hinge name="l_gripper_l_parallel_tip_joint">
+      <body1>l_gripper_l_parallel_link</body1>
+      <body2>l_gripper_l_finger_tip_link</body2>
+      <anchor>l_gripper_l_finger_tip_link</anchor>
+      <axis>0 0 1</axis>
+      <anchorOffset>-0.018 0.021 0</anchorOffset>
+    </joint:hinge>
+    <joint:slider name="l_gripper_joint">
+      <body1>l_gripper_l_finger_tip_link</body1>
+      <body2>l_gripper_r_finger_tip_link</body2>
+      <anchor>l_gripper_r_finger_tip_link</anchor>
+      <axis>0 1 0</axis>
+    </joint:slider>
+  </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>
+  <joint name="l_gripper_joint" type="prismatic">
+    <axis xyz="0 1 0"/>
+    <dynamics damping="100.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>
+  <gazebo reference="l_gripper_palm_link">
+    <turnGravityOff>true</turnGravityOff>
+    <sensor:contact name="l_gripper_palm_contact_sensor">
+      <geom>l_gripper_palm_link_geom</geom>
+      <updateRate>100.0</updateRate>
+      <controller:gazebo_ros_bumper name="l_gripper_palm_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
+        <alwaysOn>true</alwaysOn>
+        <updateRate>100.0</updateRate>
+        <bumperTopicName>l_gripper_palm_bumper</bumperTopicName>
+        <interface:bumper name="l_gripper_palm_gazebo_ros_bumper_iface"/>
+      </controller:gazebo_ros_bumper>
+    </sensor:contact>
+    <material value="PR2/Grey"/>
+  </gazebo>
+  <gazebo>
+    <controller:gazebo_ros_p3d name="p3d_l_gripper_palm_controller" plugin="libgazebo_ros_p3d.so">
+      <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>
+      <interface:position name="p3d_l_gripper_palm_position_iface"/>
+    </controller:gazebo_ros_p3d>
+  </gazebo>
+  <transmission name="l_gripper_trans" type="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 -->
+    <use_simulated_gripper_joint/>
+    <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"/>
+    <passive_joint name="l_gripper_r_parallel_root_joint"/>
+    <passive_joint name="l_gripper_l_parallel_root_joint"/>
+  </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.785398163397 0" xyz=".15 0 .07"/>
-      <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>
-
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </visual>
-
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </collision>
-    </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">
-
-
-      <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>
-
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </visual>
-
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="l_forearm_cam_frame">
-      <sensor:camera name="l_forearm_cam_sensor">
-        <!--imageSize>${image_width} ${image_height}</imageSize-->
-        <imageSize>640 480</imageSize> <!-- FIXME: temporary fix because xacro expands above line to floats, not integers -->
-        <imageFormat>L8</imageFormat>
-        <hfov>90.0</hfov>
-        <nearClip>0.1</nearClip>
-        <farClip>100</farClip>
+    <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>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+    </collision>
+  </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"/>
+  <gazebo reference="l_forearm_cam_frame">
+    <sensor:camera name="l_forearm_cam_sensor">
+      <imageSize>640 480</imageSize>
+      <imageFormat>L8</imageFormat>
+      <hfov>90</hfov>
+      <nearClip>0.1</nearClip>
+      <farClip>100</farClip>
+      <updateRate>25.0</updateRate>
+      <controller:gazebo_ros_camera name="l_forearm_cam_controller" plugin="libgazebo_ros_camera.so">
+        <alwaysOn>true</alwaysOn>
         <updateRate>25.0</updateRate>
-        <controller:gazebo_ros_camera name="l_forearm_cam_controller" plugin="libgazebo_ros_camera.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>25.0</updateRate>
-          <imageTopicName>l_forearm_cam/image_raw</imageTopicName>
-          <cameraInfoTopicName>l_forearm_cam/camera_info</cameraInfoTopicName>
-          <frameName>l_forearm_cam_frame</frameName>
-          <hackBaseline>0.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 -->
-          <focal_length>320.0</focal_length>
-          <distortion_k1>0</distortion_k1>
-          <distortion_k2>0</distortion_k2>
-          <distortion_k3>0</distortion_k3>
-          <distortion_t1>0</distortion_t1>
-          <distortion_t2>0</distortion_t2>
-          <interface:camera name="l_forearm_cam_iface"/>
-        </controller:gazebo_ros_camera>
-      </sensor:camera>
-      <turnGravityOff>true</turnGravityOff>
-      <material>PR2/Blue</material>
-    </gazebo><gazebo reference="l_forearm_cam_optical_frame">
-      <turnGravityOff>true</turnGravityOff>
-    </gazebo>
+        <imageTopicName>l_forearm_cam/image_raw</imageTopicName>
+        <cameraInfoTopicName>l_forearm_cam/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 -->
+        <focal_length>320</focal_length>
+        <distortion_k1>0.00000001</distortion_k1>
+        <distortion_k2>0.00000001</distortion_k2>
+        <distortion_k3>0.00000001</distortion_k3>
+        <distortion_t1>0.00000001</distortion_t1>
+        <distortion_t2>0.00000001</distortion_t2>
+        <interface:camera name="l_forearm_cam_iface"/>
+      </controller:gazebo_ros_camera>
+    </sensor:camera>
+    <turnGravityOff>true</turnGravityOff>
+    <material>PR2/Blue</material>
+  </gazebo>
   <joint name="r_forearm_cam_frame_joint" type="fixed">
-      <origin rpy="-1.57079632679 -0.785398163397 0" xyz=".15 0 .07"/>
-      <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>
-
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </visual>
-
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </collision>
-    </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">
-
-
-      <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>
-
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </visual>
-
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="r_forearm_cam_frame">
-      <sensor:camera name="r_forearm_cam_sensor">
-        <!--imageSize>${image_width} ${image_height}</imageSize-->
-        <imageSize>640 480</imageSize> <!-- FIXME: temporary fix because xacro expands above line to floats, not integers -->
-        <imageFormat>L8</imageFormat>
-        <hfov>90.0</hfov>
-        <nearClip>0.1</nearClip>
-        <farClip>100</farClip>
+    <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>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+    </collision>
+  </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"/>
+  <gazebo reference="r_forearm_cam_frame">
+    <sensor:camera name="r_forearm_cam_sensor">
+      <imageSize>640 480</imageSize>
+      <imageFormat>L8</imageFormat>
+      <hfov>90</hfov>
+      <nearClip>0.1</nearClip>
+      <farClip>100</farClip>
+      <updateRate>25.0</updateRate>
+      <controller:gazebo_ros_camera name="r_forearm_cam_controller" plugin="libgazebo_ros_camera.so">
+        <alwaysOn>true</alwaysOn>
         <updateRate>25.0</updateRate>
-        <controller:gazebo_ros_camera name="r_forearm_cam_controller" plugin="libgazebo_ros_camera.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>25.0</updateRate>
-          <imageTopicName>r_forearm_cam/image_raw</imageTopicName>
-          <cameraInfoTopicName>r_forearm_cam/camera_info</cameraInfoTopicName>
-          <frameName>r_forearm_cam_frame</frameName>
-          <hackBaseline>0.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 -->
-          <focal_length>320.0</focal_length>
-          <distortion_k1>0</distortion_k1>
-          <distortion_k2>0</distortion_k2>
-          <distortion_k3>0</distortion_k3>
-          <distortion_t1>0</distortion_t1>
-          <distortion_t2>0</distortion_t2>
-          <interface:camera name="r_forearm_cam_iface"/>
-        </controller:gazebo_ros_camera>
-      </sensor:camera>
-      <turnGravityOff>true</turnGravityOff>
-      <material>PR2/Blue</material>
-    </gazebo><gazebo reference="r_forearm_cam_optical_frame">
-      <turnGravityOff>true</turnGravityOff>
-    </gazebo>
-
+        <imageTopicName>r_forearm_cam/image_raw</imageTopicName>
+        <cameraInfoTopicName>r_forearm_cam/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 -->
+        <focal_length>320</focal_length>
+        <distortion_k1>0.00000001</distortion_k1>
+        <distortion_k2>0.00000001</distortion_k2>
+        <distortion_k3>0.00000001</distortion_k3>
+        <distortion_t1>0.00000001</distortion_t1>
+        <distortion_t2>0.00000001</distortion_t2>
+        <interface:camera name="r_forearm_cam_iface"/>
+      </controller:gazebo_ros_camera>
+    </sensor:camera>
+    <turnGravityOff>true</turnGravityOff>
+    <material>PR2/Blue</material>
+  </gazebo>
 </robot>
diff --git a/kdl_parser/test/test_kdl_parser.cpp b/kdl_parser/test/test_kdl_parser.cpp
index c229cd2..9f28362 100644
--- a/kdl_parser/test/test_kdl_parser.cpp
+++ b/kdl_parser/test/test_kdl_parser.cpp
@@ -73,9 +73,9 @@ TEST_F(TestParser, test)
   }
 
   ASSERT_TRUE(treeFromFile(g_argv[g_argc-1], my_tree));
-  ASSERT_EQ(my_tree.getNrOfJoints(), (unsigned int)38);
-  ASSERT_EQ(my_tree.getNrOfSegments(), (unsigned int)72);
-  ASSERT_TRUE(my_tree.getSegment("world") == my_tree.getRootSegment());
+  ASSERT_EQ(my_tree.getNrOfJoints(), (unsigned int)42);
+  ASSERT_EQ(my_tree.getNrOfSegments(), (unsigned int)79);
+  ASSERT_TRUE(my_tree.getSegment("base_footprint") == my_tree.getRootSegment());
   ASSERT_EQ(my_tree.getRootSegment()->second.children.size(), (unsigned int)1);
   ASSERT_TRUE(my_tree.getSegment("base_link")->second.parent == my_tree.getRootSegment());
   ASSERT_EQ(my_tree.getSegment("base_link")->second.segment.getInertia().getMass(), 116.0);