From 2798a1691ce1ee067da89c19e786abe512ca6853 Mon Sep 17 00:00:00 2001
From: Wim Meeussen <wim@willowgarage.com>
Date: Thu, 24 Mar 2011 17:46:27 -0700
Subject: [PATCH] Clean up code and get regression tests working

---
 .../robot_state_publisher.h                   |   5 +-
 .../src/robot_state_publisher.cpp             |  15 +-
 robot_state_publisher/test/pr2.urdf           | 958 +++++++-----------
 .../test/test_publisher.launch                |   3 +-
 4 files changed, 367 insertions(+), 614 deletions(-)

diff --git a/robot_state_publisher/include/robot_state_publisher/robot_state_publisher.h b/robot_state_publisher/include/robot_state_publisher/robot_state_publisher.h
index 0355c13..9ea834c 100644
--- a/robot_state_publisher/include/robot_state_publisher/robot_state_publisher.h
+++ b/robot_state_publisher/include/robot_state_publisher/robot_state_publisher.h
@@ -72,9 +72,8 @@ public:
   /** Publish transforms to tf 
    * \param joint_positions A map of joint names and joint positions. 
    * \param time The time at which the joint positions were recorded
-   * returns true on success; return false when the robot model is empty or not all the joints in the robot model are specified in the joint map.
    */
-  bool publishTransforms(const std::map<std::string, double>& joint_positions, const ros::Time& time);
+  void publishTransforms(const std::map<std::string, double>& joint_positions, const ros::Time& time);
   void publishFixedTransforms();
 
 private:
@@ -82,7 +81,7 @@ private:
 
 
   std::map<std::string, SegmentPair> segments_, segments_fixed_;
-  tf::TransformBroadcaster tf_broadcaster_, tf_broadcaster_fixed_;
+  tf::TransformBroadcaster tf_broadcaster_;
 };
 
 
diff --git a/robot_state_publisher/src/robot_state_publisher.cpp b/robot_state_publisher/src/robot_state_publisher.cpp
index 6202394..d26927a 100644
--- a/robot_state_publisher/src/robot_state_publisher.cpp
+++ b/robot_state_publisher/src/robot_state_publisher.cpp
@@ -63,11 +63,11 @@ namespace robot_state_publisher{
       SegmentPair s(children[i]->second.segment, root, child.getName());
       if (child.getJoint().getType() == KDL::Joint::None){
         segments_fixed_.insert(make_pair(child.getJoint().getName(), s));
-        ROS_INFO("Adding fixed segment from %s to %s", root.c_str(), child.getName().c_str());
+        ROS_DEBUG("Adding fixed segment from %s to %s", root.c_str(), child.getName().c_str());
       }
       else{
         segments_.insert(make_pair(child.getJoint().getName(), s));
-        ROS_INFO("Adding moving segment from %s to %s", root.c_str(), child.getName().c_str());
+        ROS_DEBUG("Adding moving segment from %s to %s", root.c_str(), child.getName().c_str());
       }
       addChildren(children[i]);
     }
@@ -75,10 +75,10 @@ namespace robot_state_publisher{
 
 
   // publish moving transforms
-  bool RobotStatePublisher::publishTransforms(const map<string, double>& joint_positions, const Time& time)
+  void RobotStatePublisher::publishTransforms(const map<string, double>& joint_positions, const Time& time)
   {
+    ROS_DEBUG("Publishing transforms for moving joints");
     std::vector<tf::StampedTransform> tf_transforms;
-
     tf::StampedTransform tf_transform;
     tf_transform.stamp_ = time;
 
@@ -92,18 +92,15 @@ namespace robot_state_publisher{
         tf_transforms.push_back(tf_transform);
       }
     }
-
     tf_broadcaster_.sendTransform(tf_transforms);
-
-    return true;
   }
 
 
   // publish fixed transforms
   void RobotStatePublisher::publishFixedTransforms()
   {
+    ROS_DEBUG("Publishing transforms for moving joints");
     std::vector<tf::StampedTransform> tf_transforms;
-
     tf::StampedTransform tf_transform;
     tf_transform.stamp_ = ros::Time::now()+ros::Duration(0.5);  // future publish by 0.5 seconds
 
@@ -114,7 +111,7 @@ namespace robot_state_publisher{
       tf_transform.child_frame_id_ = seg->second.tip;
       tf_transforms.push_back(tf_transform);
     }
-    tf_broadcaster_fixed_.sendTransform(tf_transforms);
+    tf_broadcaster_.sendTransform(tf_transforms);
   }
 
 }
diff --git a/robot_state_publisher/test/pr2.urdf b/robot_state_publisher/test/pr2.urdf
index 07308a0..ae3ee20 100644
--- a/robot_state_publisher/test/pr2.urdf
+++ b/robot_state_publisher/test/pr2.urdf
@@ -1,6 +1,6 @@
 <?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 | -->
+<!-- |    This document was autogenerated by xacro from /wg/nhx/wim/ros_diamondback/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">
@@ -14,6 +14,7 @@
   <!-- PR2 base -->
   <!-- Head sensors -->
   <!-- Camera sensors -->
+  <!-- Texture projector -->
   <!-- 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">
@@ -54,17 +55,23 @@
   <material name="White">
     <color rgba="1.0 1.0 1.0 1.0"/>
   </material>
+  <material name="Black">
+    <color rgba="0.1 0.1 0.1 1.0"/>
+  </material>
+  <material name="LightGrey">
+    <color rgba="0.6 0.6 0.6 1.0"/>
+  </material>
   <material name="Caster">
-    <texture filename="package://pr2_ogre/Media/materials/textures/pr2_caster_texture.png"/>
+    <texture filename="package://pr2_description/materials/textures/pr2_caster_texture.png"/>
   </material>
   <material name="Wheel_l">
-    <texture filename="package://pr2_ogre/Media/materials/textures/pr2_wheel_left.png"/>
+    <texture filename="package://pr2_description/materials/textures/pr2_wheel_left.png"/>
   </material>
   <material name="Wheel_r">
-    <texture filename="package://pr2_ogre/Media/materials/textures/pr2_wheel_right.png"/>
+    <texture filename="package://pr2_description/materials/textures/pr2_wheel_right.png"/>
   </material>
   <material name="RollLinks">
-    <texture filename="package://pr2_ogre/Media/materials/textures/pr2_wheel_left.png"/>
+    <texture filename="package://pr2_description/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.
@@ -83,7 +90,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://pr2_description/meshes/base_v0/base.stl"/>
+        <mesh filename="package://pr2_description/meshes/base_v0/base.dae"/>
       </geometry>
       <material name="White"/>
     </visual>
@@ -121,6 +128,32 @@
     <child link="base_link"/>
     <parent link="base_footprint"/>
   </joint>
+  <link name="base_bellow_link">
+    <inertial>
+      <mass value="1.0"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.05 0.37 0.3"/>
+      </geometry>
+      <material name="Black"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.05 0.37 0.3"/>
+      </geometry>
+      <material name="Black"/>
+    </collision>
+  </link>
+  <joint name="base_bellow_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="-0.29 0 0.8"/>
+    <parent link="base_link"/>
+    <child link="base_bellow_link"/>
+  </joint>
   <joint name="base_laser_joint" type="fixed">
     <axis xyz="0 1 0"/>
     <origin rpy="0 0 0" xyz="0.275 0.0 0.252"/>
@@ -133,19 +166,6 @@
       <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">
@@ -156,7 +176,7 @@
       <displayRays>false</displayRays>
       <minAngle>-129.998394137</minAngle>
       <maxAngle>129.998394137</maxAngle>
-      <minRange>0.05</minRange>
+      <minRange>0.08</minRange>
       <maxRange>10.0</maxRange>
       <resRange>0.01</resRange>
       <updateRate>20</updateRate>
@@ -175,8 +195,7 @@
     <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 -->
+    <calibration rising="-0.785398163397"/>
     <dynamics damping="0.0" friction="0.0"/>
     <origin rpy="0 0 0" xyz="0.2246 0.2246 0.0282"/>
     <parent link="base_link"/>
@@ -202,7 +221,7 @@
       </geometry>
     </collision>
   </link>
-  <transmission name="fl_caster_rotation_trans" type="SimpleTransmission">
+  <transmission name="fl_caster_rotation_trans" type="pr2_mechanism_model/SimpleTransmission">
     <actuator name="fl_caster_rotation_motor"/>
     <joint name="fl_caster_rotation_joint"/>
     <mechanicalReduction>-79.2380952381</mechanicalReduction>
@@ -226,7 +245,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://pr2_description/meshes/base_v0/wheel.stl"/>
+        <mesh filename="package://pr2_description/meshes/base_v0/wheel.dae"/>
       </geometry>
       <material name="Wheel_l"/>
     </visual>
@@ -243,9 +262,8 @@
     <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">
+  <transmission name="fl_caster_l_wheel_trans" type="pr2_mechanism_model/SimpleTransmission">
     <actuator name="fl_caster_l_wheel_motor"/>
     <joint name="fl_caster_l_wheel_joint"/>
     <mechanicalReduction>79.2380952381</mechanicalReduction>
@@ -269,7 +287,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://pr2_description/meshes/base_v0/wheel.stl"/>
+        <mesh filename="package://pr2_description/meshes/base_v0/wheel.dae"/>
       </geometry>
       <material name="Wheel_r"/>
     </visual>
@@ -286,9 +304,8 @@
     <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">
+  <transmission name="fl_caster_r_wheel_trans" type="pr2_mechanism_model/SimpleTransmission">
     <actuator name="fl_caster_r_wheel_motor"/>
     <joint name="fl_caster_r_wheel_joint"/>
     <mechanicalReduction>-79.2380952381</mechanicalReduction>
@@ -301,8 +318,7 @@
     <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 -->
+    <calibration rising="-0.785398163397"/>
     <dynamics damping="0.0" friction="0.0"/>
     <origin rpy="0 0 0" xyz="0.2246 -0.2246 0.0282"/>
     <parent link="base_link"/>
@@ -328,7 +344,7 @@
       </geometry>
     </collision>
   </link>
-  <transmission name="fr_caster_rotation_trans" type="SimpleTransmission">
+  <transmission name="fr_caster_rotation_trans" type="pr2_mechanism_model/SimpleTransmission">
     <actuator name="fr_caster_rotation_motor"/>
     <joint name="fr_caster_rotation_joint"/>
     <mechanicalReduction>-79.2380952381</mechanicalReduction>
@@ -352,7 +368,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://pr2_description/meshes/base_v0/wheel.stl"/>
+        <mesh filename="package://pr2_description/meshes/base_v0/wheel.dae"/>
       </geometry>
       <material name="Wheel_l"/>
     </visual>
@@ -369,9 +385,8 @@
     <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">
+  <transmission name="fr_caster_l_wheel_trans" type="pr2_mechanism_model/SimpleTransmission">
     <actuator name="fr_caster_l_wheel_motor"/>
     <joint name="fr_caster_l_wheel_joint"/>
     <mechanicalReduction>79.2380952381</mechanicalReduction>
@@ -395,7 +410,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://pr2_description/meshes/base_v0/wheel.stl"/>
+        <mesh filename="package://pr2_description/meshes/base_v0/wheel.dae"/>
       </geometry>
       <material name="Wheel_r"/>
     </visual>
@@ -412,9 +427,8 @@
     <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">
+  <transmission name="fr_caster_r_wheel_trans" type="pr2_mechanism_model/SimpleTransmission">
     <actuator name="fr_caster_r_wheel_motor"/>
     <joint name="fr_caster_r_wheel_joint"/>
     <mechanicalReduction>-79.2380952381</mechanicalReduction>
@@ -427,8 +441,7 @@
     <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 -->
+    <calibration rising="2.35619449019"/>
     <dynamics damping="0.0" friction="0.0"/>
     <origin rpy="0 0 0" xyz="-0.2246 0.2246 0.0282"/>
     <parent link="base_link"/>
@@ -454,7 +467,7 @@
       </geometry>
     </collision>
   </link>
-  <transmission name="bl_caster_rotation_trans" type="SimpleTransmission">
+  <transmission name="bl_caster_rotation_trans" type="pr2_mechanism_model/SimpleTransmission">
     <actuator name="bl_caster_rotation_motor"/>
     <joint name="bl_caster_rotation_joint"/>
     <mechanicalReduction>-79.2380952381</mechanicalReduction>
@@ -478,7 +491,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://pr2_description/meshes/base_v0/wheel.stl"/>
+        <mesh filename="package://pr2_description/meshes/base_v0/wheel.dae"/>
       </geometry>
       <material name="Wheel_l"/>
     </visual>
@@ -495,9 +508,8 @@
     <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">
+  <transmission name="bl_caster_l_wheel_trans" type="pr2_mechanism_model/SimpleTransmission">
     <actuator name="bl_caster_l_wheel_motor"/>
     <joint name="bl_caster_l_wheel_joint"/>
     <mechanicalReduction>79.2380952381</mechanicalReduction>
@@ -521,7 +533,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://pr2_description/meshes/base_v0/wheel.stl"/>
+        <mesh filename="package://pr2_description/meshes/base_v0/wheel.dae"/>
       </geometry>
       <material name="Wheel_r"/>
     </visual>
@@ -538,9 +550,8 @@
     <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">
+  <transmission name="bl_caster_r_wheel_trans" type="pr2_mechanism_model/SimpleTransmission">
     <actuator name="bl_caster_r_wheel_motor"/>
     <joint name="bl_caster_r_wheel_joint"/>
     <mechanicalReduction>-79.2380952381</mechanicalReduction>
@@ -553,8 +564,7 @@
     <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 -->
+    <calibration rising="2.35619449019"/>
     <dynamics damping="0.0" friction="0.0"/>
     <origin rpy="0 0 0" xyz="-0.2246 -0.2246 0.0282"/>
     <parent link="base_link"/>
@@ -580,7 +590,7 @@
       </geometry>
     </collision>
   </link>
-  <transmission name="br_caster_rotation_trans" type="SimpleTransmission">
+  <transmission name="br_caster_rotation_trans" type="pr2_mechanism_model/SimpleTransmission">
     <actuator name="br_caster_rotation_motor"/>
     <joint name="br_caster_rotation_joint"/>
     <mechanicalReduction>-79.2380952381</mechanicalReduction>
@@ -604,7 +614,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://pr2_description/meshes/base_v0/wheel.stl"/>
+        <mesh filename="package://pr2_description/meshes/base_v0/wheel.dae"/>
       </geometry>
       <material name="Wheel_l"/>
     </visual>
@@ -621,9 +631,8 @@
     <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">
+  <transmission name="br_caster_l_wheel_trans" type="pr2_mechanism_model/SimpleTransmission">
     <actuator name="br_caster_l_wheel_motor"/>
     <joint name="br_caster_l_wheel_joint"/>
     <mechanicalReduction>79.2380952381</mechanicalReduction>
@@ -647,7 +656,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://pr2_description/meshes/base_v0/wheel.stl"/>
+        <mesh filename="package://pr2_description/meshes/base_v0/wheel.dae"/>
       </geometry>
       <material name="Wheel_r"/>
     </visual>
@@ -664,9 +673,8 @@
     <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">
+  <transmission name="br_caster_r_wheel_trans" type="pr2_mechanism_model/SimpleTransmission">
     <actuator name="br_caster_r_wheel_motor"/>
     <joint name="br_caster_r_wheel_joint"/>
     <mechanicalReduction>-79.2380952381</mechanicalReduction>
@@ -675,7 +683,7 @@
     <material value="PR2/caster_texture"/>
   </gazebo>
   <gazebo reference="base_link">
-    <selfCollide>true</selfCollide>
+    <selfCollide>false</selfCollide>
     <sensor:contact name="base_contact_sensor">
       <geom>base_link_geom</geom>
       <updateRate>100.0</updateRate>
@@ -686,7 +694,9 @@
         <interface:bumper name="base_bumper_iface"/>
       </controller:gazebo_ros_bumper>
     </sensor:contact>
-    <material value="PR2/caster_texture"/>
+  </gazebo>
+  <gazebo reference="base_bellow_link">
+    <material value="PR2/Black"/>
   </gazebo>
   <gazebo>
     <controller:gazebo_ros_p3d name="p3d_base_controller" plugin="libgazebo_ros_p3d.so">
@@ -708,8 +718,7 @@
     <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"/>
+    <calibration falling="0.00536"/>
     <dynamics damping="20000.0"/>
     <!-- strictly tuned for sim only right now -->
     <origin rpy="0 0 0" xyz="-0.05 0 0.739675"/>
@@ -725,7 +734,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://pr2_description/meshes/torso_v0/torso_lift.stl"/>
+        <mesh filename="package://pr2_description/meshes/torso_v0/torso_lift.dae"/>
       </geometry>
       <material name="Grey2"/>
     </visual>
@@ -736,6 +745,34 @@
       </geometry>
     </collision>
   </link>
+  <joint name="l_torso_lift_side_plate_joint" type="fixed">
+    <origin xyz="0.0535 -0.209285 0.176625"/>
+    <!-- location of bottom front bolt hole location -->
+    <parent link="torso_lift_link"/>
+    <child link="l_torso_lift_side_plate_link"/>
+  </joint>
+  <link name="l_torso_lift_side_plate_link">
+    <inertial>
+      <mass value="0.1"/>
+      <origin xyz="-0.0625 0.0 0.05"/>
+      <!-- center of the 12.5cm by 10cm bolt hole pattern -->
+      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
+    </inertial>
+  </link>
+  <joint name="r_torso_lift_side_plate_joint" type="fixed">
+    <origin xyz="0.0535  0.209285 0.176625"/>
+    <!-- location of bottom front bolt hole location -->
+    <parent link="torso_lift_link"/>
+    <child link="r_torso_lift_side_plate_link"/>
+  </joint>
+  <link name="r_torso_lift_side_plate_link">
+    <inertial>
+      <mass value="0.1"/>
+      <origin xyz="-0.0625 0.0 0.05"/>
+      <!-- center of the 12.5cm by 10cm bolt hole pattern -->
+      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
+    </inertial>
+  </link>
   <gazebo reference="torso_lift_link">
     <sensor:contact name="torso_lift_contact_sensor">
       <geom>torso_lift_link_geom</geom>
@@ -747,9 +784,8 @@
         <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">
+  <transmission name="torso_lift_trans" type="pr2_mechanism_model/SimpleTransmission">
     <actuator name="torso_lift_motor"/>
     <joint name="torso_lift_joint"/>
     <mechanicalReduction>-52143.33</mechanicalReduction>
@@ -766,19 +802,6 @@
       <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">
@@ -799,8 +822,7 @@
     <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"/>
+    <calibration 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"/>
@@ -808,28 +830,26 @@
   </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"/>
+      <mass value="6.339"/>
+      <!-- mass/cog/moi updated per 100505_link_data.xls -->
+      <origin rpy="0 0 0" xyz="0.010907 0.031693 0.090507"/>
+      <inertia ixx="0.032497592" ixy="0.00063604088" ixz="0.0025851534" iyy="0.046545627" iyz="-0.0024534295" izz="0.057652724"/>
     </inertial>
     <visual>
       <origin rpy="0 0 0 " xyz="0 0 0.0"/>
       <geometry>
-        <mesh filename="package://pr2_description/meshes/head_v0/head_pan.stl"/>
+        <mesh filename="package://pr2_description/meshes/head_v0/head_pan.dae"/>
       </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">
+  <xacro:pr2_head_pan_gazebo_v0 name="head_pan"/>
+  <transmission name="head_pan_trans" type="pr2_mechanism_model/SimpleTransmission">
     <actuator name="head_pan_motor"/>
     <joint name="head_pan_joint"/>
     <mechanicalReduction>6.0</mechanicalReduction>
@@ -839,8 +859,7 @@
     <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"/>
+    <calibration falling="0.0"/>
     <dynamics damping="1.0"/>
     <origin rpy="0 0 0" xyz="0.068 0 0"/>
     <parent link="head_pan_link"/>
@@ -848,14 +867,15 @@
   </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"/>
+      <mass value="4.479"/>
+      <!-- mass/cog/moi updated per 100505_link_data.xls -->
+      <origin rpy="0 0 0" xyz="0.001716 -0.019556 0.055002"/>
+      <inertia ixx="0.024223222" ixy="0.00062063507" ixz="-0.000096909696" iyy="0.054723086" iyz="0.00279702400" izz="0.067306377"/>
     </inertial>
     <visual>
       <origin rpy="0.0 0.0 0.0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://pr2_description/meshes/head_v0/head_tilt.stl"/>
+        <mesh filename="package://pr2_description/meshes/head_v0/head_tilt.dae"/>
       </geometry>
       <material name="Green"/>
     </visual>
@@ -866,10 +886,8 @@
       </geometry>
     </collision>
   </link>
-  <gazebo reference="head_tilt_link">
-    <material value="Gazebo/White"/>
-  </gazebo>
-  <transmission name="head_tilt_trans" type="SimpleTransmission">
+  <xacro:pr2_head_tilt_gazebo_v0 name="head_tilt"/>
+  <transmission name="head_tilt_trans" type="pr2_mechanism_model/SimpleTransmission">
     <actuator name="head_tilt_motor"/>
     <joint name="head_tilt_joint"/>
     <mechanicalReduction>6.0</mechanicalReduction>
@@ -923,12 +941,6 @@
       </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"/>
@@ -949,12 +961,6 @@
       </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"/>
@@ -975,12 +981,6 @@
       </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">
@@ -1032,12 +1032,6 @@
       </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"/>
@@ -1051,19 +1045,6 @@
       <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"/>
@@ -1083,18 +1064,6 @@
       <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"/>
@@ -1145,18 +1114,6 @@
       <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"/>
@@ -1216,19 +1173,6 @@
       <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"/>
@@ -1248,18 +1192,6 @@
       <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"/>
@@ -1310,18 +1242,6 @@
       <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"/>
@@ -1375,13 +1295,66 @@
   <gazebo reference="sensor_mount_sensor_link">
     <material value="PR2/Blue"/>
   </gazebo>
+  <!-- Projector -->
+  <joint name="projector_wg6802418_frame_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0.109 0.035"/>
+    <parent link="head_plate_frame"/>
+    <child link="projector_wg6802418_frame"/>
+  </joint>
+  <link name="projector_wg6802418_frame">
+    <inertial>
+      <mass value="0.01"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
+    </inertial>
+    <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>
+  </link>
+  <joint name="projector_wg6802418_child_frame_joint" type="fixed">
+    <origin rpy="0 -1.57079632679 0" xyz="0 0 0"/>
+    <parent link="projector_wg6802418_frame"/>
+    <child link="projector_wg6802418_child_frame"/>
+  </joint>
+  <link name="projector_wg6802418_child_frame">
+    <inertial>
+      <mass value="0.01"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 1.57079632679" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.05" radius="0.02"/>
+      </geometry>
+      <material name="Red"/>
+    </visual>
+  </link>
+  <gazebo>
+    <controller:gazebo_ros_projector name="projector_wg6802418_controller" plugin="libgazebo_ros_projector.so">
+      <alwaysOn>true</alwaysOn>
+      <updateRate>15.0</updateRate>
+      <textureName>stereo_projection_pattern_high_res_red.png</textureName>
+      <bodyName>projector_wg6802418_child_frame</bodyName>
+      <filterTextureName>stereo_projection_pattern_filter.png</filterTextureName>
+      <textureTopicName>projector_wg6802418_controller/image</textureTopicName>
+      <projectorTopicName>projector_wg6802418_controller/projector</projectorTopicName>
+      <fov>0.785398163397</fov>
+      <nearClipDist>0.4</nearClipDist>
+      <farClipDist>10</farClipDist>
+    </controller:gazebo_ros_projector>
+  </gazebo>
   <joint name="laser_tilt_mount_joint" type="revolute">
     <axis xyz="0 1 0"/>
     <limit effort="0.65" lower="-0.7854" upper="1.48353" velocity="10.0"/>
     <!-- alpha tested velocity and effort limits -->
     <safety_controller k_position="100" k_velocity="0.05" soft_lower_limit="-0.7354" soft_upper_limit="1.43353"/>
-    <!-- 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"/>
+    <calibration falling="0.0"/>
     <dynamics damping="0.008"/>
     <origin rpy="0 0 0" xyz="0.09893 0 0.227"/>
     <parent link="torso_lift_link"/>
@@ -1396,7 +1369,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://pr2_description/meshes/tilting_laser_v0/tilting_hokuyo.stl"/>
+        <mesh filename="package://pr2_description/meshes/tilting_laser_v0/tilting_hokuyo.dae"/>
       </geometry>
       <material name="Red"/>
     </visual>
@@ -1419,19 +1392,6 @@
       <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">
@@ -1442,7 +1402,7 @@
       <displayRays>false</displayRays>
       <minAngle>-79.9999999086</minAngle>
       <maxAngle>79.9999999086</maxAngle>
-      <minRange>0.05</minRange>
+      <minRange>0.08</minRange>
       <maxRange>10.0</maxRange>
       <resRange>0.01</resRange>
       <updateRate>40</updateRate>
@@ -1457,9 +1417,8 @@
     </sensor:ray>
   </gazebo>
   <gazebo reference="laser_tilt_mount_link">
-    <material value="Gazebo/Grey"/>
-  </gazebo>
-  <transmission name="laser_tilt_mount_trans" type="SimpleTransmission">
+    </gazebo>
+  <transmission name="laser_tilt_mount_trans" type="pr2_mechanism_model/SimpleTransmission">
     <actuator name="laser_tilt_mount_motor"/>
     <joint name="laser_tilt_mount_joint"/>
     <mechanicalReduction>-6.05</mechanicalReduction>
@@ -1476,8 +1435,7 @@
     <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"/>
+    <calibration rising="-0.785398163397"/>
   </joint>
   <link name="r_shoulder_pan_link">
     <inertial>
@@ -1488,7 +1446,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://pr2_description/meshes/shoulder_v0/shoulder_pan.stl"/>
+        <mesh filename="package://pr2_description/meshes/shoulder_v0/shoulder_pan.dae"/>
       </geometry>
       <material name="Blue"/>
     </visual>
@@ -1505,8 +1463,7 @@
     <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"/>
+    <calibration falling="0.0"/>
     <dynamics damping="10.0"/>
     <origin rpy="0 0 0" xyz="0.1 0 0"/>
     <parent link="r_shoulder_pan_link"/>
@@ -1521,7 +1478,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://pr2_description/meshes/shoulder_v0/shoulder_lift.stl"/>
+        <mesh filename="package://pr2_description/meshes/shoulder_v0/shoulder_lift.dae"/>
       </geometry>
       <material name="Grey"/>
     </visual>
@@ -1540,8 +1497,7 @@
     <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"/>
+    <calibration rising="-1.57079632679"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="r_upper_arm_roll_link">
@@ -1575,23 +1531,12 @@
     <stopKp value="1000000.0"/>
     <fudgeFactor value="0.5"/>
   </gazebo>
-  <transmission name="r_upper_arm_roll_trans" type="SimpleTransmission">
+  <transmission name="r_upper_arm_roll_trans" type="pr2_mechanism_model/SimpleTransmission">
     <actuator name="r_upper_arm_roll_motor"/>
     <joint name="r_upper_arm_roll_joint"/>
     <mechanicalReduction>32.6525111499</mechanicalReduction>
   </transmission>
   <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>
-        <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">
@@ -1599,31 +1544,23 @@
     <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>
-        <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"/>
+  <transmission name="r_shoulder_pan_trans" type="pr2_mechanism_model/SimpleTransmission">
     <joint name="r_shoulder_pan_joint"/>
+    <actuator name="r_shoulder_pan_motor"/>
     <mechanicalReduction>63.1552452977</mechanicalReduction>
+    <compensator k_belt="4000.0" kd_motor="15.0" lambda_combined="0.0" lambda_joint="40.0" lambda_motor="40.0" mass_motor="0.05"/>
   </transmission>
-  <transmission name="r_shoulder_lift_trans" type="SimpleTransmission">
-    <actuator name="r_shoulder_lift_motor"/>
+  <transmission name="r_shoulder_lift_trans" type="pr2_mechanism_model/SimpleTransmission">
     <joint name="r_shoulder_lift_joint"/>
+    <actuator name="r_shoulder_lift_motor"/>
     <mechanicalReduction>61.8948225713</mechanicalReduction>
+    <compensator k_belt="4000.0" kd_motor="10.0" lambda_combined="0.0" lambda_joint="60.0" lambda_motor="60.0" mass_motor="0.05"/>
   </transmission>
   <joint name="r_upper_arm_joint" type="fixed">
     <origin rpy="0 0 0" xyz="0 0 0"/>
@@ -1640,7 +1577,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://pr2_description/meshes/upper_arm_v0/upper_arm.stl"/>
+        <mesh filename="package://pr2_description/meshes/upper_arm_v0/upper_arm.dae"/>
       </geometry>
       <material name="Green"/>
     </visual>
@@ -1652,17 +1589,6 @@
     </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>
-        <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">
@@ -1670,8 +1596,7 @@
     <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"/>
+    <calibration rising="0.0"/>
     <dynamics damping="0.1"/>
     <origin rpy="0 0 0" xyz="0 0 0"/>
     <parent link="r_elbow_flex_link"/>
@@ -1706,7 +1631,7 @@
   <gazebo reference="r_forearm_roll_joint">
     <fudgeFactor value="0.5"/>
   </gazebo>
-  <transmission name="r_forearm_roll_trans" type="SimpleTransmission">
+  <transmission name="r_forearm_roll_trans" type="pr2_mechanism_model/SimpleTransmission">
     <actuator name="r_forearm_roll_motor"/>
     <joint name="r_forearm_roll_joint"/>
     <mechanicalReduction>-90.5142857143</mechanicalReduction>
@@ -1717,8 +1642,7 @@
     <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"/>
+    <calibration falling="-1.1606"/>
     <dynamics damping="1.0"/>
     <origin rpy="0 0 0" xyz="0.4 0 0"/>
     <parent link="r_upper_arm_link"/>
@@ -1733,7 +1657,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://pr2_description/meshes/upper_arm_v0/elbow_flex.stl"/>
+        <mesh filename="package://pr2_description/meshes/upper_arm_v0/elbow_flex.dae"/>
       </geometry>
       <material name="Grey"/>
     </visual>
@@ -1748,24 +1672,13 @@
     <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>
-        <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">
+  <transmission name="r_elbow_flex_trans" type="pr2_mechanism_model/SimpleTransmission">
     <actuator name="r_elbow_flex_motor"/>
     <joint name="r_elbow_flex_joint"/>
     <mechanicalReduction>-36.167452007</mechanicalReduction>
@@ -1785,7 +1698,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://pr2_description/meshes/forearm_v0/forearm.stl"/>
+        <mesh filename="package://pr2_description/meshes/forearm_v0/forearm.dae"/>
       </geometry>
       <material name="Grey"/>
     </visual>
@@ -1798,12 +1711,11 @@
   </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"/>
+    <limit effort="10" lower="-2.18" upper="0.0" velocity="3.078"/>
     <!-- alpha tested velocity and effort limits -->
-    <safety_controller k_position="20" k_velocity="4" soft_lower_limit="-1.994" soft_upper_limit="-0.1"/>
+    <safety_controller k_position="20" k_velocity="4" soft_lower_limit="-2.0" 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"/>
+    <calibration falling="-0.5410521"/>
     <origin rpy="0 0 0" xyz="0.321 0 0"/>
     <parent link="r_forearm_link"/>
     <child link="r_wrist_flex_link"/>
@@ -1817,7 +1729,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://pr2_description/meshes/forearm_v0/wrist_flex.stl"/>
+        <mesh filename="package://pr2_description/meshes/forearm_v0/wrist_flex.dae"/>
       </geometry>
       <material name="Grey"/>
     </visual>
@@ -1834,8 +1746,7 @@
     <!-- 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"/>
+    <calibration rising="-1.57079632679"/>
     <origin rpy="0 0 0" xyz="0 0 0"/>
     <parent link="r_wrist_flex_link"/>
     <child link="r_wrist_roll_link"/>
@@ -1864,30 +1775,9 @@
   </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>
-        <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>
-        <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"/>
@@ -1895,22 +1785,12 @@
   </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>
-        <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">
+  <transmission name="r_wrist_trans" type="pr2_mechanism_model/WristTransmission">
     <rightActuator mechanicalReduction="60.1714285714" name="r_wrist_r_motor"/>
     <leftActuator mechanicalReduction="60.1714285714" name="r_wrist_l_motor"/>
     <flexJoint mechanicalReduction="-1.0" name="r_wrist_flex_joint"/>
@@ -1930,7 +1810,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://pr2_description/meshes/gripper_v0/gripper_palm.stl"/>
+        <mesh filename="package://pr2_description/meshes/gripper_v0/gripper_palm.dae"/>
       </geometry>
       <material name="Red"/>
     </visual>
@@ -1996,7 +1876,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.stl"/>
+        <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
       </geometry>
       <material name="Grey"/>
     </visual>
@@ -2026,7 +1906,7 @@
     <visual>
       <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.stl"/>
+        <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
       </geometry>
       <material name="Grey"/>
     </visual>
@@ -2038,70 +1918,6 @@
       <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 xyz="${0.0914942479/2.0} 0 0" rpy="0 0 0.0541224233" />
-        <geometry>
-          <box size="${0.0914942479} 0.005 0.005" />
-        </geometry>
-        <material name="Green" />
-      </visual>
--->
-    <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 xyz="${0.0914942479/2.0} 0 0" rpy="0 0 -0.0541224233" />
-        <geometry>
-          <box size="${0.0914942479} 0.005 0.005" />
-        </geometry>
-        <material name="Green" />
-      </visual>
--->
-    <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"/>
@@ -2120,7 +1936,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.stl"/>
+        <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
       </geometry>
       <material name="Green"/>
     </visual>
@@ -2150,7 +1966,7 @@
     <visual>
       <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.stl"/>
+        <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
       </geometry>
       <material name="Green"/>
     </visual>
@@ -2170,6 +1986,7 @@
       <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>
+        <frameName>r_gripper_l_finger_link</frameName>
         <bumperTopicName>r_gripper_l_finger_bumper</bumperTopicName>
         <interface:bumper name="r_gripper_l_finger_gazebo_ros_bumper_iface"/>
       </controller:gazebo_ros_bumper>
@@ -2178,7 +1995,6 @@
     <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">
@@ -2194,6 +2010,7 @@
       <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>
+        <frameName>r_gripper_r_finger_link</frameName>
         <updateRate>100.0</updateRate>
         <bumperTopicName>r_gripper_r_finger_bumper</bumperTopicName>
         <interface:bumper name="r_gripper_r_finger_gazebo_ros_bumper_iface"/>
@@ -2203,7 +2020,6 @@
     <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"/>
@@ -2219,6 +2035,7 @@
       <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>
+        <frameName>r_gripper_l_finger_tip_link</frameName>
         <updateRate>100.0</updateRate>
         <bumperTopicName>r_gripper_l_finger_tip_bumper</bumperTopicName>
         <interface:bumper name="r_gripper_l_finger_tip_gazebo_ros_bumper_iface"/>
@@ -2228,7 +2045,6 @@
     <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"/>
@@ -2244,6 +2060,7 @@
       <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>
+        <frameName>r_gripper_r_finger_tip_link</frameName>
         <updateRate>100.0</updateRate>
         <bumperTopicName>r_gripper_r_finger_tip_bumper</bumperTopicName>
         <interface:bumper name="r_gripper_r_finger_tip_gazebo_ros_bumper_iface"/>
@@ -2253,7 +2070,6 @@
     <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">
@@ -2281,6 +2097,58 @@
     <provideFeedback value="true"/>
   </gazebo>
   <gazebo>
+    <body:box name="r_gripper_l_parallel_link">
+      <massMatrix>true</massMatrix>
+      <mass>0.17126</mass>
+      <ixx>7.7562e-05</ixx>
+      <ixy>1.49095e-06</ixy>
+      <ixz>-9.83385e-06</ixz>
+      <iyy>0.000197083</iyy>
+      <iyz>-3.06125e-06</iyz>
+      <izz>0.000181054</izz>
+      <cx>0.03598</cx>
+      <cy>0.0173</cy>
+      <cz>-0.00164</cz>
+      <xyz>0.82991 -0.157 0.790675</xyz>
+      <rpy>0 -0 0</rpy>
+      <turnGravityOff>true</turnGravityOff>
+      <selfCollide>false</selfCollide>
+    </body:box>
+    <body:box name="r_gripper_r_parallel_link">
+      <massMatrix>true</massMatrix>
+      <mass>0.17389</mass>
+      <ixx>7.73841e-05</ixx>
+      <ixy>-2.09309e-06</ixy>
+      <ixz>-8.36228e-06</ixz>
+      <iyy>0.000198474</iyy>
+      <iyz>2.4611e-06</iyz>
+      <izz>0.00018107</izz>
+      <cx>0.03576</cx>
+      <cy>-0.01736</cy>
+      <cz>-0.00095</cz>
+      <xyz>0.82991 -0.219 0.790675</xyz>
+      <rpy>0 -0 0</rpy>
+      <turnGravityOff>true</turnGravityOff>
+      <selfCollide>false</selfCollide>
+    </body:box>
+  </gazebo>
+  <gazebo>
+    <joint:hinge name="r_gripper_r_parallel_root_joint">
+      <body1>r_gripper_r_parallel_link</body1>
+      <body2>r_gripper_palm_link</body2>
+      <anchor>r_gripper_palm_link</anchor>
+      <axis>0 0 -1</axis>
+      <damping>0.2</damping>
+      <anchorOffset>0.05891 -0.031 0</anchorOffset>
+    </joint:hinge>
+    <joint:hinge name="r_gripper_l_parallel_root_joint">
+      <body1>r_gripper_l_parallel_link</body1>
+      <body2>r_gripper_palm_link</body2>
+      <anchor>r_gripper_palm_link</anchor>
+      <axis>0 0 1</axis>
+      <damping>0.2</damping>
+      <anchorOffset>0.05891 0.031 0</anchorOffset>
+    </joint:hinge>
     <joint:hinge name="r_gripper_r_parallel_tip_joint">
       <body1>r_gripper_r_parallel_link</body1>
       <body2>r_gripper_r_finger_tip_link</body2>
@@ -2311,11 +2179,14 @@
     <material value="PR2/Red"/>
   </gazebo>
   <joint name="r_gripper_joint" type="prismatic">
+    <parent link="r_gripper_r_finger_tip_link"/>
+    <child link="r_gripper_l_finger_tip_frame"/>
     <axis xyz="0 1 0"/>
     <dynamics damping="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>
+  <link name="r_gripper_l_finger_tip_frame"/>
   <gazebo reference="r_gripper_palm_link">
     <turnGravityOff>true</turnGravityOff>
     <sensor:contact name="r_gripper_palm_contact_sensor">
@@ -2324,11 +2195,11 @@
       <controller:gazebo_ros_bumper name="r_gripper_palm_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
         <alwaysOn>true</alwaysOn>
         <updateRate>100.0</updateRate>
+        <frameName>r_gripper_palm_link</frameName>
         <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">
@@ -2343,7 +2214,7 @@
       <interface:position name="p3d_r_gripper_palm_position_iface"/>
     </controller:gazebo_ros_p3d>
   </gazebo>
-  <transmission name="r_gripper_trans" type="PR2GripperTransmission">
+  <transmission name="r_gripper_trans" type="pr2_mechanism_model/PR2GripperTransmission">
     <actuator name="r_gripper_motor"/>
     <gap_joint L0="0.0375528" a="0.0683698" b="0.0433849" gear_ratio="40.095" h="0.0" mechanical_reduction="1.0" name="r_gripper_joint" phi0="0.518518122146" r="0.0915" screw_reduction="0.004" t0="-0.0001914" theta0="0.0628824676201"/>
     <!-- if a gazebo joint exists as [l|r]_gripper_joint, use this tag to have
@@ -2353,8 +2224,6 @@
     <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"/>
@@ -2367,8 +2236,7 @@
     <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"/>
+    <calibration rising="0.785398163397"/>
   </joint>
   <link name="l_shoulder_pan_link">
     <inertial>
@@ -2379,7 +2247,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://pr2_description/meshes/shoulder_v0/shoulder_pan.stl"/>
+        <mesh filename="package://pr2_description/meshes/shoulder_v0/shoulder_pan.dae"/>
       </geometry>
       <material name="Blue"/>
     </visual>
@@ -2396,8 +2264,7 @@
     <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"/>
+    <calibration falling="0.0"/>
     <dynamics damping="10.0"/>
     <origin rpy="0 0 0" xyz="0.1 0 0"/>
     <parent link="l_shoulder_pan_link"/>
@@ -2412,7 +2279,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://pr2_description/meshes/shoulder_v0/shoulder_lift.stl"/>
+        <mesh filename="package://pr2_description/meshes/shoulder_v0/shoulder_lift.dae"/>
       </geometry>
       <material name="Grey"/>
     </visual>
@@ -2431,8 +2298,7 @@
     <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"/>
+    <calibration rising="1.57079632679"/>
     <dynamics damping="0.1"/>
   </joint>
   <link name="l_upper_arm_roll_link">
@@ -2466,23 +2332,12 @@
     <stopKp value="1000000.0"/>
     <fudgeFactor value="0.5"/>
   </gazebo>
-  <transmission name="l_upper_arm_roll_trans" type="SimpleTransmission">
+  <transmission name="l_upper_arm_roll_trans" type="pr2_mechanism_model/SimpleTransmission">
     <actuator name="l_upper_arm_roll_motor"/>
     <joint name="l_upper_arm_roll_joint"/>
     <mechanicalReduction>32.6525111499</mechanicalReduction>
   </transmission>
   <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>
-        <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">
@@ -2490,31 +2345,23 @@
     <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>
-        <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"/>
+  <transmission name="l_shoulder_pan_trans" type="pr2_mechanism_model/SimpleTransmission">
     <joint name="l_shoulder_pan_joint"/>
+    <actuator name="l_shoulder_pan_motor"/>
     <mechanicalReduction>63.1552452977</mechanicalReduction>
+    <compensator k_belt="4000.0" kd_motor="15.0" lambda_combined="0.0" lambda_joint="40.0" lambda_motor="40.0" mass_motor="0.05"/>
   </transmission>
-  <transmission name="l_shoulder_lift_trans" type="SimpleTransmission">
-    <actuator name="l_shoulder_lift_motor"/>
+  <transmission name="l_shoulder_lift_trans" type="pr2_mechanism_model/SimpleTransmission">
     <joint name="l_shoulder_lift_joint"/>
+    <actuator name="l_shoulder_lift_motor"/>
     <mechanicalReduction>61.8948225713</mechanicalReduction>
+    <compensator k_belt="4000.0" kd_motor="10.0" lambda_combined="0.0" lambda_joint="60.0" lambda_motor="60.0" mass_motor="0.05"/>
   </transmission>
   <joint name="l_upper_arm_joint" type="fixed">
     <origin rpy="0 0 0" xyz="0 0 0"/>
@@ -2531,7 +2378,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://pr2_description/meshes/upper_arm_v0/upper_arm.stl"/>
+        <mesh filename="package://pr2_description/meshes/upper_arm_v0/upper_arm.dae"/>
       </geometry>
       <material name="Green"/>
     </visual>
@@ -2543,17 +2390,6 @@
     </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>
-        <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">
@@ -2561,8 +2397,7 @@
     <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"/>
+    <calibration rising="0.0"/>
     <dynamics damping="0.1"/>
     <origin rpy="0 0 0" xyz="0 0 0"/>
     <parent link="l_elbow_flex_link"/>
@@ -2597,7 +2432,7 @@
   <gazebo reference="l_forearm_roll_joint">
     <fudgeFactor value="0.5"/>
   </gazebo>
-  <transmission name="l_forearm_roll_trans" type="SimpleTransmission">
+  <transmission name="l_forearm_roll_trans" type="pr2_mechanism_model/SimpleTransmission">
     <actuator name="l_forearm_roll_motor"/>
     <joint name="l_forearm_roll_joint"/>
     <mechanicalReduction>-90.5142857143</mechanicalReduction>
@@ -2608,8 +2443,7 @@
     <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"/>
+    <calibration falling="-1.1606"/>
     <dynamics damping="1.0"/>
     <origin rpy="0 0 0" xyz="0.4 0 0"/>
     <parent link="l_upper_arm_link"/>
@@ -2624,7 +2458,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://pr2_description/meshes/upper_arm_v0/elbow_flex.stl"/>
+        <mesh filename="package://pr2_description/meshes/upper_arm_v0/elbow_flex.dae"/>
       </geometry>
       <material name="Grey"/>
     </visual>
@@ -2639,24 +2473,13 @@
     <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>
-        <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">
+  <transmission name="l_elbow_flex_trans" type="pr2_mechanism_model/SimpleTransmission">
     <actuator name="l_elbow_flex_motor"/>
     <joint name="l_elbow_flex_joint"/>
     <mechanicalReduction>-36.167452007</mechanicalReduction>
@@ -2676,7 +2499,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://pr2_description/meshes/forearm_v0/forearm.stl"/>
+        <mesh filename="package://pr2_description/meshes/forearm_v0/forearm.dae"/>
       </geometry>
       <material name="Grey"/>
     </visual>
@@ -2689,12 +2512,11 @@
   </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"/>
+    <limit effort="10" lower="-2.18" upper="0.0" velocity="3.078"/>
     <!-- alpha tested velocity and effort limits -->
-    <safety_controller k_position="20" k_velocity="4" soft_lower_limit="-1.994" soft_upper_limit="-0.1"/>
+    <safety_controller k_position="20" k_velocity="4" soft_lower_limit="-2.0" 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"/>
+    <calibration falling="-0.5410521"/>
     <origin rpy="0 0 0" xyz="0.321 0 0"/>
     <parent link="l_forearm_link"/>
     <child link="l_wrist_flex_link"/>
@@ -2708,7 +2530,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://pr2_description/meshes/forearm_v0/wrist_flex.stl"/>
+        <mesh filename="package://pr2_description/meshes/forearm_v0/wrist_flex.dae"/>
       </geometry>
       <material name="Grey"/>
     </visual>
@@ -2725,8 +2547,7 @@
     <!-- 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"/>
+    <calibration rising="-1.57079632679"/>
     <origin rpy="0 0 0" xyz="0 0 0"/>
     <parent link="l_wrist_flex_link"/>
     <child link="l_wrist_roll_link"/>
@@ -2755,30 +2576,9 @@
   </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>
-        <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>
-        <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"/>
@@ -2786,22 +2586,12 @@
   </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>
-        <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">
+  <transmission name="l_wrist_trans" type="pr2_mechanism_model/WristTransmission">
     <rightActuator mechanicalReduction="60.1714285714" name="l_wrist_r_motor"/>
     <leftActuator mechanicalReduction="60.1714285714" name="l_wrist_l_motor"/>
     <flexJoint mechanicalReduction="-1.0" name="l_wrist_flex_joint"/>
@@ -2821,7 +2611,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://pr2_description/meshes/gripper_v0/gripper_palm.stl"/>
+        <mesh filename="package://pr2_description/meshes/gripper_v0/gripper_palm.dae"/>
       </geometry>
       <material name="Red"/>
     </visual>
@@ -2887,7 +2677,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.stl"/>
+        <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
       </geometry>
       <material name="Grey"/>
     </visual>
@@ -2917,7 +2707,7 @@
     <visual>
       <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.stl"/>
+        <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
       </geometry>
       <material name="Grey"/>
     </visual>
@@ -2929,70 +2719,6 @@
       <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 xyz="${0.0914942479/2.0} 0 0" rpy="0 0 0.0541224233" />
-        <geometry>
-          <box size="${0.0914942479} 0.005 0.005" />
-        </geometry>
-        <material name="Green" />
-      </visual>
--->
-    <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 xyz="${0.0914942479/2.0} 0 0" rpy="0 0 -0.0541224233" />
-        <geometry>
-          <box size="${0.0914942479} 0.005 0.005" />
-        </geometry>
-        <material name="Green" />
-      </visual>
--->
-    <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"/>
@@ -3011,7 +2737,7 @@
     <visual>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.stl"/>
+        <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
       </geometry>
       <material name="Green"/>
     </visual>
@@ -3041,7 +2767,7 @@
     <visual>
       <origin rpy="3.14159265359 0 0" xyz="0 0 0"/>
       <geometry>
-        <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.stl"/>
+        <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"/>
       </geometry>
       <material name="Green"/>
     </visual>
@@ -3061,6 +2787,7 @@
       <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>
+        <frameName>l_gripper_l_finger_link</frameName>
         <bumperTopicName>l_gripper_l_finger_bumper</bumperTopicName>
         <interface:bumper name="l_gripper_l_finger_gazebo_ros_bumper_iface"/>
       </controller:gazebo_ros_bumper>
@@ -3069,7 +2796,6 @@
     <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">
@@ -3085,6 +2811,7 @@
       <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>
+        <frameName>l_gripper_r_finger_link</frameName>
         <updateRate>100.0</updateRate>
         <bumperTopicName>l_gripper_r_finger_bumper</bumperTopicName>
         <interface:bumper name="l_gripper_r_finger_gazebo_ros_bumper_iface"/>
@@ -3094,7 +2821,6 @@
     <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"/>
@@ -3110,6 +2836,7 @@
       <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>
+        <frameName>l_gripper_l_finger_tip_link</frameName>
         <updateRate>100.0</updateRate>
         <bumperTopicName>l_gripper_l_finger_tip_bumper</bumperTopicName>
         <interface:bumper name="l_gripper_l_finger_tip_gazebo_ros_bumper_iface"/>
@@ -3119,7 +2846,6 @@
     <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"/>
@@ -3135,6 +2861,7 @@
       <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>
+        <frameName>l_gripper_r_finger_tip_link</frameName>
         <updateRate>100.0</updateRate>
         <bumperTopicName>l_gripper_r_finger_tip_bumper</bumperTopicName>
         <interface:bumper name="l_gripper_r_finger_tip_gazebo_ros_bumper_iface"/>
@@ -3144,7 +2871,6 @@
     <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">
@@ -3172,6 +2898,58 @@
     <provideFeedback value="true"/>
   </gazebo>
   <gazebo>
+    <body:box name="l_gripper_l_parallel_link">
+      <massMatrix>true</massMatrix>
+      <mass>0.17126</mass>
+      <ixx>7.7562e-05</ixx>
+      <ixy>1.49095e-06</ixy>
+      <ixz>-9.83385e-06</ixz>
+      <iyy>0.000197083</iyy>
+      <iyz>-3.06125e-06</iyz>
+      <izz>0.000181054</izz>
+      <cx>0.03598</cx>
+      <cy>0.0173</cy>
+      <cz>-0.00164</cz>
+      <xyz>0.82991 0.219 0.790675</xyz>
+      <rpy>0 -0 0</rpy>
+      <turnGravityOff>true</turnGravityOff>
+      <selfCollide>false</selfCollide>
+    </body:box>
+    <body:box name="l_gripper_r_parallel_link">
+      <massMatrix>true</massMatrix>
+      <mass>0.17389</mass>
+      <ixx>7.73841e-05</ixx>
+      <ixy>-2.09309e-06</ixy>
+      <ixz>-8.36228e-06</ixz>
+      <iyy>0.000198474</iyy>
+      <iyz>2.4611e-06</iyz>
+      <izz>0.00018107</izz>
+      <cx>0.03576</cx>
+      <cy>-0.01736</cy>
+      <cz>-0.00095</cz>
+      <xyz>0.82991 0.157 0.790675</xyz>
+      <rpy>0 -0 0</rpy>
+      <turnGravityOff>true</turnGravityOff>
+      <selfCollide>false</selfCollide>
+    </body:box>
+  </gazebo>
+  <gazebo>
+    <joint:hinge name="l_gripper_r_parallel_root_joint">
+      <body1>l_gripper_r_parallel_link</body1>
+      <body2>l_gripper_palm_link</body2>
+      <anchor>l_gripper_palm_link</anchor>
+      <axis>0 0 -1</axis>
+      <damping>0.2</damping>
+      <anchorOffset>0.05891 -0.031 0</anchorOffset>
+    </joint:hinge>
+    <joint:hinge name="l_gripper_l_parallel_root_joint">
+      <body1>l_gripper_l_parallel_link</body1>
+      <body2>l_gripper_palm_link</body2>
+      <anchor>l_gripper_palm_link</anchor>
+      <axis>0 0 1</axis>
+      <damping>0.2</damping>
+      <anchorOffset>0.05891 0.031 0</anchorOffset>
+    </joint:hinge>
     <joint:hinge name="l_gripper_r_parallel_tip_joint">
       <body1>l_gripper_r_parallel_link</body1>
       <body2>l_gripper_r_finger_tip_link</body2>
@@ -3202,11 +2980,14 @@
     <material value="PR2/Red"/>
   </gazebo>
   <joint name="l_gripper_joint" type="prismatic">
+    <parent link="l_gripper_r_finger_tip_link"/>
+    <child link="l_gripper_l_finger_tip_frame"/>
     <axis xyz="0 1 0"/>
     <dynamics damping="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>
+  <link name="l_gripper_l_finger_tip_frame"/>
   <gazebo reference="l_gripper_palm_link">
     <turnGravityOff>true</turnGravityOff>
     <sensor:contact name="l_gripper_palm_contact_sensor">
@@ -3215,11 +2996,11 @@
       <controller:gazebo_ros_bumper name="l_gripper_palm_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
         <alwaysOn>true</alwaysOn>
         <updateRate>100.0</updateRate>
+        <frameName>l_gripper_palm_link</frameName>
         <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">
@@ -3234,7 +3015,7 @@
       <interface:position name="p3d_l_gripper_palm_position_iface"/>
     </controller:gazebo_ros_p3d>
   </gazebo>
-  <transmission name="l_gripper_trans" type="PR2GripperTransmission">
+  <transmission name="l_gripper_trans" type="pr2_mechanism_model/PR2GripperTransmission">
     <actuator name="l_gripper_motor"/>
     <gap_joint L0="0.0375528" a="0.0683698" b="0.0433849" gear_ratio="40.095" h="0.0" mechanical_reduction="1.0" name="l_gripper_joint" phi0="0.518518122146" r="0.0915" screw_reduction="0.004" t0="-0.0001914" theta0="0.0628824676201"/>
     <!-- if a gazebo joint exists as [l|r]_gripper_joint, use this tag to have
@@ -3244,8 +3025,6 @@
     <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 -->
@@ -3260,18 +3039,6 @@
       <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"/>
@@ -3322,18 +3089,6 @@
       <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"/>
@@ -3374,3 +3129,4 @@
     <material>PR2/Blue</material>
   </gazebo>
 </robot>
+
diff --git a/robot_state_publisher/test/test_publisher.launch b/robot_state_publisher/test/test_publisher.launch
index 29aa39d..2c954f3 100644
--- a/robot_state_publisher/test/test_publisher.launch
+++ b/robot_state_publisher/test/test_publisher.launch
@@ -1,5 +1,6 @@
 <launch>
-  <node pkg="rosrecord" name="rosplay" type="rosplay" args="$(find robot_state_publisher)/test/joint_states_indexed.bag" />
+  <param name="use_sim_time" value="true" />
+  <node pkg="rosbag" name="rosbag" type="play" args="--clock $(find robot_state_publisher)/test/joint_states_indexed.bag" />
   <node pkg="robot_state_publisher" name="robot_state_publisher" type="state_publisher" />
   <param name="robot_description" textfile="$(find robot_state_publisher)/test/pr2.urdf" />