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