diff --git a/urdf/src/model.cpp b/urdf/src/model.cpp
index dfade4c..12ef8bc 100644
--- a/urdf/src/model.cpp
+++ b/urdf/src/model.cpp
@@ -277,20 +277,11 @@ bool Model::initTree(std::map<std::string, std::string> &parent_link_tree)
     std::string child_link_name = joint->second->child_link_name;
 
     ROS_DEBUG("build tree: joint: '%s' has parent link '%s' and child  link '%s'", joint->first.c_str(), parent_link_name.c_str(),child_link_name.c_str());
-    if (parent_link_name.empty() && !child_link_name.empty())
+    if (parent_link_name.empty() || child_link_name.empty())
     {
-      ROS_ERROR("    Joint %s specifies child link but not parent link.",(joint->second)->name.c_str());
+      ROS_ERROR("    Joint %s is missing a parent and/or child link specification.",(joint->second)->name.c_str());
       return false;
     }
-    else if (!parent_link_name.empty() && child_link_name.empty())
-    {
-      ROS_ERROR("    Joint %s specifies parent link but not child link.",(joint->second)->name.c_str());
-      return false;
-    }
-    else if (parent_link_name.empty() && child_link_name.empty())
-    {
-      ROS_WARN("    Joint %s specifies no parent link and no child link. The Boxturtle urdf parser had a bug that allowed this type of joints, but this is not a valid joint according to the URDF spec.",(joint->second)->name.c_str());
-    }
     else
     {
       // find child and parent links
diff --git a/urdf/test/pr2_desc.urdf b/urdf/test/pr2_desc.urdf
index ebef6c6..cbc5c77 100644
--- a/urdf/test/pr2_desc.urdf
+++ b/urdf/test/pr2_desc.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/stacks/pr2_common/pr2_description/robots/pr2.urdf.xacro | -->
 <!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
 <!-- =================================================================================== -->
 <robot name="pr2" xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#slider" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:xacro="http://ros.org/wiki/xacro">
@@ -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,6 +55,9 @@
   <material name="White">
     <color rgba="1.0 1.0 1.0 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"/>
   </material>
@@ -83,7 +87,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>
@@ -156,7 +160,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 +179,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 +205,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 +229,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 +246,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 +271,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 +288,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 +302,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 +328,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 +352,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 +369,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 +394,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 +411,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 +425,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 +451,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 +475,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 +492,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 +517,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 +534,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 +548,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 +574,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 +598,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 +615,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 +640,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 +657,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>
@@ -686,7 +678,6 @@
         <interface:bumper name="base_bumper_iface"/>
       </controller:gazebo_ros_bumper>
     </sensor:contact>
-    <material value="PR2/caster_texture"/>
   </gazebo>
   <gazebo>
     <controller:gazebo_ros_p3d name="p3d_base_controller" plugin="libgazebo_ros_p3d.so">
@@ -708,8 +699,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 +715,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>
@@ -747,9 +737,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>
@@ -799,8 +788,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"/>
@@ -815,21 +803,18 @@
     <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 +824,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"/>
@@ -855,7 +839,7 @@
     <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 +850,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>
@@ -1375,13 +1357,78 @@
   <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>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+    </collision>
+  </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>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+    </collision>
+  </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 +1443,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>
@@ -1442,7 +1489,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 +1504,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 +1522,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 +1533,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 +1550,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 +1565,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 +1584,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 +1618,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,28 +1631,18 @@
     <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">
+  <transmission name="r_shoulder_pan_trans" type="pr2_mechanism_model/SimpleTransmission">
     <actuator name="r_shoulder_pan_motor"/>
     <joint name="r_shoulder_pan_joint"/>
     <mechanicalReduction>63.1552452977</mechanicalReduction>
   </transmission>
-  <transmission name="r_shoulder_lift_trans" type="SimpleTransmission">
+  <transmission name="r_shoulder_lift_trans" type="pr2_mechanism_model/SimpleTransmission">
     <actuator name="r_shoulder_lift_motor"/>
     <joint name="r_shoulder_lift_joint"/>
     <mechanicalReduction>61.8948225713</mechanicalReduction>
@@ -1640,7 +1662,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 +1674,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 +1681,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 +1716,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 +1727,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 +1742,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 +1757,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 +1783,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>
@@ -1802,8 +1800,7 @@
     <!-- alpha tested velocity and effort limits -->
     <safety_controller k_position="20" k_velocity="4" soft_lower_limit="-1.994" soft_upper_limit="-0.1"/>
     <dynamics damping="0.1"/>
-    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
-    <calibration falling="-0.5410521" reference_position="-0.5410521"/>
+    <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 +1814,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 +1831,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 +1860,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 +1870,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 +1895,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 +1961,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 +1991,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 +2003,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 +2021,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 +2051,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 +2071,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 +2080,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 +2095,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 +2105,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 +2120,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 +2130,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 +2145,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 +2155,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 +2182,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>
@@ -2327,11 +2280,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">
@@ -2346,7 +2299,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
@@ -2356,8 +2309,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"/>
@@ -2370,8 +2321,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>
@@ -2382,7 +2332,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>
@@ -2399,8 +2349,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"/>
@@ -2415,7 +2364,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>
@@ -2434,8 +2383,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">
@@ -2469,23 +2417,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">
@@ -2493,28 +2430,18 @@
     <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">
+  <transmission name="l_shoulder_pan_trans" type="pr2_mechanism_model/SimpleTransmission">
     <actuator name="l_shoulder_pan_motor"/>
     <joint name="l_shoulder_pan_joint"/>
     <mechanicalReduction>63.1552452977</mechanicalReduction>
   </transmission>
-  <transmission name="l_shoulder_lift_trans" type="SimpleTransmission">
+  <transmission name="l_shoulder_lift_trans" type="pr2_mechanism_model/SimpleTransmission">
     <actuator name="l_shoulder_lift_motor"/>
     <joint name="l_shoulder_lift_joint"/>
     <mechanicalReduction>61.8948225713</mechanicalReduction>
@@ -2534,7 +2461,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>
@@ -2546,17 +2473,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">
@@ -2564,8 +2480,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"/>
@@ -2600,7 +2515,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>
@@ -2611,8 +2526,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"/>
@@ -2627,7 +2541,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>
@@ -2642,24 +2556,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>
@@ -2679,7 +2582,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>
@@ -2696,8 +2599,7 @@
     <!-- alpha tested velocity and effort limits -->
     <safety_controller k_position="20" k_velocity="4" soft_lower_limit="-1.994" soft_upper_limit="-0.1"/>
     <dynamics damping="0.1"/>
-    <!-- hold both reference_position and upper/lower for tick-tock, remove reference_positiion after verifying upper/lower works with calibration controllers.  if search velocity in pr2_calibration_controllers.yaml is +, the reference_position is rising, if - then it is falling -->
-    <calibration falling="-0.5410521" reference_position="-0.5410521"/>
+    <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"/>
@@ -2711,7 +2613,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>
@@ -2728,8 +2630,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"/>
@@ -2758,30 +2659,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"/>
@@ -2789,22 +2669,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"/>
@@ -2824,7 +2694,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>
@@ -2890,7 +2760,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>
@@ -2920,7 +2790,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>
@@ -2932,70 +2802,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"/>
@@ -3014,7 +2820,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>
@@ -3044,7 +2850,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>
@@ -3064,6 +2870,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>
@@ -3072,7 +2879,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">
@@ -3088,6 +2894,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"/>
@@ -3097,7 +2904,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"/>
@@ -3113,6 +2919,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"/>
@@ -3122,7 +2929,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"/>
@@ -3138,6 +2944,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"/>
@@ -3147,7 +2954,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">
@@ -3175,6 +2981,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>
@@ -3221,11 +3079,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">
@@ -3240,7 +3098,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
@@ -3250,8 +3108,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 -->
diff --git a/urdf/test/pr2_desc_explicit_world.urdf b/urdf/test/pr2_desc_explicit_world.urdf
deleted file mode 100644
index ff3db28..0000000
--- a/urdf/test/pr2_desc_explicit_world.urdf
+++ /dev/null
@@ -1,3448 +0,0 @@
-<?xml version="1.0" ?><robot name="pr2" xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#slider" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:xacro="http://ros.org/wiki/xacro">
-  
-  <!-- The following included files set up definitions of parts of the robot body -->
-  <!-- misc common stuff? -->
-  
-  <!-- PR2 Arm -->
-  
-  
-  
-  <!-- PR2 gripper -->
-  
-  <!-- PR2 head -->
-  
-  <!-- PR2 tilting laser mount -->
-  
-  <!-- PR2 torso -->
-  
-  <!-- PR2 base -->
-  
-  <!-- Head sensors -->
-  
-  <!-- Camera sensors -->
-  
-  <!-- generic simulator_gazebo plugins for starting mechanism control, ros time, ros battery -->
-  <gazebo>
-    <controller:gazebo_ros_controller_manager name="gazebo_ros_controller_manager" plugin="libgazebo_ros_controller_manager.so">
-      <alwaysOn>true</alwaysOn>
-      <updateRate>1000.0</updateRate>
-      <interface:audio name="gazebo_ros_controller_manager_dummy_iface"/>
-    </controller:gazebo_ros_controller_manager>
-
-    <controller:gazebo_ros_battery name="gazebo_ros_battery_controller" plugin="libgazebo_ros_battery.so">
-      <alwaysOn>true</alwaysOn>
-      <updateRate>1.0</updateRate>
-      <timeout>5</timeout>
-      <interface:audio name="battery_dummy_interface"/>
-      <powerStateTopic>power_state</powerStateTopic>
-      <powerStateRate>10.0</powerStateRate>
-      <fullChargeCapacity>87.78</fullChargeCapacity>     
-      <dischargeRate>-474</dischargeRate>
-      <chargeRate>525</chargeRate>
-      <dischargeVoltage>15.52</dischargeVoltage>
-      <chargeVoltage>16.41</chargeVoltage>
-    </controller:gazebo_ros_battery>
-  </gazebo>
-  <!-- materials for visualization -->
-  <material name="Blue">
-    <color rgba="0.0 0.0 0.8 1.0"/>
-  </material><material name="Green">
-    <color rgba="0.0 0.8 0.0 1.0"/>
-  </material><material name="Grey">
-    <color rgba="0.7 0.7 0.7 1.0"/>
-  </material><material name="Grey2">
-    <color rgba="0.9 0.9 0.9 1.0"/>
-  </material><material name="Red">
-    <color rgba="0.8 0.0 0.0 1.0"/>
-  </material><material name="White">
-    <color rgba="1.0 1.0 1.0 1.0"/>
-  </material><material name="Caster">
-    <texture filename="package://pr2_ogre/Media/materials/textures/pr2_caster_texture.png"/>
-  </material><material name="Wheel_l">
-    <texture filename="package://pr2_ogre/Media/materials/textures/pr2_wheel_left.png"/>
-  </material><material name="Wheel_r">
-    <texture filename="package://pr2_ogre/Media/materials/textures/pr2_wheel_right.png"/>
-  </material><material name="RollLinks">
-    <texture filename="package://pr2_ogre/Media/materials/textures/pr2_wheel_left.png"/>
-  </material>
-
-  <!-- Now we can start using the macros included above to define the actual PR2 -->
-
-  <!-- The first use of a macro.  This one was defined in base.urdf.xacro above.
-       A macro like this will expand to a set of link and joint definitions, and to additional
-       Gazebo-related extensions (sensor plugins, etc).  The macro takes an argument, name, 
-       that equals "base", and uses it to generate names for its component links and joints 
-       (e.g., base_link).  The included origin block is also an argument to the macro.  By convention, 
-       the origin block defines where the component is w.r.t its parent (in this case the parent 
-       is the world frame). For more, see http://www.ros.org/wiki/xacro -->
-  <joint name="base_joint" type="planar">
-      <origin rpy="0 0 0" xyz="0 0 0.051"/>
-      <parent link="world"/>
-      <child link="base_link"/>
-    </joint>    <link name="world"/>
-      <link name="base_link">
-      <inertial>
-        <mass value="116.0"/>
-        <origin xyz="-0.061 0.0 0.293"/>
-        <inertia ixx="5.652232699207" ixy="-0.009719934438" ixz="1.293988226423" iyy="5.669473158652" iyz="-0.007379583694" izz="3.683196351726"/>
-      </inertial>
-
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://pr2_description/meshes/base_v0/base.stl"/>
-        </geometry>
-        
-        <material name="Green"/>
-      </visual>
-
-      <collision>
-        <!-- represent base collision with a simple rectangular model, positioned by base_size_z s.t. top
-             surface of the collision box matches the top surface of the PR2 base -->
-        <origin rpy="0 0 0" xyz="0 0 0.21"/>
-        <geometry>
-          <box size="0.65 0.65 0.04"/>
-        </geometry>
-      </collision>
-    </link><joint name="base_laser_joint" type="fixed">
-      <axis xyz="0 1 0"/>
-      <origin rpy="0 0 0" xyz="0.275 0.0 0.252"/>
-      <parent link="base_link"/>
-      <child link="base_laser_link"/>
-    </joint><link name="base_laser_link" type="laser">
-      <inertial>
-        <mass value="0.001"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.0001"/>
-      </inertial>
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.001 0.001 0.001"/>
-        </geometry>
-        <material name="Red"/>
-      </visual>
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size=".001 .001 .001"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="base_laser_link">
-      <sensor:ray name="base_laser">
-        <rayCount>640</rayCount>
-        <rangeCount>640</rangeCount>
-        <laserCount>1</laserCount>
-
-        <origin>0.0 0.0 0.0</origin>
-        <displayRays>false</displayRays>
-
-        <minAngle>-80</minAngle>
-        <maxAngle> 80</maxAngle>
-
-        <minRange>0.05</minRange>
-        <maxRange>10.0</maxRange>
-        <resRange>0.01</resRange>
-        <updateRate>20.0</updateRate>
-        <controller:gazebo_ros_laser name="gazebo_ros_base_laser_controller" plugin="libgazebo_ros_laser.so">
-          <gaussianNoise>0.005</gaussianNoise>
-          <alwaysOn>true</alwaysOn>
-          <updateRate>20.0</updateRate>
-          <topicName>base_scan</topicName>
-          <frameName>base_laser_link</frameName>
-          <interface:laser name="gazebo_ros_base_laser_iface"/>
-        </controller:gazebo_ros_laser>
-      </sensor:ray>
-    </gazebo><joint name="fl_caster_rotation_joint" type="continuous">
-      <axis xyz="0 0 1"/>
-      <limit effort="100" velocity="100"/>
-      <safety_controller k_velocity="10"/>
-      <calibration reference_position="0.0"/>
-      <dynamics damping="0.0" friction="0.0"/>
-      <origin rpy="0 0 0" xyz="0.2225 0.2225 0.0282"/>
-      <parent link="base_link"/>
-      <child link="fl_caster_rotation_link"/>
-    </joint><link name="fl_caster_rotation_link">
-
-      <inertial>
-        <mass value="3.473082"/>
-        <origin xyz="0 0 0.07"/>
-        <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
-      </inertial>
-
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://pr2_description/meshes/base_v0/caster.stl"/>
-        </geometry>
-        
-        <material name="Green"/>
-      </visual>
-
-      <collision>
-        <origin rpy="0.0 0.0 0.0" xyz="0 0 0.07"/>
-        <geometry>
-          <box size="0.192 0.164 0.013"/>
-        </geometry>
-      </collision>
-    </link><transmission name="fl_caster_rotation_trans" type="SimpleTransmission">
-      <actuator name="fl_caster_rotation_motor"/>
-      <joint name="fl_caster_rotation_joint"/>
-      <mechanicalReduction>-79.2380952381</mechanicalReduction>
-    </transmission><joint name="fl_caster_l_wheel_joint" type="continuous">
-      <axis xyz="0 1 0"/>
-      <limit effort="100" velocity="100"/>
-      <safety_controller k_velocity="10"/>
-      <dynamics damping="0.0" friction="0.0"/>
-      <origin rpy="0 0 0" xyz="0 0.049 0"/>
-      <parent link="fl_caster_rotation_link"/>
-      <child link="fl_caster_l_wheel_link"/>
-    </joint><link name="fl_caster_l_wheel_link">
-      <inertial>
-        <mass value="0.44036"/>
-        <origin xyz="0 0 0"/>
-        <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
-      </inertial>
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://pr2_description/meshes/base_v0/pr2_wheel.stl"/>
-        </geometry>
-        
-        <material name="Wheel_l"/>
-      </visual>
-      <collision>
-        <origin rpy="1.57079632679 0 0" xyz="0 0 0"/> <!-- rotation because cyl. geom primitive has symmetry axis in +x direction -->
-        <geometry>
-          <cylinder length="0.034" radius="0.074792"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="fl_caster_l_wheel_link">
-      <mu1 value="100.0"/>
-      <mu2 value="100.0"/>
-      <kp value="1000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/wheel_l"/>
-    </gazebo><transmission name="fl_caster_l_wheel_trans" type="SimpleTransmission">
-      <actuator name="fl_caster_l_wheel_motor"/>
-      <joint name="fl_caster_l_wheel_joint"/>
-      <mechanicalReduction>79.2380952381</mechanicalReduction>
-    </transmission><joint name="fl_caster_r_wheel_joint" type="continuous">
-      <axis xyz="0 1 0"/>
-      <limit effort="100" velocity="100"/>
-      <safety_controller k_velocity="10"/>
-      <dynamics damping="0.0" friction="0.0"/>
-      <origin rpy="0 0 0" xyz="0 -0.049 0"/>
-      <parent link="fl_caster_rotation_link"/>
-      <child link="fl_caster_r_wheel_link"/>
-    </joint><link name="fl_caster_r_wheel_link">
-      <inertial>
-        <mass value="0.44036"/>
-        <origin xyz="0 0 0"/>
-        <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
-      </inertial>
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://pr2_description/meshes/base_v0/pr2_wheel.stl"/>
-        </geometry>
-        
-        <material name="Wheel_r"/>
-      </visual>
-      <collision>
-        <origin rpy="1.57079632679 0 0" xyz="0 0 0"/> <!-- rotation because cyl. geom primitive has symmetry axis in +x direction -->
-        <geometry>
-          <cylinder length="0.034" radius="0.074792"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="fl_caster_r_wheel_link">
-      <mu1 value="100.0"/>
-      <mu2 value="100.0"/>
-      <kp value="1000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/wheel_r"/>
-    </gazebo><transmission name="fl_caster_r_wheel_trans" type="SimpleTransmission">
-      <actuator name="fl_caster_r_wheel_motor"/>
-      <joint name="fl_caster_r_wheel_joint"/>
-      <mechanicalReduction>-79.2380952381</mechanicalReduction>
-    </transmission><gazebo reference="fl_caster_rotation_link">
-      <material value="PR2/caster_texture"/>
-    </gazebo><joint name="fr_caster_rotation_joint" type="continuous">
-      <axis xyz="0 0 1"/>
-      <limit effort="100" velocity="100"/>
-      <safety_controller k_velocity="10"/>
-      <calibration reference_position="0.0"/>
-      <dynamics damping="0.0" friction="0.0"/>
-      <origin rpy="0 0 0" xyz="0.2225 -0.2225 0.0282"/>
-      <parent link="base_link"/>
-      <child link="fr_caster_rotation_link"/>
-    </joint><link name="fr_caster_rotation_link">
-
-      <inertial>
-        <mass value="3.473082"/>
-        <origin xyz="0 0 0.07"/>
-        <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
-      </inertial>
-
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://pr2_description/meshes/base_v0/caster.stl"/>
-        </geometry>
-        
-        <material name="Green"/>
-      </visual>
-
-      <collision>
-        <origin rpy="0.0 0.0 0.0" xyz="0 0 0.07"/>
-        <geometry>
-          <box size="0.192 0.164 0.013"/>
-        </geometry>
-      </collision>
-    </link><transmission name="fr_caster_rotation_trans" type="SimpleTransmission">
-      <actuator name="fr_caster_rotation_motor"/>
-      <joint name="fr_caster_rotation_joint"/>
-      <mechanicalReduction>-79.2380952381</mechanicalReduction>
-    </transmission><joint name="fr_caster_l_wheel_joint" type="continuous">
-      <axis xyz="0 1 0"/>
-      <limit effort="100" velocity="100"/>
-      <safety_controller k_velocity="10"/>
-      <dynamics damping="0.0" friction="0.0"/>
-      <origin rpy="0 0 0" xyz="0 0.049 0"/>
-      <parent link="fr_caster_rotation_link"/>
-      <child link="fr_caster_l_wheel_link"/>
-    </joint><link name="fr_caster_l_wheel_link">
-      <inertial>
-        <mass value="0.44036"/>
-        <origin xyz="0 0 0"/>
-        <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
-      </inertial>
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://pr2_description/meshes/base_v0/pr2_wheel.stl"/>
-        </geometry>
-        
-        <material name="Wheel_l"/>
-      </visual>
-      <collision>
-        <origin rpy="1.57079632679 0 0" xyz="0 0 0"/> <!-- rotation because cyl. geom primitive has symmetry axis in +x direction -->
-        <geometry>
-          <cylinder length="0.034" radius="0.074792"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="fr_caster_l_wheel_link">
-      <mu1 value="100.0"/>
-      <mu2 value="100.0"/>
-      <kp value="1000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/wheel_l"/>
-    </gazebo><transmission name="fr_caster_l_wheel_trans" type="SimpleTransmission">
-      <actuator name="fr_caster_l_wheel_motor"/>
-      <joint name="fr_caster_l_wheel_joint"/>
-      <mechanicalReduction>79.2380952381</mechanicalReduction>
-    </transmission><joint name="fr_caster_r_wheel_joint" type="continuous">
-      <axis xyz="0 1 0"/>
-      <limit effort="100" velocity="100"/>
-      <safety_controller k_velocity="10"/>
-      <dynamics damping="0.0" friction="0.0"/>
-      <origin rpy="0 0 0" xyz="0 -0.049 0"/>
-      <parent link="fr_caster_rotation_link"/>
-      <child link="fr_caster_r_wheel_link"/>
-    </joint><link name="fr_caster_r_wheel_link">
-      <inertial>
-        <mass value="0.44036"/>
-        <origin xyz="0 0 0"/>
-        <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
-      </inertial>
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://pr2_description/meshes/base_v0/pr2_wheel.stl"/>
-        </geometry>
-        
-        <material name="Wheel_r"/>
-      </visual>
-      <collision>
-        <origin rpy="1.57079632679 0 0" xyz="0 0 0"/> <!-- rotation because cyl. geom primitive has symmetry axis in +x direction -->
-        <geometry>
-          <cylinder length="0.034" radius="0.074792"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="fr_caster_r_wheel_link">
-      <mu1 value="100.0"/>
-      <mu2 value="100.0"/>
-      <kp value="1000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/wheel_r"/>
-    </gazebo><transmission name="fr_caster_r_wheel_trans" type="SimpleTransmission">
-      <actuator name="fr_caster_r_wheel_motor"/>
-      <joint name="fr_caster_r_wheel_joint"/>
-      <mechanicalReduction>-79.2380952381</mechanicalReduction>
-    </transmission><gazebo reference="fr_caster_rotation_link">
-      <material value="PR2/caster_texture"/>
-    </gazebo><joint name="bl_caster_rotation_joint" type="continuous">
-      <axis xyz="0 0 1"/>
-      <limit effort="100" velocity="100"/>
-      <safety_controller k_velocity="10"/>
-      <calibration reference_position="0.0"/>
-      <dynamics damping="0.0" friction="0.0"/>
-      <origin rpy="0 0 0" xyz="-0.2225 0.2225 0.0282"/>
-      <parent link="base_link"/>
-      <child link="bl_caster_rotation_link"/>
-    </joint><link name="bl_caster_rotation_link">
-
-      <inertial>
-        <mass value="3.473082"/>
-        <origin xyz="0 0 0.07"/>
-        <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
-      </inertial>
-
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://pr2_description/meshes/base_v0/caster.stl"/>
-        </geometry>
-        
-        <material name="Green"/>
-      </visual>
-
-      <collision>
-        <origin rpy="0.0 0.0 0.0" xyz="0 0 0.07"/>
-        <geometry>
-          <box size="0.192 0.164 0.013"/>
-        </geometry>
-      </collision>
-    </link><transmission name="bl_caster_rotation_trans" type="SimpleTransmission">
-      <actuator name="bl_caster_rotation_motor"/>
-      <joint name="bl_caster_rotation_joint"/>
-      <mechanicalReduction>-79.2380952381</mechanicalReduction>
-    </transmission><joint name="bl_caster_l_wheel_joint" type="continuous">
-      <axis xyz="0 1 0"/>
-      <limit effort="100" velocity="100"/>
-      <safety_controller k_velocity="10"/>
-      <dynamics damping="0.0" friction="0.0"/>
-      <origin rpy="0 0 0" xyz="0 0.049 0"/>
-      <parent link="bl_caster_rotation_link"/>
-      <child link="bl_caster_l_wheel_link"/>
-    </joint><link name="bl_caster_l_wheel_link">
-      <inertial>
-        <mass value="0.44036"/>
-        <origin xyz="0 0 0"/>
-        <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
-      </inertial>
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://pr2_description/meshes/base_v0/pr2_wheel.stl"/>
-        </geometry>
-        
-        <material name="Wheel_l"/>
-      </visual>
-      <collision>
-        <origin rpy="1.57079632679 0 0" xyz="0 0 0"/> <!-- rotation because cyl. geom primitive has symmetry axis in +x direction -->
-        <geometry>
-          <cylinder length="0.034" radius="0.074792"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="bl_caster_l_wheel_link">
-      <mu1 value="100.0"/>
-      <mu2 value="100.0"/>
-      <kp value="1000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/wheel_l"/>
-    </gazebo><transmission name="bl_caster_l_wheel_trans" type="SimpleTransmission">
-      <actuator name="bl_caster_l_wheel_motor"/>
-      <joint name="bl_caster_l_wheel_joint"/>
-      <mechanicalReduction>79.2380952381</mechanicalReduction>
-    </transmission><joint name="bl_caster_r_wheel_joint" type="continuous">
-      <axis xyz="0 1 0"/>
-      <limit effort="100" velocity="100"/>
-      <safety_controller k_velocity="10"/>
-      <dynamics damping="0.0" friction="0.0"/>
-      <origin rpy="0 0 0" xyz="0 -0.049 0"/>
-      <parent link="bl_caster_rotation_link"/>
-      <child link="bl_caster_r_wheel_link"/>
-    </joint><link name="bl_caster_r_wheel_link">
-      <inertial>
-        <mass value="0.44036"/>
-        <origin xyz="0 0 0"/>
-        <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
-      </inertial>
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://pr2_description/meshes/base_v0/pr2_wheel.stl"/>
-        </geometry>
-        
-        <material name="Wheel_r"/>
-      </visual>
-      <collision>
-        <origin rpy="1.57079632679 0 0" xyz="0 0 0"/> <!-- rotation because cyl. geom primitive has symmetry axis in +x direction -->
-        <geometry>
-          <cylinder length="0.034" radius="0.074792"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="bl_caster_r_wheel_link">
-      <mu1 value="100.0"/>
-      <mu2 value="100.0"/>
-      <kp value="1000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/wheel_r"/>
-    </gazebo><transmission name="bl_caster_r_wheel_trans" type="SimpleTransmission">
-      <actuator name="bl_caster_r_wheel_motor"/>
-      <joint name="bl_caster_r_wheel_joint"/>
-      <mechanicalReduction>-79.2380952381</mechanicalReduction>
-    </transmission><gazebo reference="bl_caster_rotation_link">
-      <material value="PR2/caster_texture"/>
-    </gazebo><joint name="br_caster_rotation_joint" type="continuous">
-      <axis xyz="0 0 1"/>
-      <limit effort="100" velocity="100"/>
-      <safety_controller k_velocity="10"/>
-      <calibration reference_position="0.0"/>
-      <dynamics damping="0.0" friction="0.0"/>
-      <origin rpy="0 0 0" xyz="-0.2225 -0.2225 0.0282"/>
-      <parent link="base_link"/>
-      <child link="br_caster_rotation_link"/>
-    </joint><link name="br_caster_rotation_link">
-
-      <inertial>
-        <mass value="3.473082"/>
-        <origin xyz="0 0 0.07"/>
-        <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
-      </inertial>
-
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://pr2_description/meshes/base_v0/caster.stl"/>
-        </geometry>
-        
-        <material name="Green"/>
-      </visual>
-
-      <collision>
-        <origin rpy="0.0 0.0 0.0" xyz="0 0 0.07"/>
-        <geometry>
-          <box size="0.192 0.164 0.013"/>
-        </geometry>
-      </collision>
-    </link><transmission name="br_caster_rotation_trans" type="SimpleTransmission">
-      <actuator name="br_caster_rotation_motor"/>
-      <joint name="br_caster_rotation_joint"/>
-      <mechanicalReduction>-79.2380952381</mechanicalReduction>
-    </transmission><joint name="br_caster_l_wheel_joint" type="continuous">
-      <axis xyz="0 1 0"/>
-      <limit effort="100" velocity="100"/>
-      <safety_controller k_velocity="10"/>
-      <dynamics damping="0.0" friction="0.0"/>
-      <origin rpy="0 0 0" xyz="0 0.049 0"/>
-      <parent link="br_caster_rotation_link"/>
-      <child link="br_caster_l_wheel_link"/>
-    </joint><link name="br_caster_l_wheel_link">
-      <inertial>
-        <mass value="0.44036"/>
-        <origin xyz="0 0 0"/>
-        <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
-      </inertial>
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://pr2_description/meshes/base_v0/pr2_wheel.stl"/>
-        </geometry>
-        
-        <material name="Wheel_l"/>
-      </visual>
-      <collision>
-        <origin rpy="1.57079632679 0 0" xyz="0 0 0"/> <!-- rotation because cyl. geom primitive has symmetry axis in +x direction -->
-        <geometry>
-          <cylinder length="0.034" radius="0.074792"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="br_caster_l_wheel_link">
-      <mu1 value="100.0"/>
-      <mu2 value="100.0"/>
-      <kp value="1000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/wheel_l"/>
-    </gazebo><transmission name="br_caster_l_wheel_trans" type="SimpleTransmission">
-      <actuator name="br_caster_l_wheel_motor"/>
-      <joint name="br_caster_l_wheel_joint"/>
-      <mechanicalReduction>79.2380952381</mechanicalReduction>
-    </transmission><joint name="br_caster_r_wheel_joint" type="continuous">
-      <axis xyz="0 1 0"/>
-      <limit effort="100" velocity="100"/>
-      <safety_controller k_velocity="10"/>
-      <dynamics damping="0.0" friction="0.0"/>
-      <origin rpy="0 0 0" xyz="0 -0.049 0"/>
-      <parent link="br_caster_rotation_link"/>
-      <child link="br_caster_r_wheel_link"/>
-    </joint><link name="br_caster_r_wheel_link">
-      <inertial>
-        <mass value="0.44036"/>
-        <origin xyz="0 0 0"/>
-        <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
-      </inertial>
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <mesh filename="package://pr2_description/meshes/base_v0/pr2_wheel.stl"/>
-        </geometry>
-        
-        <material name="Wheel_r"/>
-      </visual>
-      <collision>
-        <origin rpy="1.57079632679 0 0" xyz="0 0 0"/> <!-- rotation because cyl. geom primitive has symmetry axis in +x direction -->
-        <geometry>
-          <cylinder length="0.034" radius="0.074792"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="br_caster_r_wheel_link">
-      <mu1 value="100.0"/>
-      <mu2 value="100.0"/>
-      <kp value="1000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/wheel_r"/>
-    </gazebo><transmission name="br_caster_r_wheel_trans" type="SimpleTransmission">
-      <actuator name="br_caster_r_wheel_motor"/>
-      <joint name="br_caster_r_wheel_joint"/>
-      <mechanicalReduction>-79.2380952381</mechanicalReduction>
-    </transmission><gazebo reference="br_caster_rotation_link">
-      <material value="PR2/caster_texture"/>
-    </gazebo><gazebo reference="base_link">
-      <selfCollide>true</selfCollide>
-      <sensor:contact name="base_contact_sensor">
-        <geom>base_link_geom</geom>
-        <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="base_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>base_bumper</bumperTopicName>
-          <interface:bumper name="base_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <material value="PR2/Green"/>
-    </gazebo><gazebo>
-      <controller:gazebo_ros_p3d name="p3d_base_controller" plugin="libgazebo_ros_p3d.so">
-        <alwaysOn>true</alwaysOn>
-        <updateRate>100.0</updateRate>
-        <bodyName>base_link</bodyName>
-        <topicName>base_pose_ground_truth</topicName>
-        <gaussianNoise>0.01</gaussianNoise>
-        <frameName>map</frameName>
-        <xyzOffsets>25.7 25.7 0</xyzOffsets> <!-- initialize odometry for fake localization-->
-        <rpyOffsets>0 0 0</rpyOffsets>
-        <interface:position name="p3d_base_position"/>
-      </controller:gazebo_ros_p3d>
-    </gazebo>
-
-  <joint name="torso_lift_joint" type="prismatic">
-      <axis xyz="0 0 1"/>
-      <!-- KWatts: Max should be 0.31, changed to 0.20 to prevent jamming -->
-      <!-- HW ticket 481, will examine spine to determine failures -->
-      <limit effort="10000" lower="0.0" upper="0.20" velocity="0.015"/>
-      <safety_controller k_position="1000" k_velocity="400000" soft_lower_limit="0.005" soft_upper_limit="0.195"/>
-      <calibration reference_position="0.00536"/>
-      <dynamics damping="10.0"/>
-      <origin rpy="0 0 0" xyz="-0.05 0 0.739675"/>
-      <parent link="base_link"/>
-      <child link="torso_lift_link"/>
-    </joint><link name="torso_lift_link">
-
-      <inertial>
-        <mass value="36.248046"/>
-        <origin xyz="-0.1 0 -0.0885"/>
-        <inertia ixx="2.771653750257" ixy="0.004284522609" ixz="-0.160418504506" iyy="2.510019507959" iyz="0.029664468704" izz="0.526432355569"/>
-      </inertial>
-
-      <visual name="torso_lift_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="torso_lift_visual_geom">
-          <mesh filename="package://pr2_description/meshes/torso_v0/torso.stl"/>
-        </geometry>
-        
-        <material name="Grey2"/>
-      </visual>
-
-      <collision name="torso_lift_collision">
-        <origin rpy="0 0 0" xyz="0 0 0.16"/>
-        <geometry name="torso_lift_collision_geom">
-          <!-- arbitrarily chosen collision box by JH, torso collision is not important for now -->
-          <box size="0.30 0.620 0.02"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="torso_lift_link">
-      <sensor:contact name="torso_lift_contact_sensor">
-        <geom>torso_lift_link_geom</geom>
-        <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="torso_lift_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>torso_lift_bumper</bumperTopicName>
-          <interface:bumper name="torso_lift_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <material value="PR2/Grey2"/>
-    </gazebo><transmission name="torso_lift_trans" type="SimpleTransmission">
-      <actuator name="torso_lift_motor"/>
-      <joint name="torso_lift_joint"/>
-      <mechanicalReduction>-52143.33</mechanicalReduction>
-    </transmission>
-
-  <!-- The xacro preprocesser will replace the parameters below, such as ${cal_head_x}, with
-       numerical values that were specified in common.xacro which was included above -->
-  <joint name="head_pan_joint" type="revolute">
-      <axis xyz="0 0 1"/>
-      <limit effort="2.645" lower="-3.007" upper="3.007" velocity="6"/>
-      <safety_controller k_position="100" k_velocity="1.5" soft_lower_limit="-2.857" soft_upper_limit="2.857"/>
-      <calibration reference_position="0.0"/>
-      <dynamics damping="1.0"/>
-      <origin rpy="0.0 0.0 0.0" xyz="0.0    0.0     0.3915"/>
-      <parent link="torso_lift_link"/>
-      <child link="head_pan_link"/>
-    </joint><link name="head_pan_link">
-
-
-      <inertial>
-        <mass value="1.611118"/>
-        <origin rpy="0 0 0" xyz="-0.005717  0.010312 -0.029649"/>
-        <inertia ixx="0.00482611007" ixy="-0.000144683999" ixz="0.000110076136" iyy="0.005218991412" iyz="-0.000314239509" izz="0.008618784925"/>
-      </inertial>
-
-      <visual name="head_pan_visual">
-        <origin rpy="0 0 0 " xyz="0 0 0.0"/>
-        <geometry name="head_pan_visual_geom">
-          <mesh filename="package://pr2_description/meshes/head_v0/head_pan.stl"/>
-        </geometry>
-        
-        <material name="Blue"/>
-      </visual>
-
-      <collision name="head_pan_collision">
-        <origin rpy="0.0 0.0 0.0 " xyz="-0.005717  0.010312 -0.029649"/>
-        <geometry name="head_pan_collision_geom">
-          <box size="0.188 0.219 0.137"/>
-        </geometry>
-      </collision>
-
-    </link><gazebo reference="head_pan_link">
-      <material value="PR2/Blue"/>
-    </gazebo><transmission name="head_pan_trans" type="SimpleTransmission">
-      <actuator name="head_pan_motor"/>
-      <joint name="head_pan_joint"/>
-      <mechanicalReduction>6.0</mechanicalReduction>
-    </transmission><joint name="head_tilt_joint" type="revolute">
-      <axis xyz="0 1 0"/>
-      <limit effort="15" lower="-0.471238" upper="1.39626" velocity="5"/>
-      <safety_controller k_position="100" k_velocity="1.5" soft_lower_limit="-0.3712" soft_upper_limit="1.29626"/>
-      <calibration reference_position="0.0"/>
-      <dynamics damping="20.0"/>
-      <origin rpy="0 0 0" xyz="0.058 0 0"/>
-      <parent link="head_pan_link"/>
-      <child link="head_tilt_link"/>
-    </joint><link name="head_tilt_link">
-
-
-      <inertial>
-        <mass value="1.749727"/>
-        <origin rpy="0 0 0" xyz="0.041935 0.003569 0.028143"/>
-        <inertia ixx="0.010602303435" ixy="-0.000408814235" ixz="0.00198303894" iyy="0.011874383747" iyz="0.000197908779" izz="0.005516790626"/>
-      </inertial>
-
-      <visual name="head_tilt_visual">
-        <origin rpy="0.0 0.0 0.0 " xyz="0 0 0"/>
-        <geometry name="head_tilt_visual_geom">
-          <mesh filename="package://pr2_description/meshes/head_v0/head_tilt.stl"/>
-        </geometry>
-        
-        <material name="Green"/>
-      </visual>
-
-      <collision name="head_tilt_collision">
-        <origin rpy="0 0 0" xyz="0.041935 0.003569 0.028143"/>
-        <geometry name="head_tilt_collision_geom">
-          <box size="0.064 0.253 0.181"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="head_tilt_link">
-      <material value="PR2/Green"/>
-    </gazebo><transmission name="head_tilt_trans" type="SimpleTransmission">
-      <actuator name="head_tilt_motor"/>
-      <joint name="head_tilt_joint"/>
-      <mechanicalReduction>6.0</mechanicalReduction>
-    </transmission><joint name="head_plate_frame_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="0.0232 0 0.0645"/>     
-      <parent link="head_tilt_link"/>
-      <child link="head_plate_frame"/>
-    </joint><link name="head_plate_frame">
-
-      <inertial>
-        <mass value="0.01"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
-      </inertial>
-
-      <visual name="head_plate_frame_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="head_plate_frame_visual_geom">
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-        
-        <material name="Blue"/>
-      </visual>
-
-      <collision name="head_plate_frame_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="head_plate_frame_collision_geom">
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="head_plate_frame">
-      <material value="PR2/Blue"/>
-    </gazebo>
-
-  <!-- Camera package: double stereo, prosilica -->
-  <joint name="sensor_mount_frame_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-      <parent link="head_plate_frame"/>
-      <child link="sensor_mount_link"/>
-    </joint><link name="sensor_mount_link">
-
-      <inertial>
-        <!-- Needs verification with CAD -->
-        <mass value="0.05"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.01"/>
-      </inertial>
-
-      <!-- Should probably get real visuals here at some point -->
-      <visual name="sensor_mount_sensor_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="sensor_mount_sensor_visual_geom">
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-        
-        <material name="Blue"/>
-      </visual>
-
-      <collision name="sensor_mount_sensor_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="sensor_mount_sensor_collision_geom">
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </collision>
-    </link><joint name="high_def_frame_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="0 -0.109 0.035"/>
-      <parent link="sensor_mount_link"/>
-      <child link="high_def_frame"/>
-    </joint><link name="high_def_frame">
-
-      <inertial>
-        <mass value="0.01"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
-      </inertial>
-
-      <visual>
-        <!-- bogus values for visual aesthetics -->
-        <origin rpy="0 0 0" xyz="-0.02 0 0"/>
-        <geometry>
-          <box size="0.04 0.04 0.04"/>
-        </geometry>
-        <material name="Blue"/>
-      </visual>
-
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </collision>
-
-
-    </link><joint name="high_def_optical_frame_joint" type="fixed">
-      <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0.0            0.0         0.0"/>
-      <parent link="high_def_frame"/>
-      <child link="high_def_optical_frame"/>
-    </joint><link name="high_def_optical_frame">
-
-      <inertial>
-        <mass value="0.01"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
-      </inertial>
-
-      <visual>
-        <!-- bogus values for visual aesthetics -->
-        <origin rpy="0 0 1.57079632679" xyz="0 0 0"/>
-        <geometry>
-          <cylinder length="0.05" radius="0.02"/>
-        </geometry>
-        <material name="Blue"/>
-      </visual>
-
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="high_def_frame">
-    <sensor:camera name="high_def_sensor">
-      <imageFormat>R8G8B8</imageFormat>
-      <imageSize>2448 2050</imageSize>
-      <hfov>45</hfov>
-      <nearClip>0.1</nearClip>
-      <farClip>100</farClip>
-      <updateRate>20.0</updateRate>
-      <controller:gazebo_ros_prosilica name="high_def_controller" plugin="libgazebo_ros_prosilica.so">
-        <alwaysOn>true</alwaysOn>
-        <updateRate>20.0</updateRate>
-        <camInfoTopicName>/prosilica/cam_info</camInfoTopicName>
-        <imageTopicName>/prosilica/image</imageTopicName>
-        <imageRectTopicName>/prosilica/image_rect</imageRectTopicName>
-        <camInfoServiceName>/prosilica/cam_info_service</camInfoServiceName>
-        <pollServiceName>/prosilica/poll</pollServiceName>
-        <frameName>hight_def_optical_frame</frameName>
-        <CxPrime>1224.5</CxPrime>
-        <Cx>1224.5</Cx>
-        <Cy>1025.5</Cy>
-        <focal_length>2955</focal_length> <!-- image_width / (2*tan(hfov_radian /2)) -->
-        <distortion_k1>0</distortion_k1>
-        <distortion_k2>0</distortion_k2>
-        <distortion_k3>0</distortion_k3>
-        <distortion_t1>0</distortion_t1>
-        <distortion_t2>0</distortion_t2>
-        <interface:camera name="high_def_iface"/>
-      </controller:gazebo_ros_prosilica>
-    </sensor:camera>
-    <material value="PR2/Blue"/>
-  </gazebo><gazebo reference="high_def_optical_frame">
-    <material value="PR2/Blue"/>
-  </gazebo><joint name="double_stereo_frame_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="0.0 0.0 0.003"/>
-      <parent link="sensor_mount_link"/>
-      <child link="double_stereo_link"/>
-    </joint><link name="double_stereo_link">
-
-      <inertial>
-        <!-- Needs verification with CAD -->
-        <mass value="0.1"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.01"/>
-      </inertial>
-
-      <!-- Should probably get real visuals here at some point -->
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-        <material name="Blue"/>
-      </visual>
-
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </collision>
-    </link><joint name="wide_stereo_frame_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="0 -0.015 0"/>
-      <parent link="double_stereo_link"/>
-      <child link="wide_stereo_link"/>
-    </joint><link name="wide_stereo_link">
-      
-      <inertial>
-        <mass value="0.1"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <inertia ixx="0.00482611007" ixy="-0.000144683999" ixz="0.000110076136" iyy="0.005218991412" iyz="-0.000314239509" izz="0.008618784925"/>
-      </inertial>
-
-      <visual>
-        <origin rpy="0 0 0" xyz="0.0 0 0.05"/>
-        <geometry>
-          <box size="0.005 0.01 0.005"/>
-        </geometry>
-        
-        <material name="Blue"/>
-      </visual>
-
-      <collision>
-        <origin rpy="0 0 0" xyz="0.0 0 0.05"/>
-        <geometry>
-          <box size="0.005 0.01 0.005"/>
-        </geometry>
-      </collision>
-      
-    </link><joint name="wide_stereo_optical_frame_joint" type="fixed">
-      <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0.0            0.045   0.0305"/>
-      <parent link="wide_stereo_link"/>
-      <child link="wide_stereo_optical_frame"/>
-    </joint><link name="wide_stereo_optical_frame" type="camera">
-
-      <inertial>
-        <mass value="0.01"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
-      </inertial>
-
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.001 0.001 0.001"/>
-        </geometry>
-        
-        <material name="Blue"/>
-      </visual>
-
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.0 0.0 0.0"/>
-        </geometry>
-      </collision>
-
-    </link><joint name="wide_stereo_l_stereo_camera_frame_joint" type="fixed">
-      <origin rpy="0 -1.57079632679 1.57079632679" xyz="0 0 0"/>
-      <parent link="wide_stereo_optical_frame"/>
-      <child link="wide_stereo_l_stereo_camera_frame"/>
-    </joint><link name="wide_stereo_l_stereo_camera_frame">
-
-      <inertial>
-        <mass value="0.01"/>
-        <origin xyz="0 0 0"/>
-        <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
-      </inertial>
-
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </visual>
-
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </collision>
-    </link><joint name="wide_stereo_l_stereo_camera_optical_frame_joint" type="fixed">
-      <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
-      <parent link="wide_stereo_l_stereo_camera_frame"/>
-      <child link="wide_stereo_l_stereo_camera_optical_frame"/>
-    </joint><link name="wide_stereo_l_stereo_camera_optical_frame">
-
-
-      <inertial>
-        <mass value="0.01"/>
-        <origin xyz="0 0 0"/>
-        <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
-      </inertial>
-
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </visual>
-
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="wide_stereo_l_stereo_camera_frame">
-      <sensor:camera name="wide_stereo_l_stereo_camera_sensor">
-        <!--imageSize>${image_width} ${image_height}</imageSize-->
-        <imageSize>640 480</imageSize> <!-- FIXME: temporary fix because xacro expands above line to floats, not integers -->
-        <imageFormat>B8G8R8</imageFormat>
-        <hfov>90.0</hfov>
-        <nearClip>0.1</nearClip>
-        <farClip>100</farClip>
-        <updateRate>25.0</updateRate>
-        <controller:gazebo_ros_camera name="wide_stereo_l_stereo_camera_controller" plugin="libgazebo_ros_camera.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>25.0</updateRate>
-          <imageTopicName>wide_stereo/left/image_raw</imageTopicName>
-          <cameraInfoTopicName>wide_stereo/left/camera_info</cameraInfoTopicName>
-          <frameName>wide_stereo_optical_frame</frameName>
-          <hackBaseline>0.0</hackBaseline>
-          <CxPrime>320.5</CxPrime>
-          <Cx>320.5</Cx>
-          <Cy>240.5</Cy>
-          <!-- image_width / (2*tan(hfov_radian /2)) -->
-          <!-- 320 for wide and 772.55 for narrow stereo camera -->
-          <focal_length>320.0</focal_length>
-          <distortion_k1>0</distortion_k1>
-          <distortion_k2>0</distortion_k2>
-          <distortion_k3>0</distortion_k3>
-          <distortion_t1>0</distortion_t1>
-          <distortion_t2>0</distortion_t2>
-          <interface:camera name="wide_stereo_l_stereo_camera_iface"/>
-        </controller:gazebo_ros_camera>
-      </sensor:camera>
-      <turnGravityOff>true</turnGravityOff>
-      <material>PR2/Blue</material>
-    </gazebo><gazebo reference="wide_stereo_l_stereo_camera_optical_frame">
-      <turnGravityOff>true</turnGravityOff>
-    </gazebo><joint name="wide_stereo_r_stereo_camera_frame_joint" type="fixed">
-      <origin rpy="0.0 0.0 0.0" xyz="0.0 -0.09 0.0"/>
-      <parent link="wide_stereo_l_stereo_camera_frame"/>
-      <child link="wide_stereo_r_stereo_camera_frame"/>
-    </joint><link name="wide_stereo_r_stereo_camera_frame">
-
-      <inertial>
-        <mass value="0.01"/>
-        <origin xyz="0 0 0"/>
-        <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
-      </inertial>
-
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </visual>
-
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </collision>
-    </link><joint name="wide_stereo_r_stereo_camera_optical_frame_joint" type="fixed">
-      <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
-      <parent link="wide_stereo_r_stereo_camera_frame"/>
-      <child link="wide_stereo_r_stereo_camera_optical_frame"/>
-    </joint><link name="wide_stereo_r_stereo_camera_optical_frame">
-
-
-      <inertial>
-        <mass value="0.01"/>
-        <origin xyz="0 0 0"/>
-        <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
-      </inertial>
-
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </visual>
-
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="wide_stereo_r_stereo_camera_frame">
-      <sensor:camera name="wide_stereo_r_stereo_camera_sensor">
-        <!--imageSize>${image_width} ${image_height}</imageSize-->
-        <imageSize>640 480</imageSize> <!-- FIXME: temporary fix because xacro expands above line to floats, not integers -->
-        <imageFormat>B8G8R8</imageFormat>
-        <hfov>90.0</hfov>
-        <nearClip>0.1</nearClip>
-        <farClip>100</farClip>
-        <updateRate>25.0</updateRate>
-        <controller:gazebo_ros_camera name="wide_stereo_r_stereo_camera_controller" plugin="libgazebo_ros_camera.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>25.0</updateRate>
-          <imageTopicName>wide_stereo/right/image_raw</imageTopicName>
-          <cameraInfoTopicName>wide_stereo/right/camera_info</cameraInfoTopicName>
-          <frameName>wide_stereo_optical_frame</frameName>
-          <hackBaseline>0.09</hackBaseline>
-          <CxPrime>320.5</CxPrime>
-          <Cx>320.5</Cx>
-          <Cy>240.5</Cy>
-          <!-- image_width / (2*tan(hfov_radian /2)) -->
-          <!-- 320 for wide and 772.55 for narrow stereo camera -->
-          <focal_length>320.0</focal_length>
-          <distortion_k1>0</distortion_k1>
-          <distortion_k2>0</distortion_k2>
-          <distortion_k3>0</distortion_k3>
-          <distortion_t1>0</distortion_t1>
-          <distortion_t2>0</distortion_t2>
-          <interface:camera name="wide_stereo_r_stereo_camera_iface"/>
-        </controller:gazebo_ros_camera>
-      </sensor:camera>
-      <turnGravityOff>true</turnGravityOff>
-      <material>PR2/Blue</material>
-    </gazebo><gazebo reference="wide_stereo_r_stereo_camera_optical_frame">
-      <turnGravityOff>true</turnGravityOff>
-    </gazebo><gazebo reference="wide_stereo_link">
-      <material value="PR2/Blue"/>
-    </gazebo><gazebo reference="wide_stereo_optical_frame">
-      <material value="PR2/Blue"/>
-    </gazebo><gazebo> <!-- this is not attached to any link, there no reference -->
-      <controller:gazebo_ros_stereo_camera name="wide_stereo_controller" plugin="libgazebo_ros_stereo_camera.so">
-        <alwaysOn>true</alwaysOn>
-        <updateRate>20.0</updateRate>
-        <leftCamera>wide_stereo_l_stereo_camera_sensor</leftCamera>
-        <rightCamera>wide_stereo_r_stereo_camera_sensor</rightCamera>
-        <topicName>wide_stereo/raw_stereo</topicName>
-        <frameName>wide_stereo_optical_frame</frameName>
-        <CxPrime>320.5</CxPrime>
-        <Cx>320.5</Cx>
-        <Cy>240.5</Cy>
-        <!-- image_width / (2*tan(hfov_radian /2)) -->
-        <!-- 320 for wide and 772.55 for narrow stereo camera -->
-        <focal_length>320.0</focal_length>
-        <distortion_k1>0</distortion_k1>
-        <distortion_k2>0</distortion_k2>
-        <distortion_k3>0</distortion_k3>
-        <distortion_t1>0</distortion_t1>
-        <distortion_t2>0</distortion_t2>
-        <baseline>-0.09</baseline> <!-- home pos. of robot has +x forward, +y left -->
-        <interface:stereocamera name="wide_stereo_iface"/>
-      </controller:gazebo_ros_stereo_camera>
-    </gazebo><joint name="narrow_stereo_frame_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="0 0.015 0"/>
-      <parent link="double_stereo_link"/>
-      <child link="narrow_stereo_link"/>
-    </joint><link name="narrow_stereo_link">
-      
-      <inertial>
-        <mass value="0.1"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <inertia ixx="0.00482611007" ixy="-0.000144683999" ixz="0.000110076136" iyy="0.005218991412" iyz="-0.000314239509" izz="0.008618784925"/>
-      </inertial>
-
-      <visual>
-        <origin rpy="0 0 0" xyz="0.0 0 0.05"/>
-        <geometry>
-          <box size="0.005 0.01 0.005"/>
-        </geometry>
-        
-        <material name="Blue"/>
-      </visual>
-
-      <collision>
-        <origin rpy="0 0 0" xyz="0.0 0 0.05"/>
-        <geometry>
-          <box size="0.005 0.01 0.005"/>
-        </geometry>
-      </collision>
-      
-    </link><joint name="narrow_stereo_optical_frame_joint" type="fixed">
-      <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0.0            0.045   0.0305"/>
-      <parent link="narrow_stereo_link"/>
-      <child link="narrow_stereo_optical_frame"/>
-    </joint><link name="narrow_stereo_optical_frame" type="camera">
-
-      <inertial>
-        <mass value="0.01"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
-      </inertial>
-
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.001 0.001 0.001"/>
-        </geometry>
-        
-        <material name="Blue"/>
-      </visual>
-
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.0 0.0 0.0"/>
-        </geometry>
-      </collision>
-
-    </link><joint name="narrow_stereo_l_stereo_camera_frame_joint" type="fixed">
-      <origin rpy="0 -1.57079632679 1.57079632679" xyz="0 0 0"/>
-      <parent link="narrow_stereo_optical_frame"/>
-      <child link="narrow_stereo_l_stereo_camera_frame"/>
-    </joint><link name="narrow_stereo_l_stereo_camera_frame">
-
-      <inertial>
-        <mass value="0.01"/>
-        <origin xyz="0 0 0"/>
-        <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
-      </inertial>
-
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </visual>
-
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </collision>
-    </link><joint name="narrow_stereo_l_stereo_camera_optical_frame_joint" type="fixed">
-      <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
-      <parent link="narrow_stereo_l_stereo_camera_frame"/>
-      <child link="narrow_stereo_l_stereo_camera_optical_frame"/>
-    </joint><link name="narrow_stereo_l_stereo_camera_optical_frame">
-
-
-      <inertial>
-        <mass value="0.01"/>
-        <origin xyz="0 0 0"/>
-        <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
-      </inertial>
-
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </visual>
-
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="narrow_stereo_l_stereo_camera_frame">
-      <sensor:camera name="narrow_stereo_l_stereo_camera_sensor">
-        <!--imageSize>${image_width} ${image_height}</imageSize-->
-        <imageSize>640 480</imageSize> <!-- FIXME: temporary fix because xacro expands above line to floats, not integers -->
-        <imageFormat>L8</imageFormat>
-        <hfov>45.0</hfov>
-        <nearClip>0.1</nearClip>
-        <farClip>100</farClip>
-        <updateRate>25.0</updateRate>
-        <controller:gazebo_ros_camera name="narrow_stereo_l_stereo_camera_controller" plugin="libgazebo_ros_camera.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>25.0</updateRate>
-          <imageTopicName>narrow_stereo/left/image_raw</imageTopicName>
-          <cameraInfoTopicName>narrow_stereo/left/camera_info</cameraInfoTopicName>
-          <frameName>narrow_stereo_optical_frame</frameName>
-          <hackBaseline>0.0</hackBaseline>
-          <CxPrime>320.5</CxPrime>
-          <Cx>320.5</Cx>
-          <Cy>240.5</Cy>
-          <!-- image_width / (2*tan(hfov_radian /2)) -->
-          <!-- 320 for wide and 772.55 for narrow stereo camera -->
-          <focal_length>772.55</focal_length>
-          <distortion_k1>0</distortion_k1>
-          <distortion_k2>0</distortion_k2>
-          <distortion_k3>0</distortion_k3>
-          <distortion_t1>0</distortion_t1>
-          <distortion_t2>0</distortion_t2>
-          <interface:camera name="narrow_stereo_l_stereo_camera_iface"/>
-        </controller:gazebo_ros_camera>
-      </sensor:camera>
-      <turnGravityOff>true</turnGravityOff>
-      <material>PR2/Blue</material>
-    </gazebo><gazebo reference="narrow_stereo_l_stereo_camera_optical_frame">
-      <turnGravityOff>true</turnGravityOff>
-    </gazebo><joint name="narrow_stereo_r_stereo_camera_frame_joint" type="fixed">
-      <origin rpy="0.0 0.0 0.0" xyz="0.0 -0.09 0.0"/>
-      <parent link="narrow_stereo_l_stereo_camera_frame"/>
-      <child link="narrow_stereo_r_stereo_camera_frame"/>
-    </joint><link name="narrow_stereo_r_stereo_camera_frame">
-
-      <inertial>
-        <mass value="0.01"/>
-        <origin xyz="0 0 0"/>
-        <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
-      </inertial>
-
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </visual>
-
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </collision>
-    </link><joint name="narrow_stereo_r_stereo_camera_optical_frame_joint" type="fixed">
-      <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
-      <parent link="narrow_stereo_r_stereo_camera_frame"/>
-      <child link="narrow_stereo_r_stereo_camera_optical_frame"/>
-    </joint><link name="narrow_stereo_r_stereo_camera_optical_frame">
-
-
-      <inertial>
-        <mass value="0.01"/>
-        <origin xyz="0 0 0"/>
-        <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
-      </inertial>
-
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </visual>
-
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="narrow_stereo_r_stereo_camera_frame">
-      <sensor:camera name="narrow_stereo_r_stereo_camera_sensor">
-        <!--imageSize>${image_width} ${image_height}</imageSize-->
-        <imageSize>640 480</imageSize> <!-- FIXME: temporary fix because xacro expands above line to floats, not integers -->
-        <imageFormat>L8</imageFormat>
-        <hfov>45.0</hfov>
-        <nearClip>0.1</nearClip>
-        <farClip>100</farClip>
-        <updateRate>25.0</updateRate>
-        <controller:gazebo_ros_camera name="narrow_stereo_r_stereo_camera_controller" plugin="libgazebo_ros_camera.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>25.0</updateRate>
-          <imageTopicName>narrow_stereo/right/image_raw</imageTopicName>
-          <cameraInfoTopicName>narrow_stereo/right/camera_info</cameraInfoTopicName>
-          <frameName>narrow_stereo_optical_frame</frameName>
-          <hackBaseline>0.09</hackBaseline>
-          <CxPrime>320.5</CxPrime>
-          <Cx>320.5</Cx>
-          <Cy>240.5</Cy>
-          <!-- image_width / (2*tan(hfov_radian /2)) -->
-          <!-- 320 for wide and 772.55 for narrow stereo camera -->
-          <focal_length>772.55</focal_length>
-          <distortion_k1>0</distortion_k1>
-          <distortion_k2>0</distortion_k2>
-          <distortion_k3>0</distortion_k3>
-          <distortion_t1>0</distortion_t1>
-          <distortion_t2>0</distortion_t2>
-          <interface:camera name="narrow_stereo_r_stereo_camera_iface"/>
-        </controller:gazebo_ros_camera>
-      </sensor:camera>
-      <turnGravityOff>true</turnGravityOff>
-      <material>PR2/Blue</material>
-    </gazebo><gazebo reference="narrow_stereo_r_stereo_camera_optical_frame">
-      <turnGravityOff>true</turnGravityOff>
-    </gazebo><gazebo reference="narrow_stereo_link">
-      <material value="PR2/Blue"/>
-    </gazebo><gazebo reference="narrow_stereo_optical_frame">
-      <material value="PR2/Blue"/>
-    </gazebo><gazebo> <!-- this is not attached to any link, there no reference -->
-      <controller:gazebo_ros_stereo_camera name="narrow_stereo_controller" plugin="libgazebo_ros_stereo_camera.so">
-        <alwaysOn>true</alwaysOn>
-        <updateRate>20.0</updateRate>
-        <leftCamera>narrow_stereo_l_stereo_camera_sensor</leftCamera>
-        <rightCamera>narrow_stereo_r_stereo_camera_sensor</rightCamera>
-        <topicName>narrow_stereo/raw_stereo</topicName>
-        <frameName>narrow_stereo_optical_frame</frameName>
-        <CxPrime>320.5</CxPrime>
-        <Cx>320.5</Cx>
-        <Cy>240.5</Cy>
-        <!-- image_width / (2*tan(hfov_radian /2)) -->
-        <!-- 320 for wide and 772.55 for narrow stereo camera -->
-        <focal_length>772.55</focal_length>
-        <distortion_k1>0</distortion_k1>
-        <distortion_k2>0</distortion_k2>
-        <distortion_k3>0</distortion_k3>
-        <distortion_t1>0</distortion_t1>
-        <distortion_t2>0</distortion_t2>
-        <baseline>-0.09</baseline> <!-- home pos. of robot has +x forward, +y left -->
-        <interface:stereocamera name="narrow_stereo_iface"/>
-      </controller:gazebo_ros_stereo_camera>
-    </gazebo><gazebo reference="double_stereo_double_stereo_link">
-      <material value="PR2/Blue"/>
-    </gazebo><gazebo reference="sensor_mount_sensor_link">
-      <material value="PR2/Blue"/>
-    </gazebo>
-
-  <joint name="laser_tilt_mount_joint" type="revolute">
-      <axis xyz="0 1 0"/>
-      <limit effort="0.65" lower="-0.7854" upper="1.48353" velocity="10.0"/>
-      <safety_controller k_position="100" k_velocity="0.05" soft_lower_limit="-0.7354" soft_upper_limit="1.43353"/>
-      <calibration reference_position="-0.35"/>
-      <dynamics damping="0.008"/>
-      <origin rpy="0 0 0" xyz="0.1 0 0.235"/>
-      <parent link="torso_lift_link"/>
-      <child link="laser_tilt_mount_link"/>
-    </joint><link name="laser_tilt_mount_link">
-
-      <inertial>
-        <mass value="0.05"/>
-        <origin rpy="0 0 0" xyz="-0.03 0 -0.03"/>
-        <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.0001"/>
-      </inertial>
-
-      <visual name="laser_tilt_mount_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="laser_tilt_mount_visual_geom">
-          <mesh filename="package://pr2_description/meshes/tilting_laser_v0/hok_tilt.stl"/>
-        </geometry>
-        
-        <material name="Red"/>
-      </visual>
-
-      <collision name="laser_tilt_mount_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="laser_tilt_mount_collision_geom">
-          <box size=".001 .001 .001"/>
-        </geometry>
-      </collision>
-    </link><joint name="laser_tilt_joint" type="fixed">
-      <axis xyz="0 1 0"/>
-      <origin rpy="0 0 0" xyz="0 0 0.03"/>
-      <parent link="laser_tilt_mount_link"/>
-      <child link="laser_tilt_link"/>
-    </joint><link name="laser_tilt_link" type="laser">
-      <inertial>
-        <mass value="0.001"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.0001"/>
-      </inertial>
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.001 0.001 0.001"/>
-        </geometry>
-        <material name="Red"/>
-      </visual>
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size=".001 .001 .001"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="laser_tilt_link">
-      <sensor:ray name="laser_tilt">
-        <rayCount>640</rayCount>
-        <rangeCount>640</rangeCount>
-        <laserCount>1</laserCount>
-
-        <origin>0.0 0.0 0.0</origin>
-        <displayRays>false</displayRays>
-
-        <minAngle>-80</minAngle>
-        <maxAngle> 80</maxAngle>
-
-        <minRange>0.05</minRange>
-        <maxRange>10.0</maxRange>
-        <resRange>0.01</resRange>
-        <updateRate>40.0</updateRate>
-        <controller:gazebo_ros_laser name="gazebo_ros_laser_tilt_controller" plugin="libgazebo_ros_laser.so">
-          <gaussianNoise>0.005</gaussianNoise>
-          <alwaysOn>true</alwaysOn>
-          <updateRate>40.0</updateRate>
-          <topicName>tilt_scan</topicName>
-          <frameName>laser_tilt_link</frameName>
-          <interface:laser name="gazebo_ros_laser_tilt_iface"/>
-        </controller:gazebo_ros_laser>
-      </sensor:ray>
-    </gazebo><gazebo reference="laser_tilt_mount_link">
-      <material value="PR2/Red"/>
-    </gazebo><transmission name="laser_tilt_mount_trans" type="SimpleTransmission">
-      <actuator name="laser_tilt_mount_motor"/>
-      <joint name="laser_tilt_mount_joint"/>
-      <mechanicalReduction>-6.05</mechanicalReduction>
-    </transmission>
-
-  <!-- This is a common convention, to use a reflect parameter that equals +-1 to distinguish left from right -->
-  <joint name="r_shoulder_pan_joint" type="revolute">
-      <axis xyz="0 0 1"/>
-      <origin rpy="0 0 0" xyz="0.0 -0.188 0.0"/> <!-- transform from parent link to this joint frame -->
-      <parent link="torso_lift_link"/>
-      <child link="r_shoulder_pan_link"/>
-      <limit effort="30" lower="-2.2853981634" upper="0.714601836603" velocity="2.088"/>
-
-      <dynamics damping="10.0"/>
-
-      <safety_controller k_position="100" k_velocity="10" soft_lower_limit="-2.1353981634" soft_upper_limit="0.564601836603"/>
-
-      <!-- joint angle when the rising or the falling flag is activated on PR2 -->
-      <calibration reference_position="-0.785398163397"/>
-
-    </joint><link name="r_shoulder_pan_link">
-
-
-      <inertial>
-        <mass value="25.799322"/>
-        <origin rpy="0 0 0" xyz="-0.001201 0.024513 -0.098231"/>
-        <inertia ixx="0.866179142480" ixy="-0.06086507933" ixz="-0.12118061183" iyy="0.87421714893" iyz="-0.05886609911" izz="0.27353821674"/>
-      </inertial>
-
-      <visual name="r_shoulder_pan_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_shoulder_pan_visual_geom">
-          <mesh filename="package://pr2_description/meshes/shoulder_v0/shoulder_yaw.stl"/>
-        </geometry>
-        <material name="Blue"/>
-      </visual>
-
-      <collision name="r_shoulder_pan_collision">
-        <origin rpy="0 0 0" xyz="0.0 0 0.0"/>
-        <geometry name="r_shoulder_pan_collision_geom">
-          <mesh filename="package://pr2_description/meshes/shoulder_v0/convex/shoulder_yaw_convex.stlb"/>
-        </geometry>
-      </collision>
-    </link><joint name="r_shoulder_lift_joint" type="revolute">
-      <axis xyz="0 1 0"/>
-
-      <!-- Limits updated from Function's CAD values as of 2009_02_24 (link_data.xls) -->
-      <limit effort="30" lower="-0.5236" upper="1.3963" velocity="2.082"/>
-
-      <safety_controller k_position="100" k_velocity="10" soft_lower_limit="-0.3536" soft_upper_limit="1.3463"/>
-      <calibration reference_position="0.0"/>
-      <dynamics damping="10.0"/>
-      <origin rpy="0 0 0" xyz="0.1 0 0"/>
-      <parent link="r_shoulder_pan_link"/>
-      <child link="r_shoulder_lift_link"/>
-    </joint><link name="r_shoulder_lift_link">
-
-      <inertial>
-        <mass value="2.74988"/>
-        <origin rpy="0 0 0" xyz="0.02195 -0.02664 -0.03127"/>
-        <inertia ixx="0.02105584615" ixy="0.00496704022" ixz="-0.00194808955" iyy="0.02127223737" iyz="0.00110425490" izz="0.01975753814"/>
-      </inertial>
-
-      <visual name="r_shoulder_lift_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_shoulder_lift_visual_geom">
-          <mesh filename="package://pr2_description/meshes/shoulder_v0/shoulder_lift.stl"/>
-        </geometry>
-        <material name="Grey"/>
-      </visual>
-
-      <collision name="r_shoulder_lift_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_shoulder_lift_collision_geom">
-          <mesh filename="package://pr2_description/meshes/shoulder_v0/convex/shoulder_lift_convex.stlb"/>
-        </geometry>
-      </collision>
-    </link><joint name="r_upper_arm_roll_joint" type="revolute">
-      <axis xyz="1 0 0"/>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <parent link="r_shoulder_lift_link"/>
-      <child link="r_upper_arm_roll_link"/>
-      <limit effort="30" lower="-3.9" upper="0.8" velocity="3.27"/>
-      <safety_controller k_position="100" k_velocity="2" soft_lower_limit="-3.75" soft_upper_limit="0.65"/>
-      <calibration reference_position="-1.57079632679"/>
-      <dynamics damping="0.1"/>
-    </joint><link name="r_upper_arm_roll_link">
-
-
-      <inertial>
-        <!-- dummy mass, to be removed -->
-        <mass value="0.1"/>
-        <origin rpy="0 0 0" xyz="0.0 0 0"/>
-        <inertia ixx="0.01" ixy="0.00" ixz="0.00" iyy="0.01" iyz="0.00" izz="0.01"/>
-      </inertial>
-      <visual name="r_upper_arm_roll_visual">
-        <!-- TODO: This component doesn't actually have a mesh -->
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_upper_arm_roll_visual_geom">
-          <mesh filename="package://pr2_description/meshes/shoulder_v0/upper_arm_roll.stl"/>
-        </geometry>
-
-        <material name="RollLinks"/>
-      </visual>
-      <collision name="r_upper_arm_roll_collision">
-        <!-- TODO: collision tag should be optional -->
-        <origin rpy="0 0 0" xyz="0.0 0 0"/>
-        <geometry name="r_upper_arm_roll_collision_geom">
-          <box size="0.1 0.1 0.1"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="r_shoulder_pan_link">
-      <sensor:contact name="r_shoulder_pan_contact_sensor">
-        <geom>r_shoulder_pan_link_geom</geom>
-        <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="r_shoulder_pan_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>r_shoulder_pan_bumper</bumperTopicName>
-          <interface:bumper name="r_shoulder_pan_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <material value="PR2/Blue"/>
-      <turnGravityOff>true</turnGravityOff>
-    </gazebo><gazebo reference="r_shoulder_pan_joint">
-      <stopKd value="1.0"/>
-      <stopKp value="1000000.0"/>
-    </gazebo><gazebo reference="r_shoulder_lift_link">
-      <sensor:contact name="r_shoulder_lift_contact_sensor">
-        <geom>r_shoulder_lift_link_geom</geom>
-        <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="r_shoulder_lift_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>r_r_shoulder_lift_bumper</bumperTopicName>
-          <interface:bumper name="r_shoulder_lift_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <turnGravityOff>true</turnGravityOff>
-    </gazebo><gazebo reference="r_shoulder_lift_joint">
-      <stopKd value="1.0"/>
-      <stopKp value="1000000.0"/>
-    </gazebo><gazebo reference="r_upper_arm_roll_link">
-      <material value="PR2/RollLinks"/>
-      <turnGravityOff>true</turnGravityOff>
-    </gazebo><gazebo reference="r_upper_arm_roll_joint">
-      <stopKd value="1.0"/>
-      <stopKp value="1000000.0"/>
-      <fudgeFactor value="0.5"/>
-    </gazebo><transmission name="r_shoulder_pan_trans" type="SimpleTransmission">
-      <actuator name="r_shoulder_pan_motor"/>
-      <joint name="r_shoulder_pan_joint"/>
-      <mechanicalReduction>63.16</mechanicalReduction>
-    </transmission><transmission name="r_shoulder_lift_trans" type="SimpleTransmission">
-      <actuator name="r_shoulder_lift_motor"/>
-      <joint name="r_shoulder_lift_joint"/>
-      <mechanicalReduction>61.89</mechanicalReduction>
-    </transmission><transmission name="r_upper_arm_roll_trans" type="SimpleTransmission">
-      <actuator name="r_upper_arm_roll_motor"/>
-      <joint name="r_upper_arm_roll_joint"/>
-      <mechanicalReduction>32.65</mechanicalReduction>
-    </transmission>
-  <joint name="r_upper_arm_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <parent link="r_upper_arm_roll_link"/>
-      <child link="r_upper_arm_link"/>
-    </joint><link name="r_upper_arm_link">
-
-      <inertial>
-        <!-- NOTE:reflect==-1 for right side, reflect==1 for the left side -->
-        <mass value="6.01769"/>
-        <origin xyz="0.21398 -0.01621 -0.0002"/>
-        <inertia ixx="0.01537748957" ixy="0.00375711247" ixz="-0.00070852914" iyy="0.0747367044" iyz="-0.0001793645" izz="0.07608763307"/>
-      </inertial>
-
-      <visual name="{side}_upper_arm_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_upper_arm_visual_geom">
-          <mesh filename="package://pr2_description/meshes/upper_arm_v0/upper_arm.stl"/>
-        </geometry>
-
-        <material name="Green"/>
-      </visual>
-
-      <collision name="r_upper_arm_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_upper_arm_collision_geom">
-          <mesh filename="package://pr2_description/meshes/upper_arm_v0/convex/upper_arm_convex.stlb"/>
-        </geometry>
-      </collision>
-
-    </link><joint name="r_elbow_flex_joint" type="revolute">
-      <axis xyz="0 1 0"/>
-      <!-- Note: Overtravel limits are 140, -7 degrees instead of 133, 0 -->
-      <limit effort="30" lower="-2.3213" upper="0.00" velocity="3.3"/>
-      <safety_controller k_position="100" k_velocity="3" soft_lower_limit="-2.1213" soft_upper_limit="-0.15"/>
-      <calibration reference_position="-1.1606"/>
-      <dynamics damping="1.0"/>
-      <origin rpy="0 0 0" xyz="0.4 0 0"/>
-      <parent link="r_upper_arm_link"/>
-      <child link="r_elbow_flex_link"/>
-    </joint><link name="r_elbow_flex_link">
-
-
-      <inertial>
-        <mass value="1.90327"/>
-        <origin xyz="0.01014 0.00032 -0.01211"/>
-        <inertia ixx="0.00346541989" ixy="0.00004066825" ixz="0.00043171614" iyy="0.00441606455" iyz="-0.00003968914" izz="0.00359156824"/>
-      </inertial>
-
-      <visual name="r_elbow_flex_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_elbow_flex_visual_geom">
-          <mesh filename="package://pr2_description/meshes/upper_arm_v0/elbow_flex.stl"/>
-        </geometry>
-
-        <material name="Grey"/>
-      </visual>
-
-      <collision name="r_elbow_flex_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_elbow_flex_collision_geom">
-          <mesh filename="package://pr2_description/meshes/upper_arm_v0/convex/elbow_flex_convex.stlb"/>
-        </geometry>
-      </collision>
-    </link><joint name="r_forearm_roll_joint" type="continuous">
-      <axis xyz="-1 0 0"/>
-      <limit effort="30" velocity="3.6"/>
-      <safety_controller k_velocity="1"/>
-      <calibration reference_position="0.0"/>
-      <dynamics damping="0.1"/>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <parent link="r_elbow_flex_link"/>
-      <child link="r_forearm_roll_link"/>
-    </joint><link name="r_forearm_roll_link">
-      <inertial>
-        <!-- dummy masses, to be removed -->
-        <mass value="0.1"/>
-        <origin xyz="0 0 0"/>
-        <inertia ixx="0.01" ixy="0.00" ixz="0.00" iyy="0.01" iyz="0.00" izz="0.01"/>
-      </inertial>
-      <visual name="r_forearm_roll_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_forearm_roll_visual_geom">
-          <mesh filename="package://pr2_description/meshes/upper_arm_v0/forearm_roll.stl"/>
-        </geometry>
-
-        <material name="RollLinks"/>
-      </visual>
-      <!-- TODO: collision tag should be optional -->
-      <collision name="r_forearm_roll_collision">
-        <!-- dummy geom, to be removed -->
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_forearm_roll_collision_geom">
-          <box size="0.1 0.1 0.1"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="r_upper_arm_link">
-      <sensor:contact name="r_upper_arm_contact_sensor">
-        <geom>r_upper_arm_link_geom</geom><!-- TODO -->
-        <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="r_upper_arm_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>r_upper_arm_bumper</bumperTopicName>
-          <interface:bumper name="r_upper_arm_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <turnGravityOff>true</turnGravityOff>
-      <material value="PR2/Green"/>
-    </gazebo><gazebo reference="r_elbow_flex_joint">
-      <initial_joint_position>-1.0</initial_joint_position>
-    </gazebo><gazebo reference="r_elbow_flex_link">
-      <sensor:contact name="r_elbow_flex_contact_sensor">
-        <geom>r_elbow_flex_link_geom</geom>
-        <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="r_elbow_flex_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>r_elbow_flex_bumper</bumperTopicName>
-          <interface:bumper name="r_elbow_flex_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <material value="PR2/Grey"/>
-      <turnGravityOff>true</turnGravityOff>
-    </gazebo><gazebo reference="r_elbow_flex_joint">
-      <stopKd value="1.0"/>
-      <stopKp value="1000000.0"/>
-    </gazebo><gazebo reference="r_forearm_roll_link">
-      <material value="PR2/RollLinks"/>
-      <turnGravityOff>true</turnGravityOff>
-    </gazebo><gazebo reference="r_forearm_roll_joint">
-      <fudgeFactor value="0.5"/>
-    </gazebo><transmission name="r_elbow_flex_trans" type="SimpleTransmission">
-      <actuator name="r_elbow_flex_motor"/>
-      <joint name="r_elbow_flex_joint"/>
-      <mechanicalReduction>-36.17</mechanicalReduction>
-    </transmission><transmission name="r_forearm_roll_trans" type="SimpleTransmission">
-      <actuator name="r_forearm_roll_motor"/>
-      <joint name="r_forearm_roll_joint"/>
-      <mechanicalReduction>90.5142857143</mechanicalReduction>
-    </transmission>
-  <joint name="r_forearm_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <parent link="r_forearm_roll_link"/>
-      <child link="r_forearm_link"/>
-    </joint><link name="r_forearm_link">
-
-      <inertial>
-        <mass value="2.57968"/>
-        <origin xyz="0.18791 -0.00017 -0.00912"/>
-        <inertia ixx="0.00364857222" ixy="0.00005215877" ixz="0.00071534842" iyy="0.01507736897" iyz="-0.00001310770" izz="0.01659310749"/>
-      </inertial>
-
-      <visual name="r_forearm_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_forearm_visual_geom">
-          <mesh filename="package://pr2_description/meshes/forearm_v0/forearm.stl"/>
-        </geometry>
-
-        <material name="Blue"/>
-      </visual>
-
-      <collision name="r_forearm_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_forearm_collision_geom">
-          <mesh filename="package://pr2_description/meshes/forearm_v0/convex/forearm_convex.stlb"/>
-        </geometry>
-      </collision>
-    </link><joint name="r_wrist_flex_joint" type="revolute">
-      <axis xyz="0 -1 0"/>
-      <limit effort="10" lower="0.0" upper="2.094" velocity="3.078"/>
-      <safety_controller k_position="20" k_velocity="4" soft_lower_limit="0.15" soft_upper_limit="1.944"/>
-      <dynamics damping="0.1"/>
-      <calibration reference_position="0.5410521"/>
-      <origin rpy="0 0 0" xyz="0.32025 0 0"/>
-      <parent link="r_forearm_link"/>
-      <child link="r_wrist_flex_link"/>
-    </joint><link name="r_wrist_flex_link">
-
-      <inertial>
-        <mass value="0.61402"/>
-        <origin xyz="-0.00157 0.0 -0.00075"/>
-        <inertia ixx="0.00065165722" ixy="0.00000028864" ixz="0.00000303477" iyy="0.00019824443" iyz="-0.00000022645" izz="0.00064450498"/>
-      </inertial>
-
-      <visual name="r_wrist_flex_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_wrist_flex_visual_geom">
-          <mesh filename="package://pr2_description/meshes/forearm_v0/wrist_flex.stl"/>
-        </geometry>
-
-        <material name="Grey"/>
-      </visual>
-
-      <collision name="r_wrist_flex_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_wrist_flex_collision_geom">
-          <mesh filename="package://pr2_description/meshes/forearm_v0/convex/wrist_flex_convex.stlb"/>
-        </geometry>
-      </collision>
-    </link><joint name="r_wrist_roll_joint" type="continuous">
-      <axis xyz="1 0 0"/>
-      <limit effort="10" velocity="3.6"/>
-      <safety_controller k_velocity="2"/>
-      <dynamics damping="0.1"/>
-      <calibration reference_position="1.57079632679"/>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <parent link="r_wrist_flex_link"/>
-      <child link="r_wrist_roll_link"/>
-    </joint><link name="r_wrist_roll_link">
-      <inertial>
-        <!-- dummy masses, to be removed.  wrist roll masses are on "gripper_palm" -->
-        <mass value="0.1"/>
-        <origin xyz="0 0 0"/>
-        <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
-      </inertial>
-      <visual name="r_wrist_roll_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_wrist_roll_visual_geom">
-          <mesh filename="package://pr2_description/meshes/forearm_v0/wrist_roll.stl"/>
-        </geometry>
-
-        <material name="RollLinks"/>
-      </visual>
-      <collision name="r_wrist_roll_collision">
-        <origin rpy="0 0 0" xyz="0.0 0 0"/>
-        <geometry name="r_wrist_roll_collision_geom">
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-        <verbose value="Yes"/>
-      </collision>
-    </link><gazebo reference="r_forearm_link">
-      <turnGravityOff>true</turnGravityOff>
-      <sensor:contact name="r_forearm_contact_sensor">
-        <geom>r_forearm_link_geom</geom>
-        <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="r_forearm_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>r_forearm_bumper</bumperTopicName>
-          <interface:bumper name="r_forearm_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <material value="PR2/Blue"/>
-    </gazebo><gazebo reference="r_wrist_flex_link">
-      <turnGravityOff>true</turnGravityOff>
-      <sensor:contact name="r_wrist_flex_contact_sensor">
-        <geom>r_wrist_flex_link_geom</geom>
-        <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="r_wrist_flex_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>r_wrist_flex_bumper</bumperTopicName>
-          <interface:bumper name="r_wrist_flex_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <material value="PR2/Grey"/>
-    </gazebo><gazebo reference="r_wrist_flex_joint">
-      <stopKd value="1.0"/>
-      <stopKp value="1000000.0"/>
-    </gazebo><gazebo reference="r_wrist_roll_link">
-      <turnGravityOff>true</turnGravityOff>
-      <sensor:contact name="r_wrist_roll_contact_sensor">
-        <geom>r_wrist_roll_link_geom</geom>
-        <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="r_wrist_roll_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>r_wrist_roll_bumper</bumperTopicName>
-          <interface:bumper name="r_wrist_roll_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <material value="PR2/RollLinks"/>
-    </gazebo><gazebo reference="r_wrist_roll_joint">
-      <fudgeFactor value="0.5"/>
-    </gazebo><transmission name="r_wrist_trans" type="WristTransmission">
-      <rightActuator name="r_wrist_r_motor"/>
-      <leftActuator name="r_wrist_l_motor"/>
-      <flexJoint mechanicalReduction="60.1714285714" name="r_wrist_flex_joint"/>
-      <rollJoint mechanicalReduction="60.1714285714" name="r_wrist_roll_joint"/>
-    </transmission>
-
-  <joint name="r_gripper_palm_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <parent link="r_wrist_roll_link"/>
-      <child link="r_gripper_palm_link"/>
-    </joint><link name="r_gripper_palm_link">
-      <inertial>
-        <mass value="0.58007"/>
-        <origin rpy="0 0 0" xyz="0.06623 0.00053 -0.00119"/>
-        <inertia ixx="0.00035223921" ixy="-0.00001580476" ixz="-0.00000091750" iyy="0.00067741312" iyz="-0.00000059554" izz="0.00086563316"/>
-      </inertial>
-
-      <visual name="r_gripper_palm_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_gripper_palm_visual_geom">
-          <mesh filename="package://pr2_defs/meshes/gripper_palm.stl"/>
-        </geometry>
-        
-        <material name="Red"/>
-      </visual>
-      <collision name="r_gripper_palm_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_gripper_palm_collision_geom">
-          <mesh filename="package://pr2_defs/meshes/convex/gripper_palm_convex.stlb"/>
-        </geometry>
-        <verbose value="Yes"/>
-      </collision>
-    </link><joint name="r_gripper_motor_accelerometer_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <parent link="r_gripper_palm_link"/>
-      <child link="r_gripper_motor_accelerometer_link"/>
-    </joint><link name="r_gripper_motor_accelerometer_link">
-      <inertial>
-        <mass value="0.001"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
-      </inertial>
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.001 0.001 0.001"/>
-        </geometry>
-      </visual>
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.001 0.001 0.001"/>
-        </geometry>
-      </collision>
-    </link><joint name="r_gripper_tool_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="0.18 0 0"/>
-      <parent link="r_gripper_palm_link"/>
-      <child link="r_gripper_tool_frame"/>
-    </joint><link name="r_gripper_tool_frame">
-      <!-- TODO: all that follows is made-up and useless, but the old parser crashes without it -->
-      <inertial>
-        <mass value="0.01"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
-      </inertial>
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_gripper_tool_visual_geom">
-          <box size="0.001 0.001 0.001"/>
-        </geometry>
-        <material name="Grey"/>
-      </visual>
-      <collision>
-        <origin rpy="0 0 0" xyz="-0.1 0 0"/> <!-- move it out of the way -->
-        <geometry name="r_gripper_tool_collision_geom">
-          <box size="0.001 0.001 0.001"/>
-        </geometry>
-      </collision>
-    </link><joint name="r_gripper_l_finger_joint" type="revolute">
-      <axis xyz="0 0 1"/>
-
-      <limit effort="100.0" lower="0.0" upper="0.548" velocity="0.5"/><safety_controller k_position="100.0" k_velocity="100.0" soft_lower_limit="0.0" soft_upper_limit="0.548"/>
-
-      <dynamics damping="0.2"/>
-      <origin rpy="0 0 0" xyz="0.07691 0.01 0"/>
-      <parent link="r_gripper_palm_link"/>
-      <child link="r_gripper_l_finger_link"/>
-    </joint><link name="r_gripper_l_finger_link">
-
-
-      <inertial>
-        <mass value="0.17126"/>
-        <origin rpy="0 0 0" xyz="0.03598 0.01730 -0.00164"/>
-        <inertia ixx="0.00007756198" ixy="0.00000149095" ixz="-0.00000983385" iyy="0.00019708305" iyz="-0.00000306125" izz="0.00018105446"/>
-      </inertial>
-      <visual name="r_gripper_l_finger_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_gripper_l_finger_visual_geom">
-          <mesh filename="package://pr2_defs/meshes/upper_finger_l.stl"/>
-        </geometry>
-        
-        <material name="Grey"/>
-      </visual>
-      <collision name="r_gripper_l_finger_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_gripper_l_finger_collision_geom">
-          <mesh filename="package://pr2_defs/meshes/convex/upper_finger_l_convex.stlb"/>
-        </geometry>
-        <verbose value="Yes"/>
-      </collision>
-    </link><joint name="r_gripper_r_finger_joint" type="revolute">
-      <axis xyz="0 0 -1"/>
-      <origin rpy="0 0 0" xyz="0.07691 -0.01 0"/>
-      <limit effort="100.0" lower="0.0" upper="0.548" velocity="0.5"/><safety_controller k_position="100.0" k_velocity="100.0" soft_lower_limit="0.0" soft_upper_limit="0.548"/>
-      <dynamics damping="0.2"/>
-      <mimic joint="r_gripper_l_finger_joint" multiplier="1" offset="0"/>
-      <parent link="r_gripper_palm_link"/>
-      <child link="r_gripper_r_finger_link"/>
-    </joint><link name="r_gripper_r_finger_link">
-
-
-      <inertial>
-        <mass value="0.17389"/>
-        <origin rpy="0 0 0" xyz="0.03576 -0.01736 -0.00095"/>
-        <inertia ixx="0.00007738410" ixy="-0.00000209309" ixz="-0.00000836228" iyy="0.00019847383" iyz="0.00000246110" izz="0.00018106988"/>
-      </inertial>
-      <visual name="r_gripper_r_finger_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_gripper_r_finger_visual_geom">
-          <mesh filename="package://pr2_defs/meshes/upper_finger_r.stl"/>
-        </geometry>
-        
-        <material name="Grey"/>
-      </visual>
-      <collision name="r_gripper_r_finger_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_gripper_r_finger_collision_geom">
-          <mesh filename="package://pr2_defs/meshes/convex/upper_finger_r_convex.stlb"/>
-        </geometry>
-        <verbose value="Yes"/>
-      </collision>
-    </link><joint name="r_gripper_l_finger_tip_joint" type="revolute">
-      <axis xyz="0 0 -1"/>
-      <origin rpy="0 0 0" xyz="0.09137 0.00495 0"/>
-      <limit effort="100.0" lower="0.0" upper="0.548" velocity="0.5"/><safety_controller k_position="100.0" k_velocity="100.0" soft_lower_limit="0.0" soft_upper_limit="0.548"/>
-      <dynamics damping="0.01"/>
-      <mimic joint="r_gripper_l_finger_joint" multiplier="1" offset="0"/>
-      <parent link="r_gripper_l_finger_link"/>
-      <child link="r_gripper_l_finger_tip_link"/>
-    </joint><link name="r_gripper_l_finger_tip_link">
-
-
-      <inertial>
-        <mass value="0.04419"/>
-        <origin rpy="0 0 0" xyz="0.00423 0.00284 0.0"/>
-        <inertia ixx="0.00000837047" ixy="0.00000583632" ixz="0.0" iyy="0.00000987067" iyz="0.0" izz="0.00001541768"/>
-      </inertial>
-      <visual name="r_gripper_l_finger_tip_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_gripper_l_finger_tip_visual_geom">
-          <mesh filename="package://pr2_defs/meshes/finger_tip_l.stl"/>
-        </geometry>
-        
-        <material name="Green"/>
-      </visual>
-      <collision name="r_gripper_l_finger_tip_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_gripper_l_finger_tip_collision_geom">
-          <mesh filename="package://pr2_defs/meshes/finger_tip_pad2_l.stl"/>
-        </geometry>
-        <verbose value="Yes"/>
-      </collision>
-    </link><joint name="r_gripper_r_finger_tip_joint" type="revolute">
-      <axis xyz="0 0 1"/>
-      <origin rpy="0 0 0" xyz="0.09137 -0.00495 0"/>
-      <limit effort="100.0" lower="0.0" upper="0.548" velocity="0.5"/><safety_controller k_position="100.0" k_velocity="100.0" soft_lower_limit="0.0" soft_upper_limit="0.548"/>
-      <dynamics damping="0.01"/>
-      <mimic joint="r_gripper_l_finger_joint" multiplier="1" offset="0"/>
-      <parent link="r_gripper_r_finger_link"/>
-      <child link="r_gripper_r_finger_tip_link"/>
-    </joint><link name="r_gripper_r_finger_tip_link">
-
-
-      <inertial>
-        <mass value="0.04419"/>
-        <origin rpy="0 0 0" xyz="0.00423 -0.00284 0.0"/>
-        <inertia ixx="0.00000837047" ixy="-0.00000583632" ixz="0.0" iyy="0.00000987067" iyz="0.0" izz="0.00001541768"/>
-      </inertial>
-      <visual name="r_gripper_r_finger_tip_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_gripper_r_finger_tip_visual_geom">
-          <mesh filename="package://pr2_defs/meshes/finger_tip_r.stl"/>
-        </geometry>
-        
-        <material name="Green"/>
-      </visual>
-      <collision name="r_gripper_r_finger_tip_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_gripper_r_finger_tip_collision_geom">
-          <mesh filename="package://pr2_defs/meshes/finger_tip_pad2_r.stl"/>
-        </geometry>
-        <verbose value="Yes"/>
-      </collision>
-    </link><gazebo reference="r_gripper_l_finger_link">
-      <turnGravityOff>true</turnGravityOff>
-      <sensor:contact name="r_gripper_l_finger_contact_sensor">
-        <geom>r_gripper_l_finger_link_geom</geom>
-        <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="r_gripper_l_finger_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>r_gripper_l_finger_bumper</bumperTopicName>
-          <interface:bumper name="r_gripper_l_finger_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <mu1 value="500.0"/>
-      <mu2 value="500.0"/>
-      <kp value="1000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/Grey"/>
-      <!-- for "${prefix}_l_finger_joint"-->
-    </gazebo><gazebo reference="r_gripper_l_finger_joint">
-      <stopKd value="1.0"/>
-      <stopKp value="10000000.0"/>
-      <fudgeFactor value="1.0"/>
-      <provideFeedback value="true"/>
-    </gazebo><gazebo reference="r_gripper_r_finger_link">
-      <turnGravityOff>true</turnGravityOff>
-      <sensor:contact name="r_gripper_r_finger_contact_sensor">
-        <geom>r_gripper_r_finger_link_geom</geom>
-        <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="r_gripper_r_finger_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>r_gripper_r_finger_bumper</bumperTopicName>
-          <interface:bumper name="r_gripper_r_finger_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <mu1 value="500.0"/>
-      <mu2 value="500.0"/>
-      <kp value="1000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/Grey"/>
-    </gazebo><gazebo reference="r_gripper_r_finger_joint">
-      <stopKd value="1.0"/>
-      <stopKp value="10000000.0"/>
-      <fudgeFactor value="1.0"/>
-      <provideFeedback value="true"/>
-    </gazebo><gazebo reference="r_gripper_l_finger_tip_link">
-      <turnGravityOff>true</turnGravityOff>
-      <selfCollide>false</selfCollide>
-      <sensor:contact name="r_gripper_l_finger_tip_contact_sensor">
-        <geom>r_gripper_l_finger_tip_link_geom</geom>
-        <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="r_gripper_l_finger_tip_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>r_gripper_l_finger_tip_bumper</bumperTopicName>
-          <interface:bumper name="r_gripper_l_finger_tip_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <mu1 value="500.0"/>
-      <mu2 value="500.0"/>
-      <kp value="10000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/Red"/>
-    </gazebo><gazebo reference="r_gripper_l_finger_tip_joint">
-      <stopKd value="1.0"/>
-      <stopKp value="10000000.0"/>
-      <fudgeFactor value="1.0"/>
-      <provideFeedback value="true"/>
-    </gazebo><gazebo reference="r_gripper_r_finger_tip_link">
-      <turnGravityOff>true</turnGravityOff>
-      <selfCollide>false</selfCollide>
-      <sensor:contact name="r_gripper_r_finger_tip_contact_sensor">
-        <geom>r_gripper_r_finger_tip_link_geom</geom>
-        <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="r_gripper_r_finger_tip_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>r_gripper_r_finger_tip_bumper</bumperTopicName>
-          <interface:bumper name="r_gripper_r_finger_tip_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <mu1 value="500.0"/>
-      <mu2 value="500.0"/>
-      <kp value="10000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/Red"/>
-    </gazebo><gazebo>
-      <controller:gazebo_ros_p3d name="p3d_r_gripper_l_finger_controller" plugin="libgazebo_ros_p3d.so">
-        <alwaysOn>true</alwaysOn>
-        <updateRate>100.0</updateRate>
-        <bodyName>r_gripper_l_finger_link</bodyName>
-        <topicName>r_gripper_l_finger_pose_ground_truth</topicName>
-        <gaussianNoise>0.0</gaussianNoise>
-        <frameName>map</frameName>
-        <interface:position name="p3d_r_gripper_l_finger_position_iface"/>
-      </controller:gazebo_ros_p3d>
-      <controller:gazebo_ros_f3d name="f3d_r_gripper_l_finger_controller" plugin="libgazebo_ros_f3d.so">
-        <alwaysOn>true</alwaysOn>
-        <updateRate>100.0</updateRate>
-        <bodyName>r_gripper_l_finger_link</bodyName>
-        <topicName>r_gripper_l_finger_force_ground_truth</topicName>
-        <frameName>r_gripper_l_finger_link</frameName>
-        <interface:position name="f3d_r_gripper_l_finger_force_iface"/>
-      </controller:gazebo_ros_f3d>
-    </gazebo><gazebo reference="r_gripper_r_finger_tip_joint">
-      <stopKd value="1.0"/>
-      <stopKp value="10000000.0"/>
-      <fudgeFactor value="1.0"/>
-      <provideFeedback value="true"/>
-    </gazebo><joint name="r_gripper_joint" type="prismatic">
-      <axis xyz="0 0 1"/>
-      <limit effort="100.0" lower="0.0" upper="0.09" velocity="0.2"/>
-      <safety_controller k_position="20.0" k_velocity="500.0" soft_lower_limit="-0.01" soft_upper_limit="0.088"/>
-    </joint><gazebo reference="r_gripper_palm_link">
-      <turnGravityOff>true</turnGravityOff>
-      <sensor:contact name="r_gripper_palm_contact_sensor">
-        <geom>r_gripper_palm_link_geom</geom>
-        <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="r_gripper_palm_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>r_gripper_palm_bumper</bumperTopicName>
-          <interface:bumper name="r_gripper_palm_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <material value="PR2/Red"/>
-    </gazebo><gazebo reference="r_gripper_float_link">
-      <turnGravityOff>true</turnGravityOff>
-      <material value="PR2/White"/>
-    </gazebo><gazebo>
-      <!-- floating link for enforcing gripper constraints -->
-      <body:box name="r_gripper_float_link">
-          <massMatrix>true</massMatrix>
-          <mass>0.01</mass>
-          <ixx>0.0001</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.0001</iyy>
-          <iyz>0</iyz>
-          <izz>0.0001</izz>
-          <cx>0</cx>
-          <cy>0</cy>
-          <cz>0</cz>
-          <xyz>0.82025 0.188 0.790675</xyz>
-          <rpy>0 -0 0</rpy>
-          <geom:box name="r_gripper_float_link_geom">
-              <xyz>0 0 0</xyz>
-              <rpy>0 -0 0</rpy>
-              <size>0.0 0.0 0.0</size>
-              <visual>
-                  <xyz>0 0 0</xyz>
-                  <rpy>0 -0 0</rpy>
-                  <scale>0.0 0.0 0.0</scale>
-                  <mesh>unit_box</mesh>
-                  <material>PR2/White</material>
-              </visual>
-          </geom:box>
-          <turnGravityOff>true</turnGravityOff>
-          <selfCollide>false</selfCollide>
-      </body:box>
-      <joint:slider name="r_gripper_float_joint">
-          <body1>r_gripper_float_link</body1>
-          <body2>r_gripper_palm_link</body2>
-          <anchor>r_gripper_float_link</anchor>
-          <axis>1 0 0</axis>
-          <lowStop>-0.05</lowStop>
-          <highStop>0.001</highStop>
-      </joint:slider>
-      <joint:slider name="r_gripper_l_finger_tip_slider_joint">
-          <body1>r_gripper_l_finger_tip_link</body1>
-          <body2>r_gripper_float_link</body2>
-          <anchor>r_gripper_l_finger_tip_link</anchor>
-          <axis>0 1 0</axis>
-          <anchorOffset>0 0 0</anchorOffset>
-      </joint:slider>
-      <joint:slider name="r_gripper_r_finger_tip_slider_joint">
-          <body1>r_gripper_r_finger_tip_link</body1>
-          <body2>r_gripper_float_link</body2>
-          <anchor>r_gripper_r_finger_tip_link</anchor>
-          <axis>0 1 0</axis>
-          <anchorOffset>0 0 0</anchorOffset>
-      </joint:slider>
-      <!--
-      <joint:slider name="${side}_gripper_finger_tip_slider_joint">
-          <body1>${side}_gripper_l_finger_tip_link</body1>
-          <body2>${side}_gripper_r_finger_tip_link</body2>
-          <anchor>${side}_gripper_l_finger_tip_link</anchor>
-          <axis>0 1 0</axis>
-          <anchorOffset>0 0 0</anchorOffset>
-          <lowStop>0.0</lowStop>
-          <highStop>0.08</highStop>
-      </joint:slider>
-      -->
-    </gazebo><gazebo>
-      <controller:gazebo_ros_p3d name="p3d_r_gripper_palm_controller" plugin="libgazebo_ros_p3d.so">
-        <alwaysOn>true</alwaysOn>
-        <updateRate>100.0</updateRate>
-        <bodyName>r_gripper_palm_link</bodyName>
-        <topicName>r_gripper_palm_pose_ground_truth</topicName>
-        <xyzOffsets>0 0 0</xyzOffsets>
-        <rpyOffsets>0 0 0</rpyOffsets>
-        <gaussianNoise>0.0</gaussianNoise>
-        <frameName>map</frameName>
-        <interface:position name="p3d_r_gripper_palm_position_iface"/>
-      </controller:gazebo_ros_p3d>
-      <controller:gazebo_ros_p3d name="p3d_r_gripper_tool_frame" plugin="libgazebo_ros_p3d.so">
-        <alwaysOn>true</alwaysOn>
-        <updateRate>100.0</updateRate>
-        <bodyName>r_gripper_tool_frame</bodyName>
-        <topicName>r_gripper_tool_frame_pose_ground_truth</topicName>
-        <xyzOffsets>0 0 0</xyzOffsets>
-        <rpyOffsets>0 0 0</rpyOffsets>
-        <gaussianNoise>0.0</gaussianNoise>
-        <frameName>/map</frameName>
-        <interface:position name="p3d_r_gripper_tool_frame_position_iface"/>
-      </controller:gazebo_ros_p3d>
-    </gazebo><gazebo reference="r_gripper_tool_frame">
-      <turnGravityOff>true</turnGravityOff>
-      <material value="PR2/Grey"/>
-    </gazebo><transmission name="r_gripper_trans" type="PR2GripperTransmission">
-      <actuator name="r_gripper_motor"/>
-      <gap_joint L0="0.0375528" a="0.0683698" b="0.0433849" gear_ratio="40.095" h="0.0" mechanical_reduction="1.0" name="r_gripper_joint" phi0="0.518518122146" r="0.0915" screw_reduction="0.004" t0="-0.0001914" theta0="0.0628824676201"/>
-      <passive_joint name="r_gripper_l_finger_joint"/>
-      <passive_joint name="r_gripper_r_finger_joint"/>
-      <passive_joint name="r_gripper_r_finger_tip_joint"/>
-      <passive_joint name="r_gripper_l_finger_tip_joint"/>
-    </transmission>
-
-  <joint name="l_shoulder_pan_joint" type="revolute">
-      <axis xyz="0 0 1"/>
-      <origin rpy="0 0 0" xyz="0.0 0.188 0.0"/> <!-- transform from parent link to this joint frame -->
-      <parent link="torso_lift_link"/>
-      <child link="l_shoulder_pan_link"/>
-      <limit effort="30" lower="-0.714601836603" upper="2.2853981634" velocity="2.088"/>
-
-      <dynamics damping="10.0"/>
-
-      <safety_controller k_position="100" k_velocity="10" soft_lower_limit="-0.564601836603" soft_upper_limit="2.1353981634"/>
-
-      <!-- joint angle when the rising or the falling flag is activated on PR2 -->
-      <calibration reference_position="0.785398163397"/>
-
-    </joint><link name="l_shoulder_pan_link">
-
-
-      <inertial>
-        <mass value="25.799322"/>
-        <origin rpy="0 0 0" xyz="-0.001201 0.024513 -0.098231"/>
-        <inertia ixx="0.866179142480" ixy="-0.06086507933" ixz="-0.12118061183" iyy="0.87421714893" iyz="-0.05886609911" izz="0.27353821674"/>
-      </inertial>
-
-      <visual name="l_shoulder_pan_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_shoulder_pan_visual_geom">
-          <mesh filename="package://pr2_description/meshes/shoulder_v0/shoulder_yaw.stl"/>
-        </geometry>
-        <material name="Blue"/>
-      </visual>
-
-      <collision name="l_shoulder_pan_collision">
-        <origin rpy="0 0 0" xyz="0.0 0 0.0"/>
-        <geometry name="l_shoulder_pan_collision_geom">
-          <mesh filename="package://pr2_description/meshes/shoulder_v0/convex/shoulder_yaw_convex.stlb"/>
-        </geometry>
-      </collision>
-    </link><joint name="l_shoulder_lift_joint" type="revolute">
-      <axis xyz="0 1 0"/>
-
-      <!-- Limits updated from Function's CAD values as of 2009_02_24 (link_data.xls) -->
-      <limit effort="30" lower="-0.5236" upper="1.3963" velocity="2.082"/>
-
-      <safety_controller k_position="100" k_velocity="10" soft_lower_limit="-0.3536" soft_upper_limit="1.3463"/>
-      <calibration reference_position="0.0"/>
-      <dynamics damping="10.0"/>
-      <origin rpy="0 0 0" xyz="0.1 0 0"/>
-      <parent link="l_shoulder_pan_link"/>
-      <child link="l_shoulder_lift_link"/>
-    </joint><link name="l_shoulder_lift_link">
-
-      <inertial>
-        <mass value="2.74988"/>
-        <origin rpy="0 0 0" xyz="0.02195 -0.02664 -0.03127"/>
-        <inertia ixx="0.02105584615" ixy="0.00496704022" ixz="-0.00194808955" iyy="0.02127223737" iyz="0.00110425490" izz="0.01975753814"/>
-      </inertial>
-
-      <visual name="l_shoulder_lift_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_shoulder_lift_visual_geom">
-          <mesh filename="package://pr2_description/meshes/shoulder_v0/shoulder_lift.stl"/>
-        </geometry>
-        <material name="Grey"/>
-      </visual>
-
-      <collision name="l_shoulder_lift_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_shoulder_lift_collision_geom">
-          <mesh filename="package://pr2_description/meshes/shoulder_v0/convex/shoulder_lift_convex.stlb"/>
-        </geometry>
-      </collision>
-    </link><joint name="l_upper_arm_roll_joint" type="revolute">
-      <axis xyz="1 0 0"/>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <parent link="l_shoulder_lift_link"/>
-      <child link="l_upper_arm_roll_link"/>
-      <limit effort="30" lower="-0.8" upper="3.9" velocity="3.27"/>
-      <safety_controller k_position="100" k_velocity="2" soft_lower_limit="-0.65" soft_upper_limit="3.75"/>
-      <calibration reference_position="1.57079632679"/>
-      <dynamics damping="0.1"/>
-    </joint><link name="l_upper_arm_roll_link">
-
-
-      <inertial>
-        <!-- dummy mass, to be removed -->
-        <mass value="0.1"/>
-        <origin rpy="0 0 0" xyz="0.0 0 0"/>
-        <inertia ixx="0.01" ixy="0.00" ixz="0.00" iyy="0.01" iyz="0.00" izz="0.01"/>
-      </inertial>
-      <visual name="l_upper_arm_roll_visual">
-        <!-- TODO: This component doesn't actually have a mesh -->
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_upper_arm_roll_visual_geom">
-          <mesh filename="package://pr2_description/meshes/shoulder_v0/upper_arm_roll.stl"/>
-        </geometry>
-
-        <material name="RollLinks"/>
-      </visual>
-      <collision name="l_upper_arm_roll_collision">
-        <!-- TODO: collision tag should be optional -->
-        <origin rpy="0 0 0" xyz="0.0 0 0"/>
-        <geometry name="l_upper_arm_roll_collision_geom">
-          <box size="0.1 0.1 0.1"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="l_shoulder_pan_link">
-      <sensor:contact name="l_shoulder_pan_contact_sensor">
-        <geom>l_shoulder_pan_link_geom</geom>
-        <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="l_shoulder_pan_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>l_shoulder_pan_bumper</bumperTopicName>
-          <interface:bumper name="l_shoulder_pan_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <material value="PR2/Blue"/>
-      <turnGravityOff>true</turnGravityOff>
-    </gazebo><gazebo reference="l_shoulder_pan_joint">
-      <stopKd value="1.0"/>
-      <stopKp value="1000000.0"/>
-    </gazebo><gazebo reference="l_shoulder_lift_link">
-      <sensor:contact name="l_shoulder_lift_contact_sensor">
-        <geom>l_shoulder_lift_link_geom</geom>
-        <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="l_shoulder_lift_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>l_r_shoulder_lift_bumper</bumperTopicName>
-          <interface:bumper name="l_shoulder_lift_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <turnGravityOff>true</turnGravityOff>
-    </gazebo><gazebo reference="l_shoulder_lift_joint">
-      <stopKd value="1.0"/>
-      <stopKp value="1000000.0"/>
-    </gazebo><gazebo reference="l_upper_arm_roll_link">
-      <material value="PR2/RollLinks"/>
-      <turnGravityOff>true</turnGravityOff>
-    </gazebo><gazebo reference="l_upper_arm_roll_joint">
-      <stopKd value="1.0"/>
-      <stopKp value="1000000.0"/>
-      <fudgeFactor value="0.5"/>
-    </gazebo><transmission name="l_shoulder_pan_trans" type="SimpleTransmission">
-      <actuator name="l_shoulder_pan_motor"/>
-      <joint name="l_shoulder_pan_joint"/>
-      <mechanicalReduction>63.16</mechanicalReduction>
-    </transmission><transmission name="l_shoulder_lift_trans" type="SimpleTransmission">
-      <actuator name="l_shoulder_lift_motor"/>
-      <joint name="l_shoulder_lift_joint"/>
-      <mechanicalReduction>61.89</mechanicalReduction>
-    </transmission><transmission name="l_upper_arm_roll_trans" type="SimpleTransmission">
-      <actuator name="l_upper_arm_roll_motor"/>
-      <joint name="l_upper_arm_roll_joint"/>
-      <mechanicalReduction>32.65</mechanicalReduction>
-    </transmission>
-  <joint name="l_upper_arm_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <parent link="l_upper_arm_roll_link"/>
-      <child link="l_upper_arm_link"/>
-    </joint><link name="l_upper_arm_link">
-
-      <inertial>
-        <!-- NOTE:reflect==-1 for right side, reflect==1 for the left side -->
-        <mass value="6.01769"/>
-        <origin xyz="0.21405 0.01658 -0.00057"/>
-        <inertia ixx="0.01530603856" ixy="-0.00339324862" ixz="0.00060765455" iyy="0.07473694455" iyz="-0.00019953729" izz="0.07601594191"/>
-      </inertial>
-
-      <visual name="{side}_upper_arm_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_upper_arm_visual_geom">
-          <mesh filename="package://pr2_description/meshes/upper_arm_v0/upper_arm.stl"/>
-        </geometry>
-
-        <material name="Green"/>
-      </visual>
-
-      <collision name="l_upper_arm_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_upper_arm_collision_geom">
-          <mesh filename="package://pr2_description/meshes/upper_arm_v0/convex/upper_arm_convex.stlb"/>
-        </geometry>
-      </collision>
-
-    </link><joint name="l_elbow_flex_joint" type="revolute">
-      <axis xyz="0 1 0"/>
-      <!-- Note: Overtravel limits are 140, -7 degrees instead of 133, 0 -->
-      <limit effort="30" lower="-2.3213" upper="0.00" velocity="3.3"/>
-      <safety_controller k_position="100" k_velocity="3" soft_lower_limit="-2.1213" soft_upper_limit="-0.15"/>
-      <calibration reference_position="-1.1606"/>
-      <dynamics damping="1.0"/>
-      <origin rpy="0 0 0" xyz="0.4 0 0"/>
-      <parent link="l_upper_arm_link"/>
-      <child link="l_elbow_flex_link"/>
-    </joint><link name="l_elbow_flex_link">
-
-
-      <inertial>
-        <mass value="1.90327"/>
-        <origin xyz="0.01014 0.00032 -0.01211"/>
-        <inertia ixx="0.00346541989" ixy="0.00004066825" ixz="0.00043171614" iyy="0.00441606455" iyz="-0.00003968914" izz="0.00359156824"/>
-      </inertial>
-
-      <visual name="l_elbow_flex_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_elbow_flex_visual_geom">
-          <mesh filename="package://pr2_description/meshes/upper_arm_v0/elbow_flex.stl"/>
-        </geometry>
-
-        <material name="Grey"/>
-      </visual>
-
-      <collision name="l_elbow_flex_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_elbow_flex_collision_geom">
-          <mesh filename="package://pr2_description/meshes/upper_arm_v0/convex/elbow_flex_convex.stlb"/>
-        </geometry>
-      </collision>
-    </link><joint name="l_forearm_roll_joint" type="continuous">
-      <axis xyz="-1 0 0"/>
-      <limit effort="30" velocity="3.6"/>
-      <safety_controller k_velocity="1"/>
-      <calibration reference_position="0.0"/>
-      <dynamics damping="0.1"/>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <parent link="l_elbow_flex_link"/>
-      <child link="l_forearm_roll_link"/>
-    </joint><link name="l_forearm_roll_link">
-      <inertial>
-        <!-- dummy masses, to be removed -->
-        <mass value="0.1"/>
-        <origin xyz="0 0 0"/>
-        <inertia ixx="0.01" ixy="0.00" ixz="0.00" iyy="0.01" iyz="0.00" izz="0.01"/>
-      </inertial>
-      <visual name="l_forearm_roll_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_forearm_roll_visual_geom">
-          <mesh filename="package://pr2_description/meshes/upper_arm_v0/forearm_roll.stl"/>
-        </geometry>
-
-        <material name="RollLinks"/>
-      </visual>
-      <!-- TODO: collision tag should be optional -->
-      <collision name="l_forearm_roll_collision">
-        <!-- dummy geom, to be removed -->
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_forearm_roll_collision_geom">
-          <box size="0.1 0.1 0.1"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="l_upper_arm_link">
-      <sensor:contact name="l_upper_arm_contact_sensor">
-        <geom>l_upper_arm_link_geom</geom><!-- TODO -->
-        <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="l_upper_arm_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>l_upper_arm_bumper</bumperTopicName>
-          <interface:bumper name="l_upper_arm_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <turnGravityOff>true</turnGravityOff>
-      <material value="PR2/Green"/>
-    </gazebo><gazebo reference="l_elbow_flex_joint">
-      <initial_joint_position>-1.0</initial_joint_position>
-    </gazebo><gazebo reference="l_elbow_flex_link">
-      <sensor:contact name="l_elbow_flex_contact_sensor">
-        <geom>l_elbow_flex_link_geom</geom>
-        <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="l_elbow_flex_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>l_elbow_flex_bumper</bumperTopicName>
-          <interface:bumper name="l_elbow_flex_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <material value="PR2/Grey"/>
-      <turnGravityOff>true</turnGravityOff>
-    </gazebo><gazebo reference="l_elbow_flex_joint">
-      <stopKd value="1.0"/>
-      <stopKp value="1000000.0"/>
-    </gazebo><gazebo reference="l_forearm_roll_link">
-      <material value="PR2/RollLinks"/>
-      <turnGravityOff>true</turnGravityOff>
-    </gazebo><gazebo reference="l_forearm_roll_joint">
-      <fudgeFactor value="0.5"/>
-    </gazebo><transmission name="l_elbow_flex_trans" type="SimpleTransmission">
-      <actuator name="l_elbow_flex_motor"/>
-      <joint name="l_elbow_flex_joint"/>
-      <mechanicalReduction>-36.17</mechanicalReduction>
-    </transmission><transmission name="l_forearm_roll_trans" type="SimpleTransmission">
-      <actuator name="l_forearm_roll_motor"/>
-      <joint name="l_forearm_roll_joint"/>
-      <mechanicalReduction>90.5142857143</mechanicalReduction>
-    </transmission>
-  <joint name="l_forearm_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <parent link="l_forearm_roll_link"/>
-      <child link="l_forearm_link"/>
-    </joint><link name="l_forearm_link">
-
-      <inertial>
-        <mass value="2.57968"/>
-        <origin xyz="0.18791 -0.00017 -0.00912"/>
-        <inertia ixx="0.00364857222" ixy="0.00005215877" ixz="0.00071534842" iyy="0.01507736897" iyz="-0.00001310770" izz="0.01659310749"/>
-      </inertial>
-
-      <visual name="l_forearm_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_forearm_visual_geom">
-          <mesh filename="package://pr2_description/meshes/forearm_v0/forearm.stl"/>
-        </geometry>
-
-        <material name="Blue"/>
-      </visual>
-
-      <collision name="l_forearm_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_forearm_collision_geom">
-          <mesh filename="package://pr2_description/meshes/forearm_v0/convex/forearm_convex.stlb"/>
-        </geometry>
-      </collision>
-    </link><joint name="l_wrist_flex_joint" type="revolute">
-      <axis xyz="0 -1 0"/>
-      <limit effort="10" lower="0.0" upper="2.094" velocity="3.078"/>
-      <safety_controller k_position="20" k_velocity="4" soft_lower_limit="0.15" soft_upper_limit="1.944"/>
-      <dynamics damping="0.1"/>
-      <calibration reference_position="0.5410521"/>
-      <origin rpy="0 0 0" xyz="0.32025 0 0"/>
-      <parent link="l_forearm_link"/>
-      <child link="l_wrist_flex_link"/>
-    </joint><link name="l_wrist_flex_link">
-
-      <inertial>
-        <mass value="0.61402"/>
-        <origin xyz="-0.00157 0.0 -0.00075"/>
-        <inertia ixx="0.00065165722" ixy="0.00000028864" ixz="0.00000303477" iyy="0.00019824443" iyz="-0.00000022645" izz="0.00064450498"/>
-      </inertial>
-
-      <visual name="l_wrist_flex_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_wrist_flex_visual_geom">
-          <mesh filename="package://pr2_description/meshes/forearm_v0/wrist_flex.stl"/>
-        </geometry>
-
-        <material name="Grey"/>
-      </visual>
-
-      <collision name="l_wrist_flex_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_wrist_flex_collision_geom">
-          <mesh filename="package://pr2_description/meshes/forearm_v0/convex/wrist_flex_convex.stlb"/>
-        </geometry>
-      </collision>
-    </link><joint name="l_wrist_roll_joint" type="continuous">
-      <axis xyz="1 0 0"/>
-      <limit effort="10" velocity="3.6"/>
-      <safety_controller k_velocity="2"/>
-      <dynamics damping="0.1"/>
-      <calibration reference_position="1.57079632679"/>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <parent link="l_wrist_flex_link"/>
-      <child link="l_wrist_roll_link"/>
-    </joint><link name="l_wrist_roll_link">
-      <inertial>
-        <!-- dummy masses, to be removed.  wrist roll masses are on "gripper_palm" -->
-        <mass value="0.1"/>
-        <origin xyz="0 0 0"/>
-        <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
-      </inertial>
-      <visual name="l_wrist_roll_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_wrist_roll_visual_geom">
-          <mesh filename="package://pr2_description/meshes/forearm_v0/wrist_roll.stl"/>
-        </geometry>
-
-        <material name="RollLinks"/>
-      </visual>
-      <collision name="l_wrist_roll_collision">
-        <origin rpy="0 0 0" xyz="0.0 0 0"/>
-        <geometry name="l_wrist_roll_collision_geom">
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-        <verbose value="Yes"/>
-      </collision>
-    </link><gazebo reference="l_forearm_link">
-      <turnGravityOff>true</turnGravityOff>
-      <sensor:contact name="l_forearm_contact_sensor">
-        <geom>l_forearm_link_geom</geom>
-        <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="l_forearm_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>l_forearm_bumper</bumperTopicName>
-          <interface:bumper name="l_forearm_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <material value="PR2/Blue"/>
-    </gazebo><gazebo reference="l_wrist_flex_link">
-      <turnGravityOff>true</turnGravityOff>
-      <sensor:contact name="l_wrist_flex_contact_sensor">
-        <geom>l_wrist_flex_link_geom</geom>
-        <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="l_wrist_flex_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>l_wrist_flex_bumper</bumperTopicName>
-          <interface:bumper name="l_wrist_flex_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <material value="PR2/Grey"/>
-    </gazebo><gazebo reference="l_wrist_flex_joint">
-      <stopKd value="1.0"/>
-      <stopKp value="1000000.0"/>
-    </gazebo><gazebo reference="l_wrist_roll_link">
-      <turnGravityOff>true</turnGravityOff>
-      <sensor:contact name="l_wrist_roll_contact_sensor">
-        <geom>l_wrist_roll_link_geom</geom>
-        <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="l_wrist_roll_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>l_wrist_roll_bumper</bumperTopicName>
-          <interface:bumper name="l_wrist_roll_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <material value="PR2/RollLinks"/>
-    </gazebo><gazebo reference="l_wrist_roll_joint">
-      <fudgeFactor value="0.5"/>
-    </gazebo><transmission name="l_wrist_trans" type="WristTransmission">
-      <rightActuator name="l_wrist_r_motor"/>
-      <leftActuator name="l_wrist_l_motor"/>
-      <flexJoint mechanicalReduction="60.1714285714" name="l_wrist_flex_joint"/>
-      <rollJoint mechanicalReduction="60.1714285714" name="l_wrist_roll_joint"/>
-    </transmission>
-
-  <joint name="l_gripper_palm_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <parent link="l_wrist_roll_link"/>
-      <child link="l_gripper_palm_link"/>
-    </joint><link name="l_gripper_palm_link">
-      <inertial>
-        <mass value="0.58007"/>
-        <origin rpy="0 0 0" xyz="0.06623 0.00053 -0.00119"/>
-        <inertia ixx="0.00035223921" ixy="-0.00001580476" ixz="-0.00000091750" iyy="0.00067741312" iyz="-0.00000059554" izz="0.00086563316"/>
-      </inertial>
-
-      <visual name="l_gripper_palm_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_gripper_palm_visual_geom">
-          <mesh filename="package://pr2_defs/meshes/gripper_palm.stl"/>
-        </geometry>
-        
-        <material name="Red"/>
-      </visual>
-      <collision name="l_gripper_palm_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_gripper_palm_collision_geom">
-          <mesh filename="package://pr2_defs/meshes/convex/gripper_palm_convex.stlb"/>
-        </geometry>
-        <verbose value="Yes"/>
-      </collision>
-    </link><joint name="l_gripper_motor_accelerometer_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <parent link="l_gripper_palm_link"/>
-      <child link="l_gripper_motor_accelerometer_link"/>
-    </joint><link name="l_gripper_motor_accelerometer_link">
-      <inertial>
-        <mass value="0.001"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
-      </inertial>
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.001 0.001 0.001"/>
-        </geometry>
-      </visual>
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.001 0.001 0.001"/>
-        </geometry>
-      </collision>
-    </link><joint name="l_gripper_tool_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="0.18 0 0"/>
-      <parent link="l_gripper_palm_link"/>
-      <child link="l_gripper_tool_frame"/>
-    </joint><link name="l_gripper_tool_frame">
-      <!-- TODO: all that follows is made-up and useless, but the old parser crashes without it -->
-      <inertial>
-        <mass value="0.01"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
-      </inertial>
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_gripper_tool_visual_geom">
-          <box size="0.001 0.001 0.001"/>
-        </geometry>
-        <material name="Grey"/>
-      </visual>
-      <collision>
-        <origin rpy="0 0 0" xyz="-0.1 0 0"/> <!-- move it out of the way -->
-        <geometry name="l_gripper_tool_collision_geom">
-          <box size="0.001 0.001 0.001"/>
-        </geometry>
-      </collision>
-    </link><joint name="l_gripper_l_finger_joint" type="revolute">
-      <axis xyz="0 0 1"/>
-
-      <limit effort="100.0" lower="0.0" upper="0.548" velocity="0.5"/><safety_controller k_position="100.0" k_velocity="100.0" soft_lower_limit="0.0" soft_upper_limit="0.548"/>
-
-      <dynamics damping="0.2"/>
-      <origin rpy="0 0 0" xyz="0.07691 0.01 0"/>
-      <parent link="l_gripper_palm_link"/>
-      <child link="l_gripper_l_finger_link"/>
-    </joint><link name="l_gripper_l_finger_link">
-
-
-      <inertial>
-        <mass value="0.17126"/>
-        <origin rpy="0 0 0" xyz="0.03598 0.01730 -0.00164"/>
-        <inertia ixx="0.00007756198" ixy="0.00000149095" ixz="-0.00000983385" iyy="0.00019708305" iyz="-0.00000306125" izz="0.00018105446"/>
-      </inertial>
-      <visual name="l_gripper_l_finger_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_gripper_l_finger_visual_geom">
-          <mesh filename="package://pr2_defs/meshes/upper_finger_l.stl"/>
-        </geometry>
-        
-        <material name="Grey"/>
-      </visual>
-      <collision name="l_gripper_l_finger_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_gripper_l_finger_collision_geom">
-          <mesh filename="package://pr2_defs/meshes/convex/upper_finger_l_convex.stlb"/>
-        </geometry>
-        <verbose value="Yes"/>
-      </collision>
-    </link><joint name="l_gripper_r_finger_joint" type="revolute">
-      <axis xyz="0 0 -1"/>
-      <origin rpy="0 0 0" xyz="0.07691 -0.01 0"/>
-      <limit effort="100.0" lower="0.0" upper="0.548" velocity="0.5"/><safety_controller k_position="100.0" k_velocity="100.0" soft_lower_limit="0.0" soft_upper_limit="0.548"/>
-      <dynamics damping="0.2"/>
-      <mimic joint="l_gripper_l_finger_joint" multiplier="1" offset="0"/>
-      <parent link="l_gripper_palm_link"/>
-      <child link="l_gripper_r_finger_link"/>
-    </joint><link name="l_gripper_r_finger_link">
-
-
-      <inertial>
-        <mass value="0.17389"/>
-        <origin rpy="0 0 0" xyz="0.03576 -0.01736 -0.00095"/>
-        <inertia ixx="0.00007738410" ixy="-0.00000209309" ixz="-0.00000836228" iyy="0.00019847383" iyz="0.00000246110" izz="0.00018106988"/>
-      </inertial>
-      <visual name="l_gripper_r_finger_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_gripper_r_finger_visual_geom">
-          <mesh filename="package://pr2_defs/meshes/upper_finger_r.stl"/>
-        </geometry>
-        
-        <material name="Grey"/>
-      </visual>
-      <collision name="l_gripper_r_finger_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_gripper_r_finger_collision_geom">
-          <mesh filename="package://pr2_defs/meshes/convex/upper_finger_r_convex.stlb"/>
-        </geometry>
-        <verbose value="Yes"/>
-      </collision>
-    </link><joint name="l_gripper_l_finger_tip_joint" type="revolute">
-      <axis xyz="0 0 -1"/>
-      <origin rpy="0 0 0" xyz="0.09137 0.00495 0"/>
-      <limit effort="100.0" lower="0.0" upper="0.548" velocity="0.5"/><safety_controller k_position="100.0" k_velocity="100.0" soft_lower_limit="0.0" soft_upper_limit="0.548"/>
-      <dynamics damping="0.01"/>
-      <mimic joint="l_gripper_l_finger_joint" multiplier="1" offset="0"/>
-      <parent link="l_gripper_l_finger_link"/>
-      <child link="l_gripper_l_finger_tip_link"/>
-    </joint><link name="l_gripper_l_finger_tip_link">
-
-
-      <inertial>
-        <mass value="0.04419"/>
-        <origin rpy="0 0 0" xyz="0.00423 0.00284 0.0"/>
-        <inertia ixx="0.00000837047" ixy="0.00000583632" ixz="0.0" iyy="0.00000987067" iyz="0.0" izz="0.00001541768"/>
-      </inertial>
-      <visual name="l_gripper_l_finger_tip_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_gripper_l_finger_tip_visual_geom">
-          <mesh filename="package://pr2_defs/meshes/finger_tip_l.stl"/>
-        </geometry>
-        
-        <material name="Green"/>
-      </visual>
-      <collision name="l_gripper_l_finger_tip_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_gripper_l_finger_tip_collision_geom">
-          <mesh filename="package://pr2_defs/meshes/finger_tip_pad2_l.stl"/>
-        </geometry>
-        <verbose value="Yes"/>
-      </collision>
-    </link><joint name="l_gripper_r_finger_tip_joint" type="revolute">
-      <axis xyz="0 0 1"/>
-      <origin rpy="0 0 0" xyz="0.09137 -0.00495 0"/>
-      <limit effort="100.0" lower="0.0" upper="0.548" velocity="0.5"/><safety_controller k_position="100.0" k_velocity="100.0" soft_lower_limit="0.0" soft_upper_limit="0.548"/>
-      <dynamics damping="0.01"/>
-      <mimic joint="l_gripper_l_finger_joint" multiplier="1" offset="0"/>
-      <parent link="l_gripper_r_finger_link"/>
-      <child link="l_gripper_r_finger_tip_link"/>
-    </joint><link name="l_gripper_r_finger_tip_link">
-
-
-      <inertial>
-        <mass value="0.04419"/>
-        <origin rpy="0 0 0" xyz="0.00423 -0.00284 0.0"/>
-        <inertia ixx="0.00000837047" ixy="-0.00000583632" ixz="0.0" iyy="0.00000987067" iyz="0.0" izz="0.00001541768"/>
-      </inertial>
-      <visual name="l_gripper_r_finger_tip_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_gripper_r_finger_tip_visual_geom">
-          <mesh filename="package://pr2_defs/meshes/finger_tip_r.stl"/>
-        </geometry>
-        
-        <material name="Green"/>
-      </visual>
-      <collision name="l_gripper_r_finger_tip_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_gripper_r_finger_tip_collision_geom">
-          <mesh filename="package://pr2_defs/meshes/finger_tip_pad2_r.stl"/>
-        </geometry>
-        <verbose value="Yes"/>
-      </collision>
-    </link><gazebo reference="l_gripper_l_finger_link">
-      <turnGravityOff>true</turnGravityOff>
-      <sensor:contact name="l_gripper_l_finger_contact_sensor">
-        <geom>l_gripper_l_finger_link_geom</geom>
-        <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="l_gripper_l_finger_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>l_gripper_l_finger_bumper</bumperTopicName>
-          <interface:bumper name="l_gripper_l_finger_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <mu1 value="500.0"/>
-      <mu2 value="500.0"/>
-      <kp value="1000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/Grey"/>
-      <!-- for "${prefix}_l_finger_joint"-->
-    </gazebo><gazebo reference="l_gripper_l_finger_joint">
-      <stopKd value="1.0"/>
-      <stopKp value="10000000.0"/>
-      <fudgeFactor value="1.0"/>
-      <provideFeedback value="true"/>
-    </gazebo><gazebo reference="l_gripper_r_finger_link">
-      <turnGravityOff>true</turnGravityOff>
-      <sensor:contact name="l_gripper_r_finger_contact_sensor">
-        <geom>l_gripper_r_finger_link_geom</geom>
-        <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="l_gripper_r_finger_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>l_gripper_r_finger_bumper</bumperTopicName>
-          <interface:bumper name="l_gripper_r_finger_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <mu1 value="500.0"/>
-      <mu2 value="500.0"/>
-      <kp value="1000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/Grey"/>
-    </gazebo><gazebo reference="l_gripper_r_finger_joint">
-      <stopKd value="1.0"/>
-      <stopKp value="10000000.0"/>
-      <fudgeFactor value="1.0"/>
-      <provideFeedback value="true"/>
-    </gazebo><gazebo reference="l_gripper_l_finger_tip_link">
-      <turnGravityOff>true</turnGravityOff>
-      <selfCollide>false</selfCollide>
-      <sensor:contact name="l_gripper_l_finger_tip_contact_sensor">
-        <geom>l_gripper_l_finger_tip_link_geom</geom>
-        <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="l_gripper_l_finger_tip_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>l_gripper_l_finger_tip_bumper</bumperTopicName>
-          <interface:bumper name="l_gripper_l_finger_tip_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <mu1 value="500.0"/>
-      <mu2 value="500.0"/>
-      <kp value="10000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/Red"/>
-    </gazebo><gazebo reference="l_gripper_l_finger_tip_joint">
-      <stopKd value="1.0"/>
-      <stopKp value="10000000.0"/>
-      <fudgeFactor value="1.0"/>
-      <provideFeedback value="true"/>
-    </gazebo><gazebo reference="l_gripper_r_finger_tip_link">
-      <turnGravityOff>true</turnGravityOff>
-      <selfCollide>false</selfCollide>
-      <sensor:contact name="l_gripper_r_finger_tip_contact_sensor">
-        <geom>l_gripper_r_finger_tip_link_geom</geom>
-        <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="l_gripper_r_finger_tip_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>l_gripper_r_finger_tip_bumper</bumperTopicName>
-          <interface:bumper name="l_gripper_r_finger_tip_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <mu1 value="500.0"/>
-      <mu2 value="500.0"/>
-      <kp value="10000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/Red"/>
-    </gazebo><gazebo>
-      <controller:gazebo_ros_p3d name="p3d_l_gripper_l_finger_controller" plugin="libgazebo_ros_p3d.so">
-        <alwaysOn>true</alwaysOn>
-        <updateRate>100.0</updateRate>
-        <bodyName>l_gripper_l_finger_link</bodyName>
-        <topicName>l_gripper_l_finger_pose_ground_truth</topicName>
-        <gaussianNoise>0.0</gaussianNoise>
-        <frameName>map</frameName>
-        <interface:position name="p3d_l_gripper_l_finger_position_iface"/>
-      </controller:gazebo_ros_p3d>
-      <controller:gazebo_ros_f3d name="f3d_l_gripper_l_finger_controller" plugin="libgazebo_ros_f3d.so">
-        <alwaysOn>true</alwaysOn>
-        <updateRate>100.0</updateRate>
-        <bodyName>l_gripper_l_finger_link</bodyName>
-        <topicName>l_gripper_l_finger_force_ground_truth</topicName>
-        <frameName>l_gripper_l_finger_link</frameName>
-        <interface:position name="f3d_l_gripper_l_finger_force_iface"/>
-      </controller:gazebo_ros_f3d>
-    </gazebo><gazebo reference="l_gripper_r_finger_tip_joint">
-      <stopKd value="1.0"/>
-      <stopKp value="10000000.0"/>
-      <fudgeFactor value="1.0"/>
-      <provideFeedback value="true"/>
-    </gazebo><joint name="l_gripper_joint" type="prismatic">
-      <axis xyz="0 0 1"/>
-      <limit effort="100.0" lower="0.0" upper="0.09" velocity="0.2"/>
-      <safety_controller k_position="20.0" k_velocity="500.0" soft_lower_limit="-0.01" soft_upper_limit="0.088"/>
-    </joint><gazebo reference="l_gripper_palm_link">
-      <turnGravityOff>true</turnGravityOff>
-      <sensor:contact name="l_gripper_palm_contact_sensor">
-        <geom>l_gripper_palm_link_geom</geom>
-        <updateRate>100.0</updateRate>
-        <controller:gazebo_ros_bumper name="l_gripper_palm_gazebo_ros_bumper_controller" plugin="libgazebo_ros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>l_gripper_palm_bumper</bumperTopicName>
-          <interface:bumper name="l_gripper_palm_gazebo_ros_bumper_iface"/>
-        </controller:gazebo_ros_bumper>
-      </sensor:contact>
-      <material value="PR2/Red"/>
-    </gazebo><gazebo reference="l_gripper_float_link">
-      <turnGravityOff>true</turnGravityOff>
-      <material value="PR2/White"/>
-    </gazebo><gazebo>
-      <!-- floating link for enforcing gripper constraints -->
-      <body:box name="l_gripper_float_link">
-          <massMatrix>true</massMatrix>
-          <mass>0.01</mass>
-          <ixx>0.0001</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.0001</iyy>
-          <iyz>0</iyz>
-          <izz>0.0001</izz>
-          <cx>0</cx>
-          <cy>0</cy>
-          <cz>0</cz>
-          <xyz>0.82025 0.188 0.790675</xyz>
-          <rpy>0 -0 0</rpy>
-          <geom:box name="l_gripper_float_link_geom">
-              <xyz>0 0 0</xyz>
-              <rpy>0 -0 0</rpy>
-              <size>0.0 0.0 0.0</size>
-              <visual>
-                  <xyz>0 0 0</xyz>
-                  <rpy>0 -0 0</rpy>
-                  <scale>0.0 0.0 0.0</scale>
-                  <mesh>unit_box</mesh>
-                  <material>PR2/White</material>
-              </visual>
-          </geom:box>
-          <turnGravityOff>true</turnGravityOff>
-          <selfCollide>false</selfCollide>
-      </body:box>
-      <joint:slider name="l_gripper_float_joint">
-          <body1>l_gripper_float_link</body1>
-          <body2>l_gripper_palm_link</body2>
-          <anchor>l_gripper_float_link</anchor>
-          <axis>1 0 0</axis>
-          <lowStop>-0.05</lowStop>
-          <highStop>0.001</highStop>
-      </joint:slider>
-      <joint:slider name="l_gripper_l_finger_tip_slider_joint">
-          <body1>l_gripper_l_finger_tip_link</body1>
-          <body2>l_gripper_float_link</body2>
-          <anchor>l_gripper_l_finger_tip_link</anchor>
-          <axis>0 1 0</axis>
-          <anchorOffset>0 0 0</anchorOffset>
-      </joint:slider>
-      <joint:slider name="l_gripper_r_finger_tip_slider_joint">
-          <body1>l_gripper_r_finger_tip_link</body1>
-          <body2>l_gripper_float_link</body2>
-          <anchor>l_gripper_r_finger_tip_link</anchor>
-          <axis>0 1 0</axis>
-          <anchorOffset>0 0 0</anchorOffset>
-      </joint:slider>
-      <!--
-      <joint:slider name="${side}_gripper_finger_tip_slider_joint">
-          <body1>${side}_gripper_l_finger_tip_link</body1>
-          <body2>${side}_gripper_r_finger_tip_link</body2>
-          <anchor>${side}_gripper_l_finger_tip_link</anchor>
-          <axis>0 1 0</axis>
-          <anchorOffset>0 0 0</anchorOffset>
-          <lowStop>0.0</lowStop>
-          <highStop>0.08</highStop>
-      </joint:slider>
-      -->
-    </gazebo><gazebo>
-      <controller:gazebo_ros_p3d name="p3d_l_gripper_palm_controller" plugin="libgazebo_ros_p3d.so">
-        <alwaysOn>true</alwaysOn>
-        <updateRate>100.0</updateRate>
-        <bodyName>l_gripper_palm_link</bodyName>
-        <topicName>l_gripper_palm_pose_ground_truth</topicName>
-        <xyzOffsets>0 0 0</xyzOffsets>
-        <rpyOffsets>0 0 0</rpyOffsets>
-        <gaussianNoise>0.0</gaussianNoise>
-        <frameName>map</frameName>
-        <interface:position name="p3d_l_gripper_palm_position_iface"/>
-      </controller:gazebo_ros_p3d>
-      <controller:gazebo_ros_p3d name="p3d_l_gripper_tool_frame" plugin="libgazebo_ros_p3d.so">
-        <alwaysOn>true</alwaysOn>
-        <updateRate>100.0</updateRate>
-        <bodyName>l_gripper_tool_frame</bodyName>
-        <topicName>l_gripper_tool_frame_pose_ground_truth</topicName>
-        <xyzOffsets>0 0 0</xyzOffsets>
-        <rpyOffsets>0 0 0</rpyOffsets>
-        <gaussianNoise>0.0</gaussianNoise>
-        <frameName>/map</frameName>
-        <interface:position name="p3d_l_gripper_tool_frame_position_iface"/>
-      </controller:gazebo_ros_p3d>
-    </gazebo><gazebo reference="l_gripper_tool_frame">
-      <turnGravityOff>true</turnGravityOff>
-      <material value="PR2/Grey"/>
-    </gazebo><transmission name="l_gripper_trans" type="PR2GripperTransmission">
-      <actuator name="l_gripper_motor"/>
-      <gap_joint L0="0.0375528" a="0.0683698" b="0.0433849" gear_ratio="40.095" h="0.0" mechanical_reduction="1.0" name="l_gripper_joint" phi0="0.518518122146" r="0.0915" screw_reduction="0.004" t0="-0.0001914" theta0="0.0628824676201"/>
-      <passive_joint name="l_gripper_l_finger_joint"/>
-      <passive_joint name="l_gripper_r_finger_joint"/>
-      <passive_joint name="l_gripper_r_finger_tip_joint"/>
-      <passive_joint name="l_gripper_l_finger_tip_joint"/>
-    </transmission>
-
-  <!-- Forearm Cam (Hand approximated values) -->
-  <joint name="l_forearm_cam_frame_joint" type="fixed">
-      <origin rpy="1.57079632679 -0.785398163397 0" xyz=".15 0 .07"/>
-      <parent link="l_forearm_roll_link"/>
-      <child link="l_forearm_cam_frame"/>
-    </joint><link name="l_forearm_cam_frame">
-
-      <inertial>
-        <mass value="0.01"/>
-        <origin xyz="0 0 0"/>
-        <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
-      </inertial>
-
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </visual>
-
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </collision>
-    </link><joint name="l_forearm_cam_optical_frame_joint" type="fixed">
-      <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
-      <parent link="l_forearm_cam_frame"/>
-      <child link="l_forearm_cam_optical_frame"/>
-    </joint><link name="l_forearm_cam_optical_frame">
-
-
-      <inertial>
-        <mass value="0.01"/>
-        <origin xyz="0 0 0"/>
-        <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
-      </inertial>
-
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </visual>
-
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="l_forearm_cam_frame">
-      <sensor:camera name="l_forearm_cam_sensor">
-        <!--imageSize>${image_width} ${image_height}</imageSize-->
-        <imageSize>640 480</imageSize> <!-- FIXME: temporary fix because xacro expands above line to floats, not integers -->
-        <imageFormat>L8</imageFormat>
-        <hfov>90.0</hfov>
-        <nearClip>0.1</nearClip>
-        <farClip>100</farClip>
-        <updateRate>25.0</updateRate>
-        <controller:gazebo_ros_camera name="l_forearm_cam_controller" plugin="libgazebo_ros_camera.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>25.0</updateRate>
-          <imageTopicName>l_forearm_cam/image_raw</imageTopicName>
-          <cameraInfoTopicName>l_forearm_cam/camera_info</cameraInfoTopicName>
-          <frameName>l_forearm_cam_frame</frameName>
-          <hackBaseline>0.0</hackBaseline>
-          <CxPrime>320.5</CxPrime>
-          <Cx>320.5</Cx>
-          <Cy>240.5</Cy>
-          <!-- image_width / (2*tan(hfov_radian /2)) -->
-          <!-- 320 for wide and 772.55 for narrow stereo camera -->
-          <focal_length>320.0</focal_length>
-          <distortion_k1>0</distortion_k1>
-          <distortion_k2>0</distortion_k2>
-          <distortion_k3>0</distortion_k3>
-          <distortion_t1>0</distortion_t1>
-          <distortion_t2>0</distortion_t2>
-          <interface:camera name="l_forearm_cam_iface"/>
-        </controller:gazebo_ros_camera>
-      </sensor:camera>
-      <turnGravityOff>true</turnGravityOff>
-      <material>PR2/Blue</material>
-    </gazebo><gazebo reference="l_forearm_cam_optical_frame">
-      <turnGravityOff>true</turnGravityOff>
-    </gazebo>
-  <joint name="r_forearm_cam_frame_joint" type="fixed">
-      <origin rpy="-1.57079632679 -0.785398163397 0" xyz=".15 0 .07"/>
-      <parent link="r_forearm_roll_link"/>
-      <child link="r_forearm_cam_frame"/>
-    </joint><link name="r_forearm_cam_frame">
-
-      <inertial>
-        <mass value="0.01"/>
-        <origin xyz="0 0 0"/>
-        <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
-      </inertial>
-
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </visual>
-
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </collision>
-    </link><joint name="r_forearm_cam_optical_frame_joint" type="fixed">
-      <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
-      <parent link="r_forearm_cam_frame"/>
-      <child link="r_forearm_cam_optical_frame"/>
-    </joint><link name="r_forearm_cam_optical_frame">
-
-
-      <inertial>
-        <mass value="0.01"/>
-        <origin xyz="0 0 0"/>
-        <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
-      </inertial>
-
-      <visual>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </visual>
-
-      <collision>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry>
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="r_forearm_cam_frame">
-      <sensor:camera name="r_forearm_cam_sensor">
-        <!--imageSize>${image_width} ${image_height}</imageSize-->
-        <imageSize>640 480</imageSize> <!-- FIXME: temporary fix because xacro expands above line to floats, not integers -->
-        <imageFormat>L8</imageFormat>
-        <hfov>90.0</hfov>
-        <nearClip>0.1</nearClip>
-        <farClip>100</farClip>
-        <updateRate>25.0</updateRate>
-        <controller:gazebo_ros_camera name="r_forearm_cam_controller" plugin="libgazebo_ros_camera.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>25.0</updateRate>
-          <imageTopicName>r_forearm_cam/image_raw</imageTopicName>
-          <cameraInfoTopicName>r_forearm_cam/camera_info</cameraInfoTopicName>
-          <frameName>r_forearm_cam_frame</frameName>
-          <hackBaseline>0.0</hackBaseline>
-          <CxPrime>320.5</CxPrime>
-          <Cx>320.5</Cx>
-          <Cy>240.5</Cy>
-          <!-- image_width / (2*tan(hfov_radian /2)) -->
-          <!-- 320 for wide and 772.55 for narrow stereo camera -->
-          <focal_length>320.0</focal_length>
-          <distortion_k1>0</distortion_k1>
-          <distortion_k2>0</distortion_k2>
-          <distortion_k3>0</distortion_k3>
-          <distortion_t1>0</distortion_t1>
-          <distortion_t2>0</distortion_t2>
-          <interface:camera name="r_forearm_cam_iface"/>
-        </controller:gazebo_ros_camera>
-      </sensor:camera>
-      <turnGravityOff>true</turnGravityOff>
-      <material>PR2/Blue</material>
-    </gazebo><gazebo reference="r_forearm_cam_optical_frame">
-      <turnGravityOff>true</turnGravityOff>
-    </gazebo>
-
-</robot>
diff --git a/urdf/test/pr2_desc_no_joint.urdf b/urdf/test/pr2_desc_no_joint.urdf
index 04839d2..11fc4fd 100644
--- a/urdf/test/pr2_desc_no_joint.urdf
+++ b/urdf/test/pr2_desc_no_joint.urdf
@@ -1,7 +1,6 @@
 <?xml version="1.0" ?>
 
 <robot name="pr2">
-    <joint name="fl_caster_rotation_joint" type="continuous"/>
     <link name="fl_caster_rotation_link">
       <inertial>
         <mass value="3.473082"/>
diff --git a/urdf/test/test_robot_model_parser.launch b/urdf/test/test_robot_model_parser.launch
index 3ee95ab..8e262c2 100644
--- a/urdf/test/test_robot_model_parser.launch
+++ b/urdf/test/test_robot_model_parser.launch
@@ -1,5 +1,5 @@
 <launch>
   <param name="robot_description" textfile="$(find urdf)/test/pr2_desc.urdf" />
 
-  <test test-name="robot_model_parser_test" pkg="urdf" type="test_parser" args="$(find urdf) fail_pr2_desc_bracket.urdf fail_pr2_desc_double_joint.urdf fail_pr2_desc_double.urdf fail_pr2_desc_loop.urdf fail_pr2_desc_no_filename_in_mesh.urdf fail_pr2_desc_no_joint2.urdf fail_pr2_desc_parent_itself.urdf fail_pr2_desc_two_trees.urdf fail_three_links_one_joint.urdf no_visual.urdf one_link.urdf pr2_desc_explicit_world.urdf pr2_desc_no_joint.urdf pr2_desc.urdf two_links_one_joint.urdf singularity.urdf" />
+  <test test-name="robot_model_parser_test" pkg="urdf" type="test_parser" args="$(find urdf) fail_pr2_desc_bracket.urdf fail_pr2_desc_double_joint.urdf fail_pr2_desc_double.urdf fail_pr2_desc_loop.urdf fail_pr2_desc_no_filename_in_mesh.urdf fail_pr2_desc_no_joint2.urdf fail_pr2_desc_parent_itself.urdf fail_pr2_desc_two_trees.urdf fail_three_links_one_joint.urdf no_visual.urdf one_link.urdf pr2_desc_no_joint.urdf pr2_desc.urdf two_links_one_joint.urdf singularity.urdf" />
 </launch>