diff --git a/kdl_parser/pr2.urdf b/kdl_parser/pr2.urdf
deleted file mode 100644
index 45e35f9c..0000000
--- a/kdl_parser/pr2.urdf
+++ /dev/null
@@ -1,2542 +0,0 @@
-<?xml version="1.0" ?><robot name="pr2">
-  
-  <!-- Include file with calibration parameters -->
-  
-  
-  <!-- declare the path where all models/textures/materials are stored -->
-  <resource location="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes" type="stl_meshes"/>
-  <resource location="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_ogre/Media/models" type="ogre"/>
-
-  <rviz_resource_location path="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_ogre/Media/materials/scripts"/><rviz_resource_location path="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_ogre/Media/materials/textures"/>
-  <rviz_resource_location path="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_ogre/Media/materials/scripts"/><rviz_resource_location path="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_ogre/Media/materials/textures"/>
-  <rviz_resource_location path="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_ogre/Media/materials/scripts"/><rviz_resource_location path="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_ogre/Media/materials/textures"/>
-  <rviz_resource_location path="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_ogre/Media/materials/scripts"/><rviz_resource_location path="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_ogre/Media/materials/textures"/>
-  <rviz_resource_location path="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_ogre/Media/materials/scripts"/><rviz_resource_location path="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_ogre/Media/materials/textures"/>
-  <rviz_resource_location path="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_ogre/Media/materials/scripts"/><rviz_resource_location path="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_ogre/Media/materials/textures"/>
-  <gazebo>
-    <!-- PR2_ACTARRAY -->
-    <controller:gazebo_mechanism_control name="gazebo_mechanism_control" plugin="libgazebo_mechanism_control.so">
-      <alwaysOn>true</alwaysOn>
-      <updateRate>1000.0</updateRate>
-      <interface:audio name="gazebo_mechanism_control_dummy_iface"/>
-    </controller:gazebo_mechanism_control>
-
-    <!-- battery controls -->
-    <controller:gazebo_battery name="gazebo_battery_controller" plugin="libgazebo_battery.so">
-      <alwaysOn>true</alwaysOn>
-      <updateRate>1.0</updateRate>
-      <timeout>5</timeout>
-      <default_consumption_rate>-10.0</default_consumption_rate> <!-- -5 is the magic number, need to be smaller than that CM -->
-      <diagnostic_rate>1.0</diagnostic_rate>
-      <battery_state_rate>10.0</battery_state_rate> <!-- does nothing for now-->
-      <full_charge_energy>1200000.0</full_charge_energy>
-      <diagnosticTopicName>diagnostic</diagnosticTopicName>
-      <stateTopicName>battery_state</stateTopicName>
-      <selfTestServiceName>self_test</selfTestServiceName>
-      <interface:audio name="battery_dummy_interface"/>
-    </controller:gazebo_battery>
-
-    <controller:ros_time name="ros_time" plugin="libros_time.so">
-      <alwaysOn>true</alwaysOn>
-      <updateRate>1000.0</updateRate>
-      <interface:audio name="dummy_ros_time_iface_should_not_be_here"/>
-    </controller:ros_time>
-  </gazebo>
-
-  <joint name="base_joint" type="planar">
-      <origin rpy="0 0 0" xyz="0 0 0.051"/>
-      <parent link="world"/>
-      <child link="base_link"/>
-    </joint><link name="base_link">
-
-      <inertial>
-        <mass value="116.0"/>
-        <origin xyz="-0.061 0.0 0.293"/>
-        <inertia ixx="5.652232699207" ixy="-0.009719934438" ixz="1.293988226423" iyy="5.669473158652" iyz="-0.007379583694" izz="3.683196351726"/>
-      </inertial>
-
-      <visual name="base_visual">
-        <origin rpy="0 0 0" xyz="0 0 0.05"/>
-        <geometry name="base_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/base.stl"/>
-        </geometry>
-      </visual>
-
-      <collision name="base_collision">
-        <origin rpy="0 0 0" xyz="0 0 0.165"/>
-        <geometry name="base_collision_geom">
-          <box size="0.65 0.65 0.23"/>
-        </geometry>
-      </collision>
-    </link><joint name="base_laser_joint" type="fixed">
-      <axis xyz="0 1 0"/>
-      <origin rpy="0 0 0" xyz="0.275 0.0 0.302"/>
-      <parent link="base_link"/>
-      <child link="base_laser"/>
-    </joint><link name="base_laser" type="laser">
-
-      <inertial>
-        <mass value="1.0"/>
-        <origin xyz="0 0 0"/>
-        <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
-      </inertial>
-
-      <visual name="base_lvisual">
-        <origin rpy="0 0 0" xyz="0 0 0.03"/>
-        <geometry name="base_laser_visual_geom">
-          <box size="0.08 0.08 0.03"/>
-        </geometry>
-      </visual>
-
-      <collision name="base_lcollision">
-        <origin rpy="0 0 0" xyz="0 0 0.03"/>
-        <geometry name="base_laser_collision_geom">
-          <box size="0.08 0.08 0.03"/>
-        </geometry>
-      </collision>
-
-    </link><joint name="plug_magnet_joint" type="fixed">
-      <axis xyz="0 1 0"/>
-      <anchor xyz="0 0 0"/>
-      <origin rpy="0 0 0" xyz="0.18 0 0.285"/>
-      <parent link="base_link"/>
-      <child link="plug_magnet"/>
-    </joint><link name="plug_magnet" type="laser">
-
-
-      <inertial>
-        <mass value="0.1"/>
-        <origin xyz="0 0 0.0"/>
-        <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
-      </inertial>
-
-      <visual name="plug_mavisual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="plug_magnet_visual_geom">
-          <box size="0.001 0.001 0.001"/>
-        </geometry>
-      </visual>
-
-      <collision name="plug_macollision">
-        <origin rpy="0 0 0" xyz="0 0 0.0"/>
-        <geometry name="plug_magnet_collision_geom">
-          <box size=".001 .001 .001"/>
-        </geometry>
-      </collision>
-
-    </link><joint name="fl_caster_rotation_joint" type="continuous">
-      <axis xyz="0 0 1"/>
-      <limit effort="100" velocity="100"/>
-      <safety_controller k_velocity="10"/>
-      <calibration reference_position="0.0"/>
-      <dynamics damping="0.0" friction="0.0"/>
-      <origin rpy="0 0 0" xyz="0.2225 0.2225 0.0282"/>
-      <parent link="base_link"/>
-      <child link="fl_caster_rotation_link"/>
-    </joint><link name="fl_caster_rotation_link">
-
-      <inertial>
-        <mass value="3.473082"/>
-        <origin xyz="0 0 0.07"/>
-        <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
-      </inertial>
-
-      <visual name="fl_caster_rotation_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="fl_caster_rotation_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/caster.stl"/>
-        </geometry>
-      </visual>
-
-      <collision name="fl_caster_rotation_collision">
-        <origin rpy="0.0 0.0 0.0" xyz="0 0 0.07"/>
-        <geometry name="fl_caster_rotation_collision_geom">
-          <box size="0.192 0.164 0.013"/>
-        </geometry>
-      </collision>
-    </link><transmission name="fl_caster_rotation_trans" type="SimpleTransmission">
-      <actuator name="fl_caster_rotation_motor"/>
-      <joint name="fl_caster_rotation_joint"/>
-      <mechanicalReduction>-75.05</mechanicalReduction>
-    </transmission><rviz reference="fl_caster_rotation_link">
-      <material value="PR2/Green"/>
-    </rviz><joint name="fl_caster_l_wheel_joint" type="continuous">
-      <axis xyz="0 1 0"/>
-      <limit effort="100" velocity="100"/>
-      <safety_controller k_velocity="10"/>
-      <dynamics damping="0.0" friction="0.0"/>
-      <origin rpy="0 0 0" xyz="0 0.049 0"/>
-      <parent link="fl_caster_rotation_link"/>
-      <child link="fl_caster_l_wheel_link"/>
-    </joint><link name="fl_caster_l_wheel_link">
-      <inertial>
-        <mass value="0.44036"/> <!-- check jmh 20081205 -->
-        <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 name="fl_caster_l_wheel_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="fl_caster_l_wheel_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/pr2_wheel.stl"/>
-        </geometry>
-
-      </visual>
-      <collision name="fl_caster_l_wheel_collision">
-        <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
-        <geometry name="fl_caster_l_wheel_collision_geom">
-          <cylinder length="0.03" radius="0.074792"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="fl_caster_l_wheel_link">
-      <sensor:contact name="fl_caster_l_wheel_contact_sensor">
-        <geom>fl_caster_l_wheel_collision</geom>
-        <updateRate>100.0</updateRate>
-        <controller:ros_bumper name="fl_caster_l_wheel_ros_bumper_controller" plugin="libros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>fl_caster_l_wheel_bumper</bumperTopicName>
-          <interface:bumper name="fl_caster_l_wheel_ros_bumper_iface"/>
-        </controller:ros_bumper>
-      </sensor:contact>
-      <mu1 value="100.0"/>
-      <mu2 value="100.0"/>
-      <kp value="1000000.0"/>
-      <kd value="1.0"/>
-    </gazebo><rviz reference="fl_caster_l_wheel_link">
-      <!-- TODO should be different for left and right wheel-->
-      <material value="PR2/fl_caster_l_wheel_link"/>
-    </rviz><transmission name="fl_caster_l_wheel_trans" type="SimpleTransmission">
-      <actuator name="fl_caster_l_wheel_motor"/>
-      <joint name="fl_caster_l_wheel_joint"/>
-      <mechanicalReduction>75.05</mechanicalReduction>
-    </transmission><joint name="fl_caster_r_wheel_joint" type="continuous">
-      <axis xyz="0 1 0"/>
-      <limit effort="100" velocity="100"/>
-      <safety_controller k_velocity="10"/>
-      <dynamics damping="0.0" friction="0.0"/>
-      <origin rpy="0 0 0" xyz="0 -0.049 0"/>
-      <parent link="fl_caster_rotation_link"/>
-      <child link="fl_caster_r_wheel_link"/>
-    </joint><link name="fl_caster_r_wheel_link">
-      <inertial>
-        <mass value="0.44036"/> <!-- check jmh 20081205 -->
-        <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 name="fl_caster_r_wheel_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="fl_caster_r_wheel_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/pr2_wheel.stl"/>
-        </geometry>
-
-      </visual>
-      <collision name="fl_caster_r_wheel_collision">
-        <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
-        <geometry name="fl_caster_r_wheel_collision_geom">
-          <cylinder length="0.03" radius="0.074792"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="fl_caster_r_wheel_link">
-      <sensor:contact name="fl_caster_r_wheel_contact_sensor">
-        <geom>fl_caster_r_wheel_collision</geom>
-        <updateRate>100.0</updateRate>
-        <controller:ros_bumper name="fl_caster_r_wheel_ros_bumper_controller" plugin="libros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>fl_caster_r_wheel_bumper</bumperTopicName>
-          <interface:bumper name="fl_caster_r_wheel_ros_bumper_iface"/>
-        </controller:ros_bumper>
-      </sensor:contact>
-      <mu1 value="100.0"/>
-      <mu2 value="100.0"/>
-      <kp value="1000000.0"/>
-      <kd value="1.0"/>
-    </gazebo><rviz reference="fl_caster_r_wheel_link">
-      <!-- TODO should be different for left and right wheel-->
-      <material value="PR2/fl_caster_r_wheel_link"/>
-    </rviz><transmission name="fl_caster_r_wheel_trans" type="SimpleTransmission">
-      <actuator name="fl_caster_r_wheel_motor"/>
-      <joint name="fl_caster_r_wheel_joint"/>
-      <mechanicalReduction>-75.05</mechanicalReduction>
-    </transmission><joint name="fr_caster_rotation_joint" type="continuous">
-      <axis xyz="0 0 1"/>
-      <limit effort="100" velocity="100"/>
-      <safety_controller k_velocity="10"/>
-      <calibration reference_position="0.0"/>
-      <dynamics damping="0.0" friction="0.0"/>
-      <origin rpy="0 0 0" xyz="0.2225 -0.2225 0.0282"/>
-      <parent link="base_link"/>
-      <child link="fr_caster_rotation_link"/>
-    </joint><link name="fr_caster_rotation_link">
-
-      <inertial>
-        <mass value="3.473082"/>
-        <origin xyz="0 0 0.07"/>
-        <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
-      </inertial>
-
-      <visual name="fr_caster_rotation_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="fr_caster_rotation_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/caster.stl"/>
-        </geometry>
-      </visual>
-
-      <collision name="fr_caster_rotation_collision">
-        <origin rpy="0.0 0.0 0.0" xyz="0 0 0.07"/>
-        <geometry name="fr_caster_rotation_collision_geom">
-          <box size="0.192 0.164 0.013"/>
-        </geometry>
-      </collision>
-    </link><transmission name="fr_caster_rotation_trans" type="SimpleTransmission">
-      <actuator name="fr_caster_rotation_motor"/>
-      <joint name="fr_caster_rotation_joint"/>
-      <mechanicalReduction>-75.05</mechanicalReduction>
-    </transmission><rviz reference="fr_caster_rotation_link">
-      <material value="PR2/Green"/>
-    </rviz><joint name="fr_caster_l_wheel_joint" type="continuous">
-      <axis xyz="0 1 0"/>
-      <limit effort="100" velocity="100"/>
-      <safety_controller k_velocity="10"/>
-      <dynamics damping="0.0" friction="0.0"/>
-      <origin rpy="0 0 0" xyz="0 0.049 0"/>
-      <parent link="fr_caster_rotation_link"/>
-      <child link="fr_caster_l_wheel_link"/>
-    </joint><link name="fr_caster_l_wheel_link">
-      <inertial>
-        <mass value="0.44036"/> <!-- check jmh 20081205 -->
-        <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 name="fr_caster_l_wheel_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="fr_caster_l_wheel_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/pr2_wheel.stl"/>
-        </geometry>
-
-      </visual>
-      <collision name="fr_caster_l_wheel_collision">
-        <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
-        <geometry name="fr_caster_l_wheel_collision_geom">
-          <cylinder length="0.03" radius="0.074792"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="fr_caster_l_wheel_link">
-      <sensor:contact name="fr_caster_l_wheel_contact_sensor">
-        <geom>fr_caster_l_wheel_collision</geom>
-        <updateRate>100.0</updateRate>
-        <controller:ros_bumper name="fr_caster_l_wheel_ros_bumper_controller" plugin="libros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>fr_caster_l_wheel_bumper</bumperTopicName>
-          <interface:bumper name="fr_caster_l_wheel_ros_bumper_iface"/>
-        </controller:ros_bumper>
-      </sensor:contact>
-      <mu1 value="100.0"/>
-      <mu2 value="100.0"/>
-      <kp value="1000000.0"/>
-      <kd value="1.0"/>
-    </gazebo><rviz reference="fr_caster_l_wheel_link">
-      <!-- TODO should be different for left and right wheel-->
-      <material value="PR2/fr_caster_l_wheel_link"/>
-    </rviz><transmission name="fr_caster_l_wheel_trans" type="SimpleTransmission">
-      <actuator name="fr_caster_l_wheel_motor"/>
-      <joint name="fr_caster_l_wheel_joint"/>
-      <mechanicalReduction>75.05</mechanicalReduction>
-    </transmission><joint name="fr_caster_r_wheel_joint" type="continuous">
-      <axis xyz="0 1 0"/>
-      <limit effort="100" velocity="100"/>
-      <safety_controller k_velocity="10"/>
-      <dynamics damping="0.0" friction="0.0"/>
-      <origin rpy="0 0 0" xyz="0 -0.049 0"/>
-      <parent link="fr_caster_rotation_link"/>
-      <child link="fr_caster_r_wheel_link"/>
-    </joint><link name="fr_caster_r_wheel_link">
-      <inertial>
-        <mass value="0.44036"/> <!-- check jmh 20081205 -->
-        <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 name="fr_caster_r_wheel_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="fr_caster_r_wheel_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/pr2_wheel.stl"/>
-        </geometry>
-
-      </visual>
-      <collision name="fr_caster_r_wheel_collision">
-        <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
-        <geometry name="fr_caster_r_wheel_collision_geom">
-          <cylinder length="0.03" radius="0.074792"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="fr_caster_r_wheel_link">
-      <sensor:contact name="fr_caster_r_wheel_contact_sensor">
-        <geom>fr_caster_r_wheel_collision</geom>
-        <updateRate>100.0</updateRate>
-        <controller:ros_bumper name="fr_caster_r_wheel_ros_bumper_controller" plugin="libros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>fr_caster_r_wheel_bumper</bumperTopicName>
-          <interface:bumper name="fr_caster_r_wheel_ros_bumper_iface"/>
-        </controller:ros_bumper>
-      </sensor:contact>
-      <mu1 value="100.0"/>
-      <mu2 value="100.0"/>
-      <kp value="1000000.0"/>
-      <kd value="1.0"/>
-    </gazebo><rviz reference="fr_caster_r_wheel_link">
-      <!-- TODO should be different for left and right wheel-->
-      <material value="PR2/fr_caster_r_wheel_link"/>
-    </rviz><transmission name="fr_caster_r_wheel_trans" type="SimpleTransmission">
-      <actuator name="fr_caster_r_wheel_motor"/>
-      <joint name="fr_caster_r_wheel_joint"/>
-      <mechanicalReduction>-75.05</mechanicalReduction>
-    </transmission><joint name="bl_caster_rotation_joint" type="continuous">
-      <axis xyz="0 0 1"/>
-      <limit effort="100" velocity="100"/>
-      <safety_controller k_velocity="10"/>
-      <calibration reference_position="0.0"/>
-      <dynamics damping="0.0" friction="0.0"/>
-      <origin rpy="0 0 0" xyz="-0.2225 0.2225 0.0282"/>
-      <parent link="base_link"/>
-      <child link="bl_caster_rotation_link"/>
-    </joint><link name="bl_caster_rotation_link">
-
-      <inertial>
-        <mass value="3.473082"/>
-        <origin xyz="0 0 0.07"/>
-        <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
-      </inertial>
-
-      <visual name="bl_caster_rotation_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="bl_caster_rotation_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/caster.stl"/>
-        </geometry>
-      </visual>
-
-      <collision name="bl_caster_rotation_collision">
-        <origin rpy="0.0 0.0 0.0" xyz="0 0 0.07"/>
-        <geometry name="bl_caster_rotation_collision_geom">
-          <box size="0.192 0.164 0.013"/>
-        </geometry>
-      </collision>
-    </link><transmission name="bl_caster_rotation_trans" type="SimpleTransmission">
-      <actuator name="bl_caster_rotation_motor"/>
-      <joint name="bl_caster_rotation_joint"/>
-      <mechanicalReduction>-75.05</mechanicalReduction>
-    </transmission><rviz reference="bl_caster_rotation_link">
-      <material value="PR2/Green"/>
-    </rviz><joint name="bl_caster_l_wheel_joint" type="continuous">
-      <axis xyz="0 1 0"/>
-      <limit effort="100" velocity="100"/>
-      <safety_controller k_velocity="10"/>
-      <dynamics damping="0.0" friction="0.0"/>
-      <origin rpy="0 0 0" xyz="0 0.049 0"/>
-      <parent link="bl_caster_rotation_link"/>
-      <child link="bl_caster_l_wheel_link"/>
-    </joint><link name="bl_caster_l_wheel_link">
-      <inertial>
-        <mass value="0.44036"/> <!-- check jmh 20081205 -->
-        <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 name="bl_caster_l_wheel_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="bl_caster_l_wheel_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/pr2_wheel.stl"/>
-        </geometry>
-
-      </visual>
-      <collision name="bl_caster_l_wheel_collision">
-        <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
-        <geometry name="bl_caster_l_wheel_collision_geom">
-          <cylinder length="0.03" radius="0.074792"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="bl_caster_l_wheel_link">
-      <sensor:contact name="bl_caster_l_wheel_contact_sensor">
-        <geom>bl_caster_l_wheel_collision</geom>
-        <updateRate>100.0</updateRate>
-        <controller:ros_bumper name="bl_caster_l_wheel_ros_bumper_controller" plugin="libros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>bl_caster_l_wheel_bumper</bumperTopicName>
-          <interface:bumper name="bl_caster_l_wheel_ros_bumper_iface"/>
-        </controller:ros_bumper>
-      </sensor:contact>
-      <mu1 value="100.0"/>
-      <mu2 value="100.0"/>
-      <kp value="1000000.0"/>
-      <kd value="1.0"/>
-    </gazebo><rviz reference="bl_caster_l_wheel_link">
-      <!-- TODO should be different for left and right wheel-->
-      <material value="PR2/bl_caster_l_wheel_link"/>
-    </rviz><transmission name="bl_caster_l_wheel_trans" type="SimpleTransmission">
-      <actuator name="bl_caster_l_wheel_motor"/>
-      <joint name="bl_caster_l_wheel_joint"/>
-      <mechanicalReduction>75.05</mechanicalReduction>
-    </transmission><joint name="bl_caster_r_wheel_joint" type="continuous">
-      <axis xyz="0 1 0"/>
-      <limit effort="100" velocity="100"/>
-      <safety_controller k_velocity="10"/>
-      <dynamics damping="0.0" friction="0.0"/>
-      <origin rpy="0 0 0" xyz="0 -0.049 0"/>
-      <parent link="bl_caster_rotation_link"/>
-      <child link="bl_caster_r_wheel_link"/>
-    </joint><link name="bl_caster_r_wheel_link">
-      <inertial>
-        <mass value="0.44036"/> <!-- check jmh 20081205 -->
-        <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 name="bl_caster_r_wheel_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="bl_caster_r_wheel_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/pr2_wheel.stl"/>
-        </geometry>
-
-      </visual>
-      <collision name="bl_caster_r_wheel_collision">
-        <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
-        <geometry name="bl_caster_r_wheel_collision_geom">
-          <cylinder length="0.03" radius="0.074792"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="bl_caster_r_wheel_link">
-      <sensor:contact name="bl_caster_r_wheel_contact_sensor">
-        <geom>bl_caster_r_wheel_collision</geom>
-        <updateRate>100.0</updateRate>
-        <controller:ros_bumper name="bl_caster_r_wheel_ros_bumper_controller" plugin="libros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>bl_caster_r_wheel_bumper</bumperTopicName>
-          <interface:bumper name="bl_caster_r_wheel_ros_bumper_iface"/>
-        </controller:ros_bumper>
-      </sensor:contact>
-      <mu1 value="100.0"/>
-      <mu2 value="100.0"/>
-      <kp value="1000000.0"/>
-      <kd value="1.0"/>
-    </gazebo><rviz reference="bl_caster_r_wheel_link">
-      <!-- TODO should be different for left and right wheel-->
-      <material value="PR2/bl_caster_r_wheel_link"/>
-    </rviz><transmission name="bl_caster_r_wheel_trans" type="SimpleTransmission">
-      <actuator name="bl_caster_r_wheel_motor"/>
-      <joint name="bl_caster_r_wheel_joint"/>
-      <mechanicalReduction>-75.05</mechanicalReduction>
-    </transmission><joint name="br_caster_rotation_joint" type="continuous">
-      <axis xyz="0 0 1"/>
-      <limit effort="100" velocity="100"/>
-      <safety_controller k_velocity="10"/>
-      <calibration reference_position="0.0"/>
-      <dynamics damping="0.0" friction="0.0"/>
-      <origin rpy="0 0 0" xyz="-0.2225 -0.2225 0.0282"/>
-      <parent link="base_link"/>
-      <child link="br_caster_rotation_link"/>
-    </joint><link name="br_caster_rotation_link">
-
-      <inertial>
-        <mass value="3.473082"/>
-        <origin xyz="0 0 0.07"/>
-        <inertia ixx="0.012411765597" ixy="-0.000711733678" ixz="0.00050272983" iyy="0.015218160428" iyz="-0.000004273467" izz="0.011763977943"/>
-      </inertial>
-
-      <visual name="br_caster_rotation_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="br_caster_rotation_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/caster.stl"/>
-        </geometry>
-      </visual>
-
-      <collision name="br_caster_rotation_collision">
-        <origin rpy="0.0 0.0 0.0" xyz="0 0 0.07"/>
-        <geometry name="br_caster_rotation_collision_geom">
-          <box size="0.192 0.164 0.013"/>
-        </geometry>
-      </collision>
-    </link><transmission name="br_caster_rotation_trans" type="SimpleTransmission">
-      <actuator name="br_caster_rotation_motor"/>
-      <joint name="br_caster_rotation_joint"/>
-      <mechanicalReduction>-75.05</mechanicalReduction>
-    </transmission><rviz reference="br_caster_rotation_link">
-      <material value="PR2/Green"/>
-    </rviz><joint name="br_caster_l_wheel_joint" type="continuous">
-      <axis xyz="0 1 0"/>
-      <limit effort="100" velocity="100"/>
-      <safety_controller k_velocity="10"/>
-      <dynamics damping="0.0" friction="0.0"/>
-      <origin rpy="0 0 0" xyz="0 0.049 0"/>
-      <parent link="br_caster_rotation_link"/>
-      <child link="br_caster_l_wheel_link"/>
-    </joint><link name="br_caster_l_wheel_link">
-      <inertial>
-        <mass value="0.44036"/> <!-- check jmh 20081205 -->
-        <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 name="br_caster_l_wheel_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="br_caster_l_wheel_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/pr2_wheel.stl"/>
-        </geometry>
-
-      </visual>
-      <collision name="br_caster_l_wheel_collision">
-        <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
-        <geometry name="br_caster_l_wheel_collision_geom">
-          <cylinder length="0.03" radius="0.074792"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="br_caster_l_wheel_link">
-      <sensor:contact name="br_caster_l_wheel_contact_sensor">
-        <geom>br_caster_l_wheel_collision</geom>
-        <updateRate>100.0</updateRate>
-        <controller:ros_bumper name="br_caster_l_wheel_ros_bumper_controller" plugin="libros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>br_caster_l_wheel_bumper</bumperTopicName>
-          <interface:bumper name="br_caster_l_wheel_ros_bumper_iface"/>
-        </controller:ros_bumper>
-      </sensor:contact>
-      <mu1 value="100.0"/>
-      <mu2 value="100.0"/>
-      <kp value="1000000.0"/>
-      <kd value="1.0"/>
-    </gazebo><rviz reference="br_caster_l_wheel_link">
-      <!-- TODO should be different for left and right wheel-->
-      <material value="PR2/br_caster_l_wheel_link"/>
-    </rviz><transmission name="br_caster_l_wheel_trans" type="SimpleTransmission">
-      <actuator name="br_caster_l_wheel_motor"/>
-      <joint name="br_caster_l_wheel_joint"/>
-      <mechanicalReduction>75.05</mechanicalReduction>
-    </transmission><joint name="br_caster_r_wheel_joint" type="continuous">
-      <axis xyz="0 1 0"/>
-      <limit effort="100" velocity="100"/>
-      <safety_controller k_velocity="10"/>
-      <dynamics damping="0.0" friction="0.0"/>
-      <origin rpy="0 0 0" xyz="0 -0.049 0"/>
-      <parent link="br_caster_rotation_link"/>
-      <child link="br_caster_r_wheel_link"/>
-    </joint><link name="br_caster_r_wheel_link">
-      <inertial>
-        <mass value="0.44036"/> <!-- check jmh 20081205 -->
-        <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 name="br_caster_r_wheel_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="br_caster_r_wheel_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/pr2_wheel.stl"/>
-        </geometry>
-
-      </visual>
-      <collision name="br_caster_r_wheel_collision">
-        <origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
-        <geometry name="br_caster_r_wheel_collision_geom">
-          <cylinder length="0.03" radius="0.074792"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="br_caster_r_wheel_link">
-      <sensor:contact name="br_caster_r_wheel_contact_sensor">
-        <geom>br_caster_r_wheel_collision</geom>
-        <updateRate>100.0</updateRate>
-        <controller:ros_bumper name="br_caster_r_wheel_ros_bumper_controller" plugin="libros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>br_caster_r_wheel_bumper</bumperTopicName>
-          <interface:bumper name="br_caster_r_wheel_ros_bumper_iface"/>
-        </controller:ros_bumper>
-      </sensor:contact>
-      <mu1 value="100.0"/>
-      <mu2 value="100.0"/>
-      <kp value="1000000.0"/>
-      <kd value="1.0"/>
-    </gazebo><rviz reference="br_caster_r_wheel_link">
-      <!-- TODO should be different for left and right wheel-->
-      <material value="PR2/br_caster_r_wheel_link"/>
-    </rviz><transmission name="br_caster_r_wheel_trans" type="SimpleTransmission">
-      <actuator name="br_caster_r_wheel_motor"/>
-      <joint name="br_caster_r_wheel_joint"/>
-      <mechanicalReduction>-75.05</mechanicalReduction>
-    </transmission><gazebo reference="base_link">
-      <sensor:contact name="base_contact_sensor">
-        <geom>base_collision</geom>
-        <updateRate>100.0</updateRate>
-        <controller:ros_bumper name="base_ros_bumper_controller" plugin="libros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>base_bumper</bumperTopicName>
-          <interface:bumper name="base_bumper_iface"/>
-        </controller:ros_bumper>
-      </sensor:contact>
-      <mu1 value="500.0"/>
-      <mu2 value="500.0"/>
-      <kp value="100000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/Green"/>
-      <controller:ros_p3d name="p3d_base_controller" plugin="libros_p3d.so">
-        <alwaysOn>true</alwaysOn>
-        <updateRate>100.0</updateRate>
-        <bodyName>base_link</bodyName>
-        <topicName>base_pose_ground_truth</topicName>
-        <gaussianNoise>0.01</gaussianNoise>
-        <frameName>map</frameName>
-        <xyzOffsets>25.7 25.7 0</xyzOffsets> <!-- initialize odometry for fake localization-->
-        <rpyOffsets>0 0 0</rpyOffsets>
-        <interface:position name="p3d_base_position"/>
-      </controller:ros_p3d>
-    </gazebo><gazebo reference="base_laser">
-      <sensor:ray name="base_laser">
-        <rayCount>640</rayCount>
-        <rangeCount>640</rangeCount>
-        <laserCount>1</laserCount>
-        <origin>0.0 0.0 0.0</origin>
-        <displayRays>false</displayRays>
-
-        <minAngle>-100</minAngle> <!-- scans own arms if -135~+135 -->
-        <maxAngle>100</maxAngle>
-
-        <minRange>0.05</minRange>
-        <maxRange>10.0</maxRange>
-        <resRange>0.01</resRange>
-        <updateRate>20.0</updateRate>
-        <controller:ros_laser name="ros_base_laser_controller" plugin="libros_laser.so">
-          <gaussianNoise>0.005</gaussianNoise>
-          <alwaysOn>true</alwaysOn>
-          <updateRate>20.0</updateRate>
-          <topicName>base_scan</topicName>
-          <frameName>base_laser</frameName>
-          <interface:laser name="ros_base_laser_iface"/>
-        </controller:ros_laser>
-      </sensor:ray>
-      <material value="PR2/Red"/>
-    </gazebo><gazebo reference="plug_magnet">
-      <material value="PR2/Red"/>
-      <controller:ros_p3d name="p3d_plug_magnet_controller" plugin="libros_p3d.so">
-        <alwaysOn>true</alwaysOn>
-        <updateRate>100.0</updateRate>
-        <bodyName>plug_magnet</bodyName>
-        <topicName>plug_magnet_pose_ground_truth</topicName>
-        <gaussianNoise>0.01</gaussianNoise>
-        <frameName>map</frameName>
-        <xyzOffsets>0 0 0</xyzOffsets> <!-- initialize odometry for fake localization-->
-        <rpyOffsets>0 0 0</rpyOffsets>
-        <interface:position name="p3d_plug_magnet_position"/>
-      </controller:ros_p3d>
-    </gazebo><rviz reference="base_link">
-      <material value="PR2/Green"/>
-    </rviz><rviz reference="base_laser">
-      <material value="PR2/Red"/>
-    </rviz>
-
-  <joint name="torso_lift_joint" type="prismatic">
-      <axis xyz="0 0 1"/>
-      <!-- KWatts: Max should be 0.31, changed to 0.20 to prevent jamming -->
-      <!-- HW ticket 481, will examine spine to determine failures -->
-      <limit effort="10000" lower="0.0" upper="0.20" velocity="0.015"/>
-      <safety_controller k_position="1000" k_velocity="400000" soft_lower_limit="0.005" soft_upper_limit="0.195"/>
-      <calibration reference_position="0.00536"/>
-      <dynamics damping="10.0"/>
-      <origin rpy="0 0 0" xyz="-0.05 0 0.739675"/>
-      <parent link="base_link"/>
-      <child link="torso_lift_link"/>
-    </joint><link name="torso_lift_link">
-
-      <inertial>
-        <mass value="36.248046"/>
-        <origin xyz="-0.1 0 -0.0885"/>
-        <inertia ixx="2.771653750257" ixy="0.004284522609" ixz="-0.160418504506" iyy="2.510019507959" iyz="0.029664468704" izz="0.526432355569"/>
-      </inertial>
-
-      <visual name="torso_lift_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="torso_lift_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/torso.stl"/>
-        </geometry>
-      </visual>
-
-      <collision name="torso_lift_collision">
-        <origin rpy="0 0 0" xyz="-0.1 0 -0.0885"/>
-        <geometry name="torso_lift_collision_geom">
-          <box size="0.30 0.620 0.823"/>
-        </geometry>
-      </collision>
-    </link><pr2_torso_gazebo name="torso_lift"/><transmission name="torso_lift_trans" type="SimpleTransmission">
-      <actuator name="torso_lift_motor"/>
-      <joint name="torso_lift_joint"/>
-      <mechanicalReduction>-52143.33</mechanicalReduction>
-    </transmission><rviz reference="torso_lift_link">
-      <material value="PR2/Grey2"/>
-    </rviz>
-
-  <joint name="head_pan_joint" type="revolute">
-      <axis xyz="0 0 1"/>
-      <limit effort="2.645" lower="-2.92" upper="2.92" velocity="6"/>
-      <safety_controller k_position="100" k_velocity="1.5" soft_lower_limit="-2.77" soft_upper_limit="2.77"/>
-      <calibration reference_position="-2.79"/>
-      <dynamics damping="1.0"/>
-      <origin rpy="0.0 0.0 0.0" xyz="0.0    0.0     0.3915"/>
-      <parent link="torso_lift_link"/>
-      <child link="head_pan_link"/>
-    </joint><link name="head_pan_link">
-
-
-      <inertial>
-        <mass value="1.611118"/>
-        <origin rpy="0 0 0" xyz="-0.005717  0.010312 -0.029649"/>
-        <inertia ixx="0.00482611007" ixy="-0.000144683999" ixz="0.000110076136" iyy="0.005218991412" iyz="-0.000314239509" izz="0.008618784925"/>
-      </inertial>
-
-      <visual name="head_pan_visual">
-        <origin rpy="0 0 0 " xyz="0 0 0.0"/>
-        <geometry name="head_pan_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/head_pan.stl"/>
-        </geometry>
-      </visual>
-
-      <collision name="head_pan_collision">
-        <origin rpy="0.0 0.0 0.0 " xyz="-0.005717  0.010312 -0.029649"/>
-        <geometry name="head_pan_collision_geom">
-          <box size="0.188 0.219 0.137"/>
-        </geometry>
-      </collision>
-
-    </link><gazebo reference="head_pan_link">
-      <material value="PR2/Blue"/>
-    </gazebo><transmission name="head_pan_trans" type="SimpleTransmission">
-      <actuator name="head_pan_motor"/>
-      <joint name="head_pan_joint"/>
-      <mechanicalReduction>6.0</mechanicalReduction>
-    </transmission><rviz reference="head_pan_link">
-      <material value="PR2/Blue"/>
-    </rviz><joint name="head_tilt_joint" type="revolute">
-      <axis xyz="0 1 0"/>
-      <limit effort="12.21" lower="-0.55" upper="1.047" velocity="100"/>
-      <safety_controller k_position="100" k_velocity="1.5" soft_lower_limit="-0.53" soft_upper_limit="1.045"/>
-      <calibration reference_position="-0.195"/>
-      <dynamics damping="1.0"/>
-      <origin rpy="0 0 0" xyz="0.058 0 0"/>
-      <parent link="head_pan_link"/>
-      <child link="head_tilt_link"/>
-    </joint><link name="head_tilt_link">
-
-
-      <inertial>
-        <mass value="1.749727"/>
-        <origin rpy="0 0 0" xyz="0.041935 0.003569 0.028143"/>
-        <inertia ixx="0.010602303435" ixy="-0.000408814235" ixz="0.00198303894" iyy="0.011874383747" iyz="0.000197908779" izz="0.005516790626"/>
-      </inertial>
-
-      <visual name="head_tilt_visual">
-        <origin rpy="0.0 0.0 0.0 " xyz="0 0 0"/>
-        <geometry name="head_tilt_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/head_tilt.stl"/>
-        </geometry>
-      </visual>
-
-      <collision name="head_tilt_collision">
-        <origin rpy="0 0 0" xyz="0.041935 0.003569 0.028143"/>
-        <geometry name="head_tilt_collision_geom">
-          <box size="0.064 0.253 0.181"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="head_tilt_link">
-      <material value="PR2/Green"/>
-    </gazebo><rviz reference="head_tilt_link">
-      <material value="PR2/Green"/>
-    </rviz><transmission name="head_tilt_trans" type="SimpleTransmission">
-      <actuator name="head_tilt_motor"/>
-      <joint name="head_tilt_joint"/>
-      <mechanicalReduction>6.0</mechanicalReduction>
-    </transmission><joint name="head_plate_frame_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="0.0232 0 0.0645"/>     
-      <parent link="head_tilt_link"/>
-      <child link="head_plate_frame"/>
-    </joint><link name="head_plate_frame">
-
-      <inertial>
-        <mass value="0.01"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
-      </inertial>
-
-      <visual name="head_plate_frame_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="head_plate_frame_visual_geom">
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </visual>
-
-      <collision name="head_plate_frame_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="head_plate_frame_collision_geom">
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-      </collision>
-    </link><gazebo reference="head_plate_frame">
-      <material value="PR2/Blue"/>
-    </gazebo><rviz reference="head_plate_frame_link">
-      <material value="PR2/Blue"/>
-    </rviz>
-
-  <!-- Camera package: double stereo, prosilica -->
-  <pr2_head_sensor_package hd_name="high_def" name="sensor_mount" parent="head_plate_frame" stereo_name="double_stereo">
-    <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
-  </pr2_head_sensor_package>
-    
-  <pr2_tilting_laser name="laser_tilt" parent="torso_lift_link">
-    <!--origin xyz="0.1 0 0.19525" rpy="0 0 0" /-->
-    <origin rpy="0 0 0" xyz="0.1 0 0.235"/>
-  </pr2_tilting_laser>
-
-  <joint name="r_shoulder_pan_joint" type="revolute">
-      <axis xyz="0 0 1"/>
-      <origin rpy="0 0 0" xyz="0 -0.188 0.0"/> <!-- transform from parent link to this joint frame -->
-      <parent link="torso_lift_link"/>
-      <child link="r_shoulder_pan_link"/>
-      <limit effort="30" lower="-2.2853981634" upper="0.714601836603" velocity="2.088"/>
-
-      <dynamics damping="10.0"/>
-
-      <safety_controller k_position="100" k_velocity="10" soft_lower_limit="-2.1353981634" soft_upper_limit="0.564601836603"/>
-
-      <!-- joint angle when the rising or the falling flag is activated on PR2 -->
-      <calibration reference_position="-0.785398163397"/>
-
-    </joint><link name="r_shoulder_pan_link">
-
-
-      <inertial>
-        <mass value="25.799322"/>
-        <origin rpy="0 0 0" xyz="-0.001201 0.024513 -0.098231"/>
-        <inertia ixx="0.866179142480" ixy="-0.06086507933" ixz="-0.12118061183" iyy="0.87421714893" iyz="-0.05886609911" izz="0.27353821674"/>
-      </inertial>
-
-      <visual name="r_shoulder_pan_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_shoulder_pan_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/shoulder_yaw.stl"/>
-        </geometry>
-        <texture name="PR2/Blue"/>
-      </visual>
-
-      <collision name="r_shoulder_pan_collision">
-        <origin rpy="0 0 0" xyz="0.0 0 0.0"/>
-        <geometry name="r_shoulder_pan_collision_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/convex/shoulder_yaw_convex.stlb"/>
-        </geometry>
-      </collision>
-    </link><joint name="r_shoulder_lift_joint" type="revolute">
-      <axis xyz="0 1 0"/>
-
-      <!-- Limits updated from Function's CAD values as of 2009_02_24 (link_data.xls) -->
-      <limit effort="30" lower="-0.5236" upper="1.3963" velocity="2.082"/>
-             
-      <safety_controller k_position="100" k_velocity="10" soft_lower="-0.3536" soft_upper="1.3463"/>
-      <calibration reference_position="0.0"/>
-      <dynamics damping="10.0"/>
-      <origin rpy="0 0 0" xyz="0.1 0 0"/>
-      <parent link="r_shoulder_pan_link"/>
-      <child link="r_shoulder_lift_link"/>
-    </joint><link name="r_shoulder_lift_link">
-
-      <inertial>
-        <mass value="2.74988"/>
-        <origin rpy="0 0 0" xyz="0.02195 -0.02664 -0.03127"/>
-        <inertia ixx="0.02105584615" ixy="0.00496704022" ixz="-0.00194808955" iyy="0.02127223737" iyz="0.00110425490" izz="0.01975753814"/>
-      </inertial>
-
-      <visual name="r_shoulder_lift_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_shoulder_lift_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/shoulder_lift.stl"/>
-        </geometry>
-        <texture name="PR2/Grey"/>
-      </visual>
-
-      <collision name="r_shoulder_lift_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_shoulder_lift_collision_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/convex/shoulder_lift_convex.stlb"/>
-        </geometry>
-      </collision>
-    </link><joint name="r_upper_arm_roll_joint" type="revolute">
-      <axis xyz="1 0 0"/>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <parent link="r_shoulder_lift_link"/>
-      <child link="r_upper_arm_roll_link"/>
-      <limit effort="30" lower="-3.9" upper="0.8" velocity="3.27"/>
-      <safety_controller k_position="100" k_velocity="2" soft_lower_limit="-3.75" soft_upper_limit="0.65"/>
-      <calibration reference_position="-1.57079632679"/>
-      <dynamics damping="0.1"/>
-    </joint><link name="r_upper_arm_roll_link">
-
-
-      <inertial>
-        <!-- dummy mass, to be removed -->
-        <mass value="0.1"/>
-        <origin rpy="0 0 0" xyz="0.0 0 0"/>
-        <inertia ixx="0.01" ixy="0.00" ixz="0.00" iyy="0.01" iyz="0.00" izz="0.01"/>
-      </inertial>
-      <visual name="r_upper_arm_roll_visual">
-        <!-- TODO: This component doesn't actually have a mesh -->
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_upper_arm_roll_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/upper_arm_roll.stl"/>
-        </geometry>
-      </visual>
-      <collision name="r_upper_arm_roll_collision">
-        <!-- TODO: collision tag should be optional -->
-        <origin rpy="0 0 0" xyz="0.0 0 0"/>
-        <geometry name="r_upper_arm_roll_collision_geom">
-          <box size="0.1 0.1 0.1"/>
-        </geometry>
-      </collision>
-    </link><joint name="r_upper_arm_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <parent link="r_upper_arm_roll_link"/>
-      <child link="r_upper_arm_link"/>
-    </joint><link name="r_upper_arm_link">
-
-      <inertial>
-        <!-- NOTE:reflect==-1 for right side, reflect==1 for the left side -->
-        <mass value="6.01769"/>
-        <origin xyz="0.21398 -0.01621 -0.0002"/>
-        <inertia ixx="0.01537748957" ixy="0.00375711247" ixz="-0.00070852914" iyy="0.0747367044" iyz="-0.0001793645" izz="0.07608763307"/>
-      </inertial>
-
-      <visual name="{side}_upper_arm_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_upper_arm_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/upper_arm.stl"/>
-        </geometry>
-      </visual>
-
-      <collision name="r_upper_arm_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_upper_arm_collision_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/convex/upper_arm_convex.stlb"/>
-        </geometry>
-      </collision>
-
-    </link><joint name="r_elbow_flex_joint" type="revolute">
-      <axis xyz="0 1 0"/>
-      <limit effort="30" lower="-2.3" upper="0.1" velocity="3.3"/>
-      <safety_controller k_position="100" k_velocity="3" soft_lower_limit="-2.05" soft_upper_limit="0.0"/>
-      <calibration reference_position="-1.1606"/>
-      <dynamics damping="1.0"/>
-      <origin rpy="0 0 0" xyz="0.4 0 0"/>
-      <parent link="r_upper_arm_link"/>
-      <child link="r_elbow_flex_link"/>
-    </joint><link name="r_elbow_flex_link">
-
-
-      <inertial>
-        <mass value="1.90327"/>
-        <origin xyz="0.01014 0.00032 -0.01211"/>
-        <inertia ixx="0.00346541989" ixy="0.00004066825" ixz="0.00043171614" iyy="0.00441606455" iyz="-0.00003968914" izz="0.00359156824"/>
-      </inertial>
-
-      <visual name="r_elbow_flex_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_elbow_flex_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/elbow_flex.stl"/>
-        </geometry>
-      </visual>
-
-      <collision name="r_elbow_flex_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_elbow_flex_collision_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/convex/elbow_flex_convex.stlb"/>
-        </geometry>
-      </collision>
-    </link><joint name="r_forearm_roll_joint" type="continuous">
-      <axis xyz="-1 0 0"/>
-
-      <limit effort="30" velocity="3.6"/>
-      <safety_controller k_velocity="1"/>
-      <calibration reference_position="0.0"/>
-
-      <dynamics damping="0.1"/>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <parent link="r_elbow_flex_link"/>
-      <child link="r_forearm_roll_link"/>
-    </joint><link name="r_forearm_roll_link">
-
-      <inertial>
-        <!-- dummy masses, to be removed -->
-        <mass value="0.1"/>
-        <origin xyz="0 0 0"/>
-        <inertia ixx="0.01" ixy="0.00" ixz="0.00" iyy="0.01" iyz="0.00" izz="0.01"/>
-      </inertial>
-      <visual name="r_forearm_roll_visual"><!-- TODO: need a mesh for this -->
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_forearm_roll_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/forearm_roll.stl"/>
-        </geometry>
-      </visual>
-      <collision name="r_forearm_roll_collision"> <!-- TODO: collision tag should be optional -->
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_forearm_roll_collision_geom">
-          <box size="0.1 0.1 0.1"/>
-        </geometry>
-      </collision>
-    </link><joint name="r_forearm_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <parent link="r_forearm_roll_link"/>
-      <child link="r_forearm_link"/>
-    </joint><link name="r_forearm_link">
-
-      <inertial>
-        <mass value="2.57968"/>
-        <origin xyz="0.18791 -0.00017 -0.00912"/>
-        <inertia ixx="0.00364857222" ixy="0.00005215877" ixz="0.00071534842" iyy="0.01507736897" iyz="-0.00001310770" izz="0.01659310749"/>
-      </inertial>
-
-      <visual name="r_forearm_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_forearm_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/forearm.stl"/>
-        </geometry>
-      </visual>
-
-      <collision name="r_forearm_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_forearm_collision_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/convex/forearm_convex.stlb"/>
-        </geometry>
-      </collision>
-    </link><joint name="r_wrist_flex_joint" type="revolute">
-      <axis xyz="0 -1 0"/>
-      <limit effort="200" lower="-0.1" upper="2.2" velocity="3.078"/>
-      <safety_controller k_position="20" k_velocity="4" soft_lower_limit="0.1" soft_upper_limit="2.0"/>
-      <dynamics damping="0.1"/>
-      <calibration reference_position="0.4363"/>
-      <origin rpy="0 0 0" xyz="0.32025 0 0"/>
-      <parent link="r_forearm_link"/>
-      <child link="r_wrist_flex_link"/>
-    </joint><link name="r_wrist_flex_link">
-
-      <inertial>
-        <mass value="0.61402"/>
-        <origin xyz="-0.00157 0.0 -0.00075"/>
-        <inertia ixx="0.00065165722" ixy="0.00000028864" ixz="0.00000303477" iyy="0.00019824443" iyz="-0.00000022645" izz="0.00064450498"/>
-      </inertial>
-
-      <visual name="r_wrist_flex_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_wrist_flex_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/wrist_flex.stl"/>
-        </geometry>
-      </visual>
-
-      <collision name="r_wrist_flex_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_wrist_flex_collision_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/convex/wrist_flex_convex.stlb"/>
-        </geometry>
-      </collision>
-    </link><joint name="r_wrist_roll_joint" type="continuous">
-      <axis xyz="1 0 0"/>
-      <limit effort="10" k_velocity="2" velocity="3.6"/>
-      <dynamics damping="0.1"/>
-      <calibration reference_position="1.67" values="1.5 -1"/>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <parent link="r_wrist_flex_link"/>
-      <child link="r_wrist_roll_link"/>
-    </joint><link name="r_wrist_roll_link">
-      <inertial>
-        <!-- dummy masses, to be removed.  wrist roll masses are on "gripper_palm" -->
-        <mass value="0.1"/>
-        <origin xyz="0 0 0"/>
-        <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
-      </inertial>
-      <visual name="r_wrist_roll_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_wrist_roll_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/wrist_roll.stl"/>
-        </geometry>
-      </visual>
-      <collision name="r_wrist_roll_collision">
-        <origin rpy="0 0 0" xyz="0.0 0 0"/>
-        <geometry name="r_wrist_roll_collision_geom">
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-        <verbose value="Yes"/>
-      </collision>
-    </link><gazebo reference="r_pan_link">
-      <sensor:contact name="r_shoulder_pan_contact_sensor">
-        <geom>r_shoulder_pan_collision</geom>
-        <updateRate>100.0</updateRate>
-        <controller:ros_bumper name="r_shoulder_pan_ros_bumper_controller" plugin="libros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>r_shoulder_pan_bumper</bumperTopicName>
-          <interface:bumper name="r_shoulder_pan_ros_bumper_iface"/>
-        </controller:ros_bumper>
-      </sensor:contact>
-      <material value="PR2/Blue"/>
-      <turnGravityOff>true</turnGravityOff>
-      <stopKd value="1.0"/>
-      <stopKp value="1000000.0"/>
-    </gazebo><gazebo reference="r_shoulder_lift_link">
-      <sensor:contact name="r_shoulder_lift_contact_sensor">
-        <geom>r_shoulder_lift_collision_geom</geom>
-        <updateRate>100.0</updateRate>
-        <controller:ros_bumper name="r_shoulder_lift_ros_bumper_controller" plugin="libros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>r_r_shoulder_lift_bumper</bumperTopicName>
-          <interface:bumper name="r_shoulder_lift_ros_bumper_iface"/>
-        </controller:ros_bumper>
-      </sensor:contact>
-      <turnGravityOff>true</turnGravityOff>
-      <stopKd value="1.0"/>
-      <stopKp value="1000000.0"/>
-    </gazebo><gazebo reference="r_upper_arm_roll_link">
-      <mesh value="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_ogre/Media/models/upper_arm_roll.mesh"/>
-      <material value="PR2/RollLinks"/>
-      <turnGravityOff>true</turnGravityOff>
-      <stopKd value="1.0"/>
-      <stopKp value="1000000.0"/>
-      <fudgeFactor value="0.5"/>
-    </gazebo><gazebo reference="r_upper_arm_link">
-      <sensor:contact name="r_upper_arm_contact_sensor">
-        <geom>r_upper_arm_collision_geom</geom><!-- TODO -->
-        <updateRate>100.0</updateRate>
-        <controller:ros_bumper name="r_upper_arm_ros_bumper_controller" plugin="libros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>r_upper_arm_bumper</bumperTopicName>
-          <interface:bumper name="r_upper_arm_ros_bumper_iface"/>
-        </controller:ros_bumper>
-      </sensor:contact>
-      <turnGravityOff>true</turnGravityOff>
-      <material value="PR2/Green"/>
-    </gazebo><gazebo reference="r_elbow_flex_link">
-      <sensor:contact name="r_elbow_flex_contact_sensor">
-        <geom>r_elbow_flex_collision_geom</geom>
-        <updateRate>100.0</updateRate>
-        <controller:ros_bumper name="r_elbow_flex_ros_bumper_controller" plugin="libros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>r_elbow_flex_bumper</bumperTopicName>
-          <interface:bumper name="r_elbow_flex_ros_bumper_iface"/>
-        </controller:ros_bumper>
-      </sensor:contact>
-      <material value="PR2/Grey"/>
-      <stopKd value="1.0"/>
-      <stopKp value="1000000.0"/>
-      <turnGravityOff>true</turnGravityOff>
-    </gazebo><gazebo reference="r_forearm_roll_link">
-      <material value="PR2/RollLinks"/>
-      <fudgeFactor value="0.5"/>
-      <turnGravityOff>true</turnGravityOff>
-    </gazebo><gazebo reference="r_forearm_link">
-      <turnGravityOff>true</turnGravityOff>
-      <sensor:contact name="r_forearm_contact_sensor">
-        <geom>r_forearm_collision</geom>
-        <updateRate>100.0</updateRate>
-        <controller:ros_bumper name="r_forearm_ros_bumper_controller" plugin="libros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>r_forearm_bumper</bumperTopicName>
-          <interface:bumper name="r_forearm_ros_bumper_iface"/>
-        </controller:ros_bumper>
-      </sensor:contact>
-      <material value="PR2/Blue"/>
-    </gazebo><gazebo reference="r_wrist_flex_link">
-      <turnGravityOff>true</turnGravityOff>
-      <sensor:contact name="r_wrist_flex_contact_sensor">
-        <geom>r_wrist_flex_collision</geom>
-        <updateRate>100.0</updateRate>
-        <controller:ros_bumper name="r_wrist_flex_ros_bumper_controller" plugin="libros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>r_wrist_flex_bumper</bumperTopicName>
-          <interface:bumper name="r_wrist_flex_ros_bumper_iface"/>
-        </controller:ros_bumper>
-      </sensor:contact>
-      <material value="PR2/Grey"/>
-      <stopKd value="1.0"/>
-      <stopKp value="1000000.0"/>
-    </gazebo><gazebo reference="r_wrist_roll_link">
-      <turnGravityOff>true</turnGravityOff>
-      <sensor:contact name="r_wrist_roll_contact_sensor">
-        <geom>r_wrist_roll_collision</geom>
-        <updateRate>100.0</updateRate>
-        <controller:ros_bumper name="r_wrist_roll_ros_bumper_controller" plugin="libros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>r_wrist_roll_bumper</bumperTopicName>
-          <interface:bumper name="r_wrist_roll_ros_bumper_iface"/>
-        </controller:ros_bumper>
-      </sensor:contact>
-      <material value="PR2/RollLinks"/>
-      <fudgeFactor value="0.5"/>
-    </gazebo><transmission name="r_shoulder_pan_trans" type="SimpleTransmission">
-      <actuator name="r_shoulder_pan_motor"/>
-      <joint name="r_shoulder_pan_joint"/>
-      <mechanicalReduction>63.16</mechanicalReduction>
-    </transmission><transmission name="r_shoulder_lift_trans" type="SimpleTransmission">
-      <actuator name="r_shoulder_lift_motor"/>
-      <joint name="r_shoulder_lift_joint"/>
-      <mechanicalReduction>61.89</mechanicalReduction>
-    </transmission><transmission name="r_upper_arm_roll_trans" type="SimpleTransmission">
-      <actuator name="r_upper_arm_roll_motor"/>
-      <joint name="r_upper_arm_roll_joint"/>
-      <mechanicalReduction>32.65</mechanicalReduction>
-    </transmission><transmission name="r_elbow_flex_trans" type="SimpleTransmission">
-      <actuator name="r_elbow_flex_motor"/>
-      <joint name="r_elbow_flex_joint"/>
-      <mechanicalReduction>-36.17</mechanicalReduction>
-    </transmission><transmission name="r_forearm_roll_trans" type="SimpleTransmission">
-      <actuator name="r_forearm_roll_motor"/>
-      <joint name="r_forearm_roll_joint"/>
-      <mechanicalReduction>90.5142857143</mechanicalReduction>
-    </transmission><transmission name="r_wrist_trans" type="WristTransmission">
-      <rightActuator name="r_wrist_r_motor"/>
-      <leftActuator name="r_wrist_l_motor"/>
-      <flexJoint mechanicalReduction="60.1714285714" name="r_wrist_flex_joint"/>
-      <rollJoint mechanicalReduction="60.1714285714" name="r_wrist_roll_joint"/>
-    </transmission><rviz reference="r_shoulder_pan_link">
-      <material value="PR2/Blue"/>
-    </rviz><rviz reference="r_shoulder_lift_link">
-      <material value="PR2/Grey"/>
-    </rviz><rviz reference="r_upper_arm_roll_link">
-      <material value="PR2/RollLinks"/>
-    </rviz><rviz reference="r_upper_arm_link">
-      <material value="PR2/Green"/>
-    </rviz><rviz reference="r_elbow_flex_link">
-      <material value="PR2/Grey"/>
-    </rviz><rviz reference="r_forearm_roll_link">
-      <material value="PR2/RollLinks"/>
-    </rviz><rviz reference="r_forearm_link">
-      <material value="PR2/Blue"/>
-    </rviz><rviz reference="r_wrist_flex_link">
-      <material value="PR2/Grey"/>
-    </rviz><rviz reference="r_wrist_roll_link">
-      <material value="PR2/RollLinks"/>
-    </rviz>
-  <joint name="r_gripper_palm_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <parent link="r_wrist_roll_link"/>
-      <child link="r_gripper_palm_link"/>
-    </joint><link name="r_gripper_palm_link">
-      <inertial>
-        <mass value="0.58007"/>
-        <origin rpy="0 0 0" xyz="0.06623 0.00053 -0.00119"/>
-        <inertia ixx="0.00035223921" ixy="-0.00001580476" ixz="-0.00000091750" iyy="0.00067741312" iyz="-0.00000059554" izz="0.00086563316"/>
-      </inertial>
-
-      <visual name="r_gripper_palm_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_gripper_palm_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/gripper_palm.stl"/>
-        </geometry>
-      </visual>
-      <collision name="r_gripper_palm_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_gripper_palm_collision_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/convex/gripper_palm_convex.stlb"/>
-        </geometry>
-        <verbose value="Yes"/>
-      </collision>
-    </link><joint name="r_gripper_float_joint" type="prismatic">
-      <limit effort="0" lower="-0.05" upper="0.001"/>
-      <safety_controller soft_lower_limit="-0.05" soft_upper_limit="0.001"/>
-      <axis xyz="1 0 0"/>
-      <dynamics damping="10.0"/>
-      <origin rpy="0 0 0" xyz="0.05 0 0"/>
-      <parent link="r_gripper_palm_link"/>
-      <child link="r_gripper_float_link"/>
-    </joint><link name="r_gripper_float_link">
-
-      <inertial>
-        <mass value="0.01"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
-      </inertial>
-      <visual name="r_gripper_float_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_gripper_float_visual_geom">
-          <box size="0.001 0.001 0.001"/>
-        </geometry>
-      </visual>
-      <collision name="r_gripper_float_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_gripper_float_collision_geom">
-          <box size="0.001 0.001 0.001"/>
-        </geometry>
-        <verbose value="Yes"/>
-      </collision>
-    </link><joint name="r_gripper_tool_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="0.18 0 0"/>
-      <parent link="r_gripper_palm_link"/>
-      <child link="r_gripper_tool_frame"/>
-    </joint><link name="r_gripper_tool_frame">
-
-      <!-- TODO: all that follows is made-up and useless, but the old parser crashes without it -->
-      <inertial>
-        <mass value="0"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
-      </inertial>
-      <visual name="r_gripper_tool_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_gripper_tool_visual_geom">
-          <box size="0.001 0.001 0.001"/>
-        </geometry>
-      </visual>
-      <collision name="r_gripper_tool_collision">
-        <origin rpy="0 0 0" xyz="-0.1 0 0"/> <!-- move it out of the way -->
-        <geometry name="r_gripper_tool_collision_geom">
-          <box size="0.001 0.001 0.001"/>
-        </geometry>
-      </collision>
-    </link><joint name="r_gripper_l_finger_joint" type="revolute">
-      <axis xyz="0 0 1"/>
-
-      <limit effort="100.0" lower="0.0" upper="0.548" velocity="0.5"/><safety_controller k_position="100.0" k_velocity="100.0" soft_lower_limit="0.0" soft_upper_limit="0.548"/>
-
-      <dynamics damping="0.2"/>
-      <origin rpy="0 0 0" xyz="0.07691 0.01 0"/>
-      <parent link="r_gripper_palm_link"/>
-      <child link="r_gripper_l_finger_link"/>
-    </joint><link name="r_gripper_l_finger_link">
-
-
-      <inertial>
-        <mass value="0.17126"/>
-        <origin rpy="0 0 0" xyz="0.03598 0.01730 -0.00164"/>
-        <inertia ixx="0.00007756198" ixy="0.00000149095" ixz="-0.00000983385" iyy="0.00019708305" iyz="-0.00000306125" izz="0.00018105446"/>
-      </inertial>
-      <visual name="r_gripper_l_finger_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_gripper_l_finger_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/upper_finger_l.stl"/>
-        </geometry>
-      </visual>
-      <collision name="r_gripper_l_finger_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_gripper_l_finger_collision_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/convex/upper_finger_l_convex.stlb"/>
-        </geometry>
-        <verbose value="Yes"/>
-      </collision>
-    </link><joint name="r_gripper_r_finger_joint" type="revolute">
-      <axis xyz="0 0 -1"/>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <limit effort="100.0" lower="0.0" upper="0.548" velocity="0.5"/><safety_controller k_position="100.0" k_velocity="100.0" soft_lower_limit="0.0" soft_upper_limit="0.548"/>
-      <dynamics damping="0.2"/>
-      <mimic mult="1" name="r_gripper_l_finger_joint" offset="0"/>
-      <origin rpy="0 0 0" xyz="0.07691 -0.01 0"/>
-      <parent link="r_gripper_palm_link"/>
-      <child link="r_gripper_r_finger_link"/>
-    </joint><link name="r_gripper_r_finger_link">
-
-
-      <inertial>
-        <mass value="0.17389"/>
-        <origin rpy="0 0 0" xyz="0.03576 -0.01736 -0.00095"/>
-        <inertia ixx="0.00007738410" ixy="-0.00000209309" ixz="-0.00000836228" iyy="0.00019847383" iyz="0.00000246110" izz="0.00018106988"/>
-      </inertial>
-      <visual name="r_gripper_r_finger_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_gripper_r_finger_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/upper_finger_r.stl"/>
-        </geometry>
-      </visual>
-      <collision name="r_gripper_r_finger_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_gripper_r_finger_collision_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/convex/upper_finger_r_convex.stlb"/>
-        </geometry>
-        <verbose value="Yes"/>
-      </collision>
-    </link><joint name="r_gripper_l_finger_tip_joint" type="revolute">
-      <axis xyz="0 0 -1"/>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <limit effort="100.0" lower="0.0" upper="0.548" velocity="0.5"/><safety_controller k_position="100.0" k_velocity="100.0" soft_lower_limit="0.0" soft_upper_limit="0.548"/>
-      <dynamics damping="0.01"/>
-      <mimic mult="1" name="r_gripper_l_finger_joint" offset="0"/>
-      <origin rpy="0 0 0" xyz="0.09137 0.00495 0"/>
-      <parent link="r_gripper_l_finger_link"/>
-      <child link="r_gripper_l_finger_tip_link"/>
-    </joint><link name="r_gripper_l_finger_tip_link">
-
-
-      <inertial>
-        <mass value="0.04419"/>
-        <origin rpy="0 0 0" xyz="0.00423 0.00284 0.0"/>
-        <inertia ixx="0.00000837047" ixy="0.00000583632" ixz="0.0" iyy="0.00000987067" iyz="0.0" izz="0.00001541768"/>
-      </inertial>
-      <visual name="r_gripper_l_finger_tip_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_gripper_l_finger_tip_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/finger_tip_l.stl"/>
-        </geometry>
-      </visual>
-      <collision name="r_gripper_l_finger_tip_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_gripper_l_finger_tip_collision_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/convex/finger_tip_l_convex.stlb"/>
-        </geometry>
-        <verbose value="Yes"/>
-      </collision>
-    </link><joint name="r_gripper_r_finger_tip_joint" type="revolute">
-      <axis xyz="0 0 1"/>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <limit effort="100.0" lower="0.0" upper="0.548" velocity="0.5"/><safety_controller k_position="100.0" k_velocity="100.0" soft_lower_limit="0.0" soft_upper_limit="0.548"/>
-      <dynamics damping="0.01"/>
-      <mimic mult="1" name="r_gripper_l_finger_joint" offset="0"/>
-      <origin rpy="0 0 0" xyz="0.09137 -0.00495 0"/>
-      <parent link="r_gripper_r_finger_link"/>
-      <child link="r_gripper_r_finger_tip_link"/>
-    </joint><link name="r_gripper_r_finger_tip_link">
-
-
-      <inertial>
-        <mass value="0.04419"/>
-        <origin rpy="0 0 0" xyz="0.00423 -0.00284 0.0"/>
-        <inertia ixx="0.00000837047" ixy="-0.00000583632" ixz="0.0" iyy="0.00000987067" iyz="0.0" izz="0.00001541768"/>
-      </inertial>
-      <visual name="r_gripper_r_finger_tip_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_gripper_r_finger_tip_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/finger_tip_r.stl"/>
-        </geometry>
-      </visual>
-      <collision name="r_gripper_r_finger_tip_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="r_gripper_r_finger_tip_collision_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/convex/finger_tip_r_convex.stlb"/>
-        </geometry>
-        <verbose value="Yes"/>
-      </collision>
-    </link><gazebo reference="r_gripper_l_finger_link">
-      <turnGravityOff>true</turnGravityOff>
-      <sensor:contact name="r_gripper_l_finger_contact_sensor">
-        <geom>r_gripper_l_finger_collision</geom>
-        <updateRate>100.0</updateRate>
-        <controller:ros_bumper name="r_gripper_l_finger_ros_bumper_controller" plugin="libros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>r_gripper_l_finger_bumper</bumperTopicName>
-          <interface:bumper name="r_gripper_l_finger_ros_bumper_iface"/>
-        </controller:ros_bumper>
-      </sensor:contact>
-      <mu1 value="500.0"/>
-      <mu2 value="500.0"/>
-      <kp value="1000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/Grey"/>
-      <!-- for "${prefix}_l_finger_joint"-->
-      <stopKd value="1.0"/>
-      <stopKp value="10000000.0"/>
-      <fudgeFactor value="1.0"/>
-      <provideFeedback value="true"/>
-    </gazebo><gazebo reference="r_gripper_r_finger_link">
-      <turnGravityOff>true</turnGravityOff>
-      <sensor:contact name="r_gripper_r_finger_contact_sensor">
-        <geom>r_gripper_r_finger_collision</geom>
-        <updateRate>100.0</updateRate>
-        <controller:ros_bumper name="r_gripper_r_finger_ros_bumper_controller" plugin="libros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>r_gripper_r_finger_bumper</bumperTopicName>
-          <interface:bumper name="r_gripper_r_finger_ros_bumper_iface"/>
-        </controller:ros_bumper>
-      </sensor:contact>
-      <mu1 value="500.0"/>
-      <mu2 value="500.0"/>
-      <kp value="1000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/Grey"/>
-      <stopKd value="1.0"/>
-      <stopKp value="10000000.0"/>
-      <fudgeFactor value="1.0"/>
-      <provideFeedback value="true"/>
-      <!-- for "${prefix}_r_finger_joint"-->
-      <mimicKp value="0.0"/>
-      <mimicKd value="0.0"/>
-      <mimicFMax value="0.0"/>
-    </gazebo><gazebo reference="r_gripper_l_finger_tip_link">
-      <turnGravityOff>true</turnGravityOff>
-      <selfCollide value="false"/>
-      <sensor:contact name="r_gripper_l_finger_tip_contact_sensor">
-        <geom>r_gripper_l_finger_tip_collision</geom>
-        <updateRate>100.0</updateRate>
-        <controller:ros_bumper name="r_gripper_l_finger_tip_ros_bumper_controller" plugin="libros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>r_gripper_l_finger_tip_bumper</bumperTopicName>
-          <interface:bumper name="r_gripper_l_finger_tip_ros_bumper_iface"/>
-        </controller:ros_bumper>
-      </sensor:contact>
-      <mu1 value="500.0"/>
-      <mu2 value="500.0"/>
-      <kp value="10000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/Green"/>
-      <stopKd value="1.0"/>
-      <stopKp value="10000000.0"/>
-      <fudgeFactor value="1.0"/>
-      <provideFeedback value="true"/>
-      <mimicKp value="0.0"/>
-      <mimicKd value="0.0"/>
-      <mimicFMax value="0.0"/>
-    </gazebo><gazebo reference="r_gripper_r_finger_tip_link">
-      <turnGravityOff>true</turnGravityOff>
-      <selfCollide value="false"/>
-      <sensor:contact name="r_gripper_r_finger_tip_contact_sensor">
-        <geom>r_gripper_r_finger_tip_collision</geom>
-        <updateRate>100.0</updateRate>
-        <controller:ros_bumper name="r_gripper_r_finger_tip_ros_bumper_controller" plugin="libros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>r_gripper_r_finger_tip_bumper</bumperTopicName>
-          <interface:bumper name="r_gripper_r_finger_tip_ros_bumper_iface"/>
-        </controller:ros_bumper>
-      </sensor:contact>
-      <mu1 value="500.0"/>
-      <mu2 value="500.0"/>
-      <kp value="10000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/Green"/>
-      <stopKd value="1.0"/>
-      <stopKp value="10000000.0"/>
-      <fudgeFactor value="1.0"/>
-      <mimicKp value="0.0"/>
-      <mimicKd value="0.0"/>
-      <mimicFMax value="0.0"/>
-      <provideFeedback value="true"/>
-      <controller:ros_p3d name="p3d_r_gripper_l_finger_controller" plugin="libros_p3d.so">
-        <alwaysOn>true</alwaysOn>
-        <updateRate>100.0</updateRate>
-        <bodyName>r_gripper_l_finger_link</bodyName>
-        <topicName>r_gripper_l_finger_pose_ground_truth</topicName>
-        <gaussianNoise>0.0</gaussianNoise>
-        <frameName>map</frameName>
-        <interface:position name="p3d_r_gripper_l_finger_position_iface"/>
-      </controller:ros_p3d>
-      <controller:ros_f3d name="f3d_r_gripper_l_finger_controller" plugin="libros_f3d.so">
-        <alwaysOn>true</alwaysOn>
-        <updateRate>100.0</updateRate>
-        <bodyName>r_gripper_l_finger_link</bodyName>
-        <topicName>r_gripper_l_finger_force_ground_truth</topicName>
-        <frameName>r_gripper_l_finger_link</frameName>
-        <interface:position name="f3d_r_gripper_l_finger_force_iface"/>
-      </controller:ros_f3d>
-    </gazebo><rviz reference="r_gripper_l_finger_link">
-      <material value="PR2/Grey"/>
-    </rviz><rviz reference="r_gripper_r_finger_link">
-      <material value="PR2/Grey"/>
-    </rviz><rviz reference="r_gripper_l_finger_tip_link">
-      <material value="PR2/Green"/>
-    </rviz><rviz reference="r_gripper_r_finger_tip_link">
-      <material value="PR2/Green"/>
-    </rviz><joint name="r_gripper_joint" type="prismatic">
-      <axis xyz="0 0 1"/>
-      <limit effort="100.0" lower="0.0" upper="0.09" velocity="0.2"/>
-      <safety_controller k_position="20.0" k_velocity="500.0" soft_lower_limit="0.0" soft_upper_limit="0.089"/>
-    </joint><gazebo reference="r_gripper_palm_link">
-      <turnGravityOff>true</turnGravityOff>
-      <sensor:contact name="r_gripper_palm_contact_sensor">
-        <geom>r_gripper_palm_collision</geom>
-        <updateRate>100.0</updateRate>
-        <controller:ros_bumper name="r_gripper_palm_ros_bumper_controller" plugin="libros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>r_gripper_palm_bumper</bumperTopicName>
-          <interface:bumper name="r_gripper_palm_ros_bumper_iface"/>
-        </controller:ros_bumper>
-      </sensor:contact>
-      <material value="PR2/Red"/>
-    </gazebo><gazebo reference="r_gripper_float_link">
-      <turnGravityOff>true</turnGravityOff>
-      <material value="PR2/White"/>
-      <joint:slider name="r_gripper_l_finger_tip_slider_joint">
-          <body1>r_gripper_l_finger_tip_link</body1>
-          <body2>r_gripper_float_link</body2>
-          <anchor>r_gripper_l_finger_tip_link</anchor>
-          <axis>0 1 0</axis>
-          <anchorOffset>0 0 0</anchorOffset>
-      </joint:slider>
-      <joint:slider name="r_gripper_r_finger_tip_slider_joint">
-          <body1>r_gripper_r_finger_tip_link</body1>
-          <body2>r_gripper_float_link</body2>
-          <anchor>r_gripper_r_finger_tip_link</anchor>
-          <axis>0 1 0</axis>
-          <anchorOffset>0 0 0</anchorOffset>
-      </joint:slider>
-      <!--
-      <joint:slider name="${side}_gripper_finger_tip_slider_joint">
-          <body1>${side}_gripper_l_finger_tip_link</body1>
-          <body2>${side}_gripper_r_finger_tip_link</body2>
-          <anchor>${side}_gripper_l_finger_tip_link</anchor>
-          <axis>0 1 0</axis>
-          <anchorOffset>0 0 0</anchorOffset>
-          <lowStop>0.0</lowStop>
-          <highStop>0.08</highStop>
-      </joint:slider>
-      -->
-    </gazebo><gazebo> <!-- not in a link but in the robot/root element, so no reference attribute -->
-      <controller:ros_p3d name="p3d_r_gripper_palm_controller" plugin="libros_p3d.so">
-        <alwaysOn>true</alwaysOn>
-        <updateRate>100.0</updateRate>
-        <bodyName>r_gripper_palm_link</bodyName>
-        <topicName>r_gripper_palm_pose_ground_truth</topicName>
-        <xyzOffsets>0 0 0</xyzOffsets>
-        <rpyOffsets>0 0 0</rpyOffsets>
-        <gaussianNoise>0.0</gaussianNoise>
-        <frameName>map</frameName>
-        <interface:position name="p3d_r_gripper_palm_position_iface"/>
-      </controller:ros_p3d>
-    </gazebo><gazebo reference="r_gripper_tool_link">
-      <turnGravityOff>true</turnGravityOff>
-      <material value="PR2/Grey"/>
-    </gazebo><rviz reference="r_gripper_palm_link">
-      <material value="PR2/Red"/>
-    </rviz><rviz reference="r_gripper_float_link">
-      <material value="PR2/White"/>
-    </rviz><rviz reference="r_gripper_tool_link">
-      <material value="PR2/Grey"/>
-    </rviz><transmission name="r_gripper_trans" type="PR2GripperTransmission">
-      <actuator name="r_gripper_motor"/>
-      <gap_joint gear_ratio="29.16" mechanical_reduction="1.0" name="r_gripper_joint" screw_reduction="0.002"/>
-      <passive_joint name="r_gripper_l_finger_joint"/>
-      <passive_joint name="r_gripper_r_finger_joint"/>
-      <passive_joint name="r_gripper_r_finger_tip_joint"/>
-      <passive_joint name="r_gripper_l_finger_tip_joint"/>
-    </transmission>
-
-  <joint name="l_shoulder_pan_joint" type="revolute">
-      <axis xyz="0 0 1"/>
-      <origin rpy="0 0 0" xyz="0.0 0.188 0.0"/> <!-- transform from parent link to this joint frame -->
-      <parent link="torso_lift_link"/>
-      <child link="l_shoulder_pan_link"/>
-      <limit effort="30" lower="-0.714601836603" upper="2.2853981634" velocity="2.088"/>
-
-      <dynamics damping="10.0"/>
-
-      <safety_controller k_position="100" k_velocity="10" soft_lower_limit="-0.564601836603" soft_upper_limit="2.1353981634"/>
-
-      <!-- joint angle when the rising or the falling flag is activated on PR2 -->
-      <calibration reference_position="0.785398163397"/>
-
-    </joint><link name="l_shoulder_pan_link">
-
-
-      <inertial>
-        <mass value="25.799322"/>
-        <origin rpy="0 0 0" xyz="-0.001201 0.024513 -0.098231"/>
-        <inertia ixx="0.866179142480" ixy="-0.06086507933" ixz="-0.12118061183" iyy="0.87421714893" iyz="-0.05886609911" izz="0.27353821674"/>
-      </inertial>
-
-      <visual name="l_shoulder_pan_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_shoulder_pan_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/shoulder_yaw.stl"/>
-        </geometry>
-        <texture name="PR2/Blue"/>
-      </visual>
-
-      <collision name="l_shoulder_pan_collision">
-        <origin rpy="0 0 0" xyz="0.0 0 0.0"/>
-        <geometry name="l_shoulder_pan_collision_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/convex/shoulder_yaw_convex.stlb"/>
-        </geometry>
-      </collision>
-    </link><joint name="l_shoulder_lift_joint" type="revolute">
-      <axis xyz="0 1 0"/>
-
-      <!-- Limits updated from Function's CAD values as of 2009_02_24 (link_data.xls) -->
-      <limit effort="30" lower="-0.5236" upper="1.3963" velocity="2.082"/>
-             
-      <safety_controller k_position="100" k_velocity="10" soft_lower="-0.3536" soft_upper="1.3463"/>
-      <calibration reference_position="0.0"/>
-      <dynamics damping="10.0"/>
-      <origin rpy="0 0 0" xyz="0.1 0 0"/>
-      <parent link="l_shoulder_pan_link"/>
-      <child link="l_shoulder_lift_link"/>
-    </joint><link name="l_shoulder_lift_link">
-
-      <inertial>
-        <mass value="2.74988"/>
-        <origin rpy="0 0 0" xyz="0.02195 -0.02664 -0.03127"/>
-        <inertia ixx="0.02105584615" ixy="0.00496704022" ixz="-0.00194808955" iyy="0.02127223737" iyz="0.00110425490" izz="0.01975753814"/>
-      </inertial>
-
-      <visual name="l_shoulder_lift_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_shoulder_lift_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/shoulder_lift.stl"/>
-        </geometry>
-        <texture name="PR2/Grey"/>
-      </visual>
-
-      <collision name="l_shoulder_lift_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_shoulder_lift_collision_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/convex/shoulder_lift_convex.stlb"/>
-        </geometry>
-      </collision>
-    </link><joint name="l_upper_arm_roll_joint" type="revolute">
-      <axis xyz="1 0 0"/>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <parent link="l_shoulder_lift_link"/>
-      <child link="l_upper_arm_roll_link"/>
-      <limit effort="30" lower="-0.8" upper="3.9" velocity="3.27"/>
-      <safety_controller k_position="100" k_velocity="2" soft_lower_limit="-0.65" soft_upper_limit="3.75"/>
-      <calibration reference_position="1.57079632679"/>
-      <dynamics damping="0.1"/>
-    </joint><link name="l_upper_arm_roll_link">
-
-
-      <inertial>
-        <!-- dummy mass, to be removed -->
-        <mass value="0.1"/>
-        <origin rpy="0 0 0" xyz="0.0 0 0"/>
-        <inertia ixx="0.01" ixy="0.00" ixz="0.00" iyy="0.01" iyz="0.00" izz="0.01"/>
-      </inertial>
-      <visual name="l_upper_arm_roll_visual">
-        <!-- TODO: This component doesn't actually have a mesh -->
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_upper_arm_roll_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/upper_arm_roll.stl"/>
-        </geometry>
-      </visual>
-      <collision name="l_upper_arm_roll_collision">
-        <!-- TODO: collision tag should be optional -->
-        <origin rpy="0 0 0" xyz="0.0 0 0"/>
-        <geometry name="l_upper_arm_roll_collision_geom">
-          <box size="0.1 0.1 0.1"/>
-        </geometry>
-      </collision>
-    </link><joint name="l_upper_arm_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <parent link="l_upper_arm_roll_link"/>
-      <child link="l_upper_arm_link"/>
-    </joint><link name="l_upper_arm_link">
-
-      <inertial>
-        <!-- NOTE:reflect==-1 for right side, reflect==1 for the left side -->
-        <mass value="6.01769"/>
-        <origin xyz="0.21405 0.01658 -0.00057"/>
-        <inertia ixx="0.01530603856" ixy="-0.00339324862" ixz="0.00060765455" iyy="0.07473694455" iyz="-0.00019953729" izz="0.07601594191"/>
-      </inertial>
-
-      <visual name="{side}_upper_arm_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_upper_arm_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/upper_arm.stl"/>
-        </geometry>
-      </visual>
-
-      <collision name="l_upper_arm_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_upper_arm_collision_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/convex/upper_arm_convex.stlb"/>
-        </geometry>
-      </collision>
-
-    </link><joint name="l_elbow_flex_joint" type="revolute">
-      <axis xyz="0 1 0"/>
-      <limit effort="30" lower="-2.3" upper="0.1" velocity="3.3"/>
-      <safety_controller k_position="100" k_velocity="3" soft_lower_limit="-2.05" soft_upper_limit="0.0"/>
-      <calibration reference_position="-1.1606"/>
-      <dynamics damping="1.0"/>
-      <origin rpy="0 0 0" xyz="0.4 0 0"/>
-      <parent link="l_upper_arm_link"/>
-      <child link="l_elbow_flex_link"/>
-    </joint><link name="l_elbow_flex_link">
-
-
-      <inertial>
-        <mass value="1.90327"/>
-        <origin xyz="0.01014 0.00032 -0.01211"/>
-        <inertia ixx="0.00346541989" ixy="0.00004066825" ixz="0.00043171614" iyy="0.00441606455" iyz="-0.00003968914" izz="0.00359156824"/>
-      </inertial>
-
-      <visual name="l_elbow_flex_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_elbow_flex_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/elbow_flex.stl"/>
-        </geometry>
-      </visual>
-
-      <collision name="l_elbow_flex_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_elbow_flex_collision_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/convex/elbow_flex_convex.stlb"/>
-        </geometry>
-      </collision>
-    </link><joint name="l_forearm_roll_joint" type="continuous">
-      <axis xyz="-1 0 0"/>
-
-      <limit effort="30" velocity="3.6"/>
-      <safety_controller k_velocity="1"/>
-      <calibration reference_position="0.0"/>
-
-      <dynamics damping="0.1"/>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <parent link="l_elbow_flex_link"/>
-      <child link="l_forearm_roll_link"/>
-    </joint><link name="l_forearm_roll_link">
-
-      <inertial>
-        <!-- dummy masses, to be removed -->
-        <mass value="0.1"/>
-        <origin xyz="0 0 0"/>
-        <inertia ixx="0.01" ixy="0.00" ixz="0.00" iyy="0.01" iyz="0.00" izz="0.01"/>
-      </inertial>
-      <visual name="l_forearm_roll_visual"><!-- TODO: need a mesh for this -->
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_forearm_roll_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/forearm_roll.stl"/>
-        </geometry>
-      </visual>
-      <collision name="l_forearm_roll_collision"> <!-- TODO: collision tag should be optional -->
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_forearm_roll_collision_geom">
-          <box size="0.1 0.1 0.1"/>
-        </geometry>
-      </collision>
-    </link><joint name="l_forearm_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <parent link="l_forearm_roll_link"/>
-      <child link="l_forearm_link"/>
-    </joint><link name="l_forearm_link">
-
-      <inertial>
-        <mass value="2.57968"/>
-        <origin xyz="0.18791 -0.00017 -0.00912"/>
-        <inertia ixx="0.00364857222" ixy="0.00005215877" ixz="0.00071534842" iyy="0.01507736897" iyz="-0.00001310770" izz="0.01659310749"/>
-      </inertial>
-
-      <visual name="l_forearm_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_forearm_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/forearm.stl"/>
-        </geometry>
-      </visual>
-
-      <collision name="l_forearm_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_forearm_collision_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/convex/forearm_convex.stlb"/>
-        </geometry>
-      </collision>
-    </link><joint name="l_wrist_flex_joint" type="revolute">
-      <axis xyz="0 -1 0"/>
-      <limit effort="200" lower="-0.1" upper="2.2" velocity="3.078"/>
-      <safety_controller k_position="20" k_velocity="4" soft_lower_limit="0.1" soft_upper_limit="2.0"/>
-      <dynamics damping="0.1"/>
-      <calibration reference_position="0.4363"/>
-      <origin rpy="0 0 0" xyz="0.32025 0 0"/>
-      <parent link="l_forearm_link"/>
-      <child link="l_wrist_flex_link"/>
-    </joint><link name="l_wrist_flex_link">
-
-      <inertial>
-        <mass value="0.61402"/>
-        <origin xyz="-0.00157 0.0 -0.00075"/>
-        <inertia ixx="0.00065165722" ixy="0.00000028864" ixz="0.00000303477" iyy="0.00019824443" iyz="-0.00000022645" izz="0.00064450498"/>
-      </inertial>
-
-      <visual name="l_wrist_flex_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_wrist_flex_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/wrist_flex.stl"/>
-        </geometry>
-      </visual>
-
-      <collision name="l_wrist_flex_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_wrist_flex_collision_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/convex/wrist_flex_convex.stlb"/>
-        </geometry>
-      </collision>
-    </link><joint name="l_wrist_roll_joint" type="continuous">
-      <axis xyz="1 0 0"/>
-      <limit effort="10" k_velocity="2" velocity="3.6"/>
-      <dynamics damping="0.1"/>
-      <calibration reference_position="1.67" values="1.5 -1"/>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <parent link="l_wrist_flex_link"/>
-      <child link="l_wrist_roll_link"/>
-    </joint><link name="l_wrist_roll_link">
-      <inertial>
-        <!-- dummy masses, to be removed.  wrist roll masses are on "gripper_palm" -->
-        <mass value="0.1"/>
-        <origin xyz="0 0 0"/>
-        <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
-      </inertial>
-      <visual name="l_wrist_roll_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_wrist_roll_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/wrist_roll.stl"/>
-        </geometry>
-      </visual>
-      <collision name="l_wrist_roll_collision">
-        <origin rpy="0 0 0" xyz="0.0 0 0"/>
-        <geometry name="l_wrist_roll_collision_geom">
-          <box size="0.01 0.01 0.01"/>
-        </geometry>
-        <verbose value="Yes"/>
-      </collision>
-    </link><gazebo reference="l_shoulder_pan_link">
-      <sensor:contact name="l_shoulder_pan_contact_sensor">
-        <geom>l_shoulder_pan_collision</geom>
-        <updateRate>100.0</updateRate>
-        <controller:ros_bumper name="l_shoulder_pan_ros_bumper_controller" plugin="libros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>l_shoulder_pan_bumper</bumperTopicName>
-          <interface:bumper name="l_shoulder_pan_ros_bumper_iface"/>
-        </controller:ros_bumper>
-      </sensor:contact>
-      <material value="PR2/Blue"/>
-      <turnGravityOff>true</turnGravityOff>
-      <stopKd value="1.0"/>
-      <stopKp value="1000000.0"/>
-    </gazebo><gazebo reference="l_shoulder_lift_link">
-      <sensor:contact name="l_shoulder_lift_contact_sensor">
-        <geom>l_shoulder_lift_collision_geom</geom>
-        <updateRate>100.0</updateRate>
-        <controller:ros_bumper name="l_shoulder_lift_ros_bumper_controller" plugin="libros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>l_r_shoulder_lift_bumper</bumperTopicName>
-          <interface:bumper name="l_shoulder_lift_ros_bumper_iface"/>
-        </controller:ros_bumper>
-      </sensor:contact>
-      <turnGravityOff>true</turnGravityOff>
-      <stopKd value="1.0"/>
-      <stopKp value="1000000.0"/>
-    </gazebo><gazebo reference="l_upper_arm_roll_link">
-      <mesh value="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_ogre/Media/models/upper_arm_roll.mesh"/>
-      <material value="PR2/RollLinks"/>
-      <turnGravityOff>true</turnGravityOff>
-      <stopKd value="1.0"/>
-      <stopKp value="1000000.0"/>
-      <fudgeFactor value="0.5"/>
-    </gazebo><gazebo reference="l_upper_arm_link">
-      <sensor:contact name="l_upper_arm_contact_sensor">
-        <geom>l_upper_arm_collision_geom</geom><!-- TODO -->
-        <updateRate>100.0</updateRate>
-        <controller:ros_bumper name="l_upper_arm_ros_bumper_controller" plugin="libros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>l_upper_arm_bumper</bumperTopicName>
-          <interface:bumper name="l_upper_arm_ros_bumper_iface"/>
-        </controller:ros_bumper>
-      </sensor:contact>
-      <turnGravityOff>true</turnGravityOff>
-      <material value="PR2/Green"/>
-    </gazebo><gazebo reference="l_elbow_flex_link">
-      <sensor:contact name="l_elbow_flex_contact_sensor">
-        <geom>l_elbow_flex_collision_geom</geom>
-        <updateRate>100.0</updateRate>
-        <controller:ros_bumper name="l_elbow_flex_ros_bumper_controller" plugin="libros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>l_elbow_flex_bumper</bumperTopicName>
-          <interface:bumper name="l_elbow_flex_ros_bumper_iface"/>
-        </controller:ros_bumper>
-      </sensor:contact>
-      <material value="PR2/Grey"/>
-      <stopKd value="1.0"/>
-      <stopKp value="1000000.0"/>
-      <turnGravityOff>true</turnGravityOff>
-    </gazebo><gazebo reference="l_forearm_roll_link">
-      <material value="PR2/RollLinks"/>
-      <fudgeFactor value="0.5"/>
-      <turnGravityOff>true</turnGravityOff>
-    </gazebo><gazebo reference="l_forearm_link">
-      <turnGravityOff>true</turnGravityOff>
-      <sensor:contact name="l_forearm_contact_sensor">
-        <geom>l_forearm_collision</geom>
-        <updateRate>100.0</updateRate>
-        <controller:ros_bumper name="l_forearm_ros_bumper_controller" plugin="libros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>l_forearm_bumper</bumperTopicName>
-          <interface:bumper name="l_forearm_ros_bumper_iface"/>
-        </controller:ros_bumper>
-      </sensor:contact>
-      <material value="PR2/Blue"/>
-    </gazebo><gazebo reference="l_wrist_flex_link">
-      <turnGravityOff>true</turnGravityOff>
-      <sensor:contact name="l_wrist_flex_contact_sensor">
-        <geom>l_wrist_flex_collision</geom>
-        <updateRate>100.0</updateRate>
-        <controller:ros_bumper name="l_wrist_flex_ros_bumper_controller" plugin="libros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>l_wrist_flex_bumper</bumperTopicName>
-          <interface:bumper name="l_wrist_flex_ros_bumper_iface"/>
-        </controller:ros_bumper>
-      </sensor:contact>
-      <material value="PR2/Grey"/>
-      <stopKd value="1.0"/>
-      <stopKp value="1000000.0"/>
-    </gazebo><gazebo reference="l_wrist_roll_link">
-      <turnGravityOff>true</turnGravityOff>
-      <sensor:contact name="l_wrist_roll_contact_sensor">
-        <geom>l_wrist_roll_collision</geom>
-        <updateRate>100.0</updateRate>
-        <controller:ros_bumper name="l_wrist_roll_ros_bumper_controller" plugin="libros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>l_wrist_roll_bumper</bumperTopicName>
-          <interface:bumper name="l_wrist_roll_ros_bumper_iface"/>
-        </controller:ros_bumper>
-      </sensor:contact>
-      <material value="PR2/RollLinks"/>
-      <fudgeFactor value="0.5"/>
-    </gazebo><transmission name="l_shoulder_pan_trans" type="SimpleTransmission">
-      <actuator name="l_shoulder_pan_motor"/>
-      <joint name="l_shoulder_pan_joint"/>
-      <mechanicalReduction>63.16</mechanicalReduction>
-    </transmission><transmission name="l_shoulder_lift_trans" type="SimpleTransmission">
-      <actuator name="l_shoulder_lift_motor"/>
-      <joint name="l_shoulder_lift_joint"/>
-      <mechanicalReduction>61.89</mechanicalReduction>
-    </transmission><transmission name="l_upper_arm_roll_trans" type="SimpleTransmission">
-      <actuator name="l_upper_arm_roll_motor"/>
-      <joint name="l_upper_arm_roll_joint"/>
-      <mechanicalReduction>32.65</mechanicalReduction>
-    </transmission><transmission name="l_elbow_flex_trans" type="SimpleTransmission">
-      <actuator name="l_elbow_flex_motor"/>
-      <joint name="l_elbow_flex_joint"/>
-      <mechanicalReduction>-36.17</mechanicalReduction>
-    </transmission><transmission name="l_forearm_roll_trans" type="SimpleTransmission">
-      <actuator name="l_forearm_roll_motor"/>
-      <joint name="l_forearm_roll_joint"/>
-      <mechanicalReduction>90.5142857143</mechanicalReduction>
-    </transmission><transmission name="l_wrist_trans" type="WristTransmission">
-      <rightActuator name="l_wrist_r_motor"/>
-      <leftActuator name="l_wrist_l_motor"/>
-      <flexJoint mechanicalReduction="60.1714285714" name="l_wrist_flex_joint"/>
-      <rollJoint mechanicalReduction="60.1714285714" name="l_wrist_roll_joint"/>
-    </transmission><rviz reference="l_shoulder_pan_link">
-      <material value="PR2/Blue"/>
-    </rviz><rviz reference="l_shoulder_lift_link">
-      <material value="PR2/Grey"/>
-    </rviz><rviz reference="l_upper_arm_roll_link">
-      <material value="PR2/RollLinks"/>
-    </rviz><rviz reference="l_upper_arm_link">
-      <material value="PR2/Green"/>
-    </rviz><rviz reference="l_elbow_flex_link">
-      <material value="PR2/Grey"/>
-    </rviz><rviz reference="l_forearm_roll_link">
-      <material value="PR2/RollLinks"/>
-    </rviz><rviz reference="l_forearm_link">
-      <material value="PR2/Blue"/>
-    </rviz><rviz reference="l_wrist_flex_link">
-      <material value="PR2/Grey"/>
-    </rviz><rviz reference="l_wrist_roll_link">
-      <material value="PR2/RollLinks"/>
-    </rviz>
-  <joint name="l_gripper_palm_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <parent link="l_wrist_roll_link"/>
-      <child link="l_gripper_palm_link"/>
-    </joint><link name="l_gripper_palm_link">
-      <inertial>
-        <mass value="0.58007"/>
-        <origin rpy="0 0 0" xyz="0.06623 0.00053 -0.00119"/>
-        <inertia ixx="0.00035223921" ixy="-0.00001580476" ixz="-0.00000091750" iyy="0.00067741312" iyz="-0.00000059554" izz="0.00086563316"/>
-      </inertial>
-
-      <visual name="l_gripper_palm_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_gripper_palm_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/gripper_palm.stl"/>
-        </geometry>
-      </visual>
-      <collision name="l_gripper_palm_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_gripper_palm_collision_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/convex/gripper_palm_convex.stlb"/>
-        </geometry>
-        <verbose value="Yes"/>
-      </collision>
-    </link><joint name="l_gripper_float_joint" type="prismatic">
-      <limit effort="0" lower="-0.05" upper="0.001"/>
-      <safety_controller soft_lower_limit="-0.05" soft_upper_limit="0.001"/>
-      <axis xyz="1 0 0"/>
-      <dynamics damping="10.0"/>
-      <origin rpy="0 0 0" xyz="0.05 0 0"/>
-      <parent link="l_gripper_palm_link"/>
-      <child link="l_gripper_float_link"/>
-    </joint><link name="l_gripper_float_link">
-
-      <inertial>
-        <mass value="0.01"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
-      </inertial>
-      <visual name="l_gripper_float_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_gripper_float_visual_geom">
-          <box size="0.001 0.001 0.001"/>
-        </geometry>
-      </visual>
-      <collision name="l_gripper_float_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_gripper_float_collision_geom">
-          <box size="0.001 0.001 0.001"/>
-        </geometry>
-        <verbose value="Yes"/>
-      </collision>
-    </link><joint name="l_gripper_tool_joint" type="fixed">
-      <origin rpy="0 0 0" xyz="0.18 0 0"/>
-      <parent link="l_gripper_palm_link"/>
-      <child link="l_gripper_tool_frame"/>
-    </joint><link name="l_gripper_tool_frame">
-
-      <!-- TODO: all that follows is made-up and useless, but the old parser crashes without it -->
-      <inertial>
-        <mass value="0"/>
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
-      </inertial>
-      <visual name="l_gripper_tool_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_gripper_tool_visual_geom">
-          <box size="0.001 0.001 0.001"/>
-        </geometry>
-      </visual>
-      <collision name="l_gripper_tool_collision">
-        <origin rpy="0 0 0" xyz="-0.1 0 0"/> <!-- move it out of the way -->
-        <geometry name="l_gripper_tool_collision_geom">
-          <box size="0.001 0.001 0.001"/>
-        </geometry>
-      </collision>
-    </link><joint name="l_gripper_l_finger_joint" type="revolute">
-      <axis xyz="0 0 1"/>
-
-      <limit effort="100.0" lower="0.0" upper="0.548" velocity="0.5"/><safety_controller k_position="100.0" k_velocity="100.0" soft_lower_limit="0.0" soft_upper_limit="0.548"/>
-
-      <dynamics damping="0.2"/>
-      <origin rpy="0 0 0" xyz="0.07691 0.01 0"/>
-      <parent link="l_gripper_palm_link"/>
-      <child link="l_gripper_l_finger_link"/>
-    </joint><link name="l_gripper_l_finger_link">
-
-
-      <inertial>
-        <mass value="0.17126"/>
-        <origin rpy="0 0 0" xyz="0.03598 0.01730 -0.00164"/>
-        <inertia ixx="0.00007756198" ixy="0.00000149095" ixz="-0.00000983385" iyy="0.00019708305" iyz="-0.00000306125" izz="0.00018105446"/>
-      </inertial>
-      <visual name="l_gripper_l_finger_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_gripper_l_finger_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/upper_finger_l.stl"/>
-        </geometry>
-      </visual>
-      <collision name="l_gripper_l_finger_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_gripper_l_finger_collision_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/convex/upper_finger_l_convex.stlb"/>
-        </geometry>
-        <verbose value="Yes"/>
-      </collision>
-    </link><joint name="l_gripper_r_finger_joint" type="revolute">
-      <axis xyz="0 0 -1"/>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <limit effort="100.0" lower="0.0" upper="0.548" velocity="0.5"/><safety_controller k_position="100.0" k_velocity="100.0" soft_lower_limit="0.0" soft_upper_limit="0.548"/>
-      <dynamics damping="0.2"/>
-      <mimic mult="1" name="l_gripper_l_finger_joint" offset="0"/>
-      <origin rpy="0 0 0" xyz="0.07691 -0.01 0"/>
-      <parent link="l_gripper_palm_link"/>
-      <child link="l_gripper_r_finger_link"/>
-    </joint><link name="l_gripper_r_finger_link">
-
-
-      <inertial>
-        <mass value="0.17389"/>
-        <origin rpy="0 0 0" xyz="0.03576 -0.01736 -0.00095"/>
-        <inertia ixx="0.00007738410" ixy="-0.00000209309" ixz="-0.00000836228" iyy="0.00019847383" iyz="0.00000246110" izz="0.00018106988"/>
-      </inertial>
-      <visual name="l_gripper_r_finger_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_gripper_r_finger_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/upper_finger_r.stl"/>
-        </geometry>
-      </visual>
-      <collision name="l_gripper_r_finger_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_gripper_r_finger_collision_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/convex/upper_finger_r_convex.stlb"/>
-        </geometry>
-        <verbose value="Yes"/>
-      </collision>
-    </link><joint name="l_gripper_l_finger_tip_joint" type="revolute">
-      <axis xyz="0 0 -1"/>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <limit effort="100.0" lower="0.0" upper="0.548" velocity="0.5"/><safety_controller k_position="100.0" k_velocity="100.0" soft_lower_limit="0.0" soft_upper_limit="0.548"/>
-      <dynamics damping="0.01"/>
-      <mimic mult="1" name="l_gripper_l_finger_joint" offset="0"/>
-      <origin rpy="0 0 0" xyz="0.09137 0.00495 0"/>
-      <parent link="l_gripper_l_finger_link"/>
-      <child link="l_gripper_l_finger_tip_link"/>
-    </joint><link name="l_gripper_l_finger_tip_link">
-
-
-      <inertial>
-        <mass value="0.04419"/>
-        <origin rpy="0 0 0" xyz="0.00423 0.00284 0.0"/>
-        <inertia ixx="0.00000837047" ixy="0.00000583632" ixz="0.0" iyy="0.00000987067" iyz="0.0" izz="0.00001541768"/>
-      </inertial>
-      <visual name="l_gripper_l_finger_tip_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_gripper_l_finger_tip_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/finger_tip_l.stl"/>
-        </geometry>
-      </visual>
-      <collision name="l_gripper_l_finger_tip_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_gripper_l_finger_tip_collision_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/convex/finger_tip_l_convex.stlb"/>
-        </geometry>
-        <verbose value="Yes"/>
-      </collision>
-    </link><joint name="l_gripper_r_finger_tip_joint" type="revolute">
-      <axis xyz="0 0 1"/>
-      <origin rpy="0 0 0" xyz="0 0 0"/>
-      <limit effort="100.0" lower="0.0" upper="0.548" velocity="0.5"/><safety_controller k_position="100.0" k_velocity="100.0" soft_lower_limit="0.0" soft_upper_limit="0.548"/>
-      <dynamics damping="0.01"/>
-      <mimic mult="1" name="l_gripper_l_finger_joint" offset="0"/>
-      <origin rpy="0 0 0" xyz="0.09137 -0.00495 0"/>
-      <parent link="l_gripper_r_finger_link"/>
-      <child link="l_gripper_r_finger_tip_link"/>
-    </joint><link name="l_gripper_r_finger_tip_link">
-
-
-      <inertial>
-        <mass value="0.04419"/>
-        <origin rpy="0 0 0" xyz="0.00423 -0.00284 0.0"/>
-        <inertia ixx="0.00000837047" ixy="-0.00000583632" ixz="0.0" iyy="0.00000987067" iyz="0.0" izz="0.00001541768"/>
-      </inertial>
-      <visual name="l_gripper_r_finger_tip_visual">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_gripper_r_finger_tip_visual_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/finger_tip_r.stl"/>
-        </geometry>
-      </visual>
-      <collision name="l_gripper_r_finger_tip_collision">
-        <origin rpy="0 0 0" xyz="0 0 0"/>
-        <geometry name="l_gripper_r_finger_tip_collision_geom">
-          <mesh filename="/u/meeussen/ros/ros-pkg-git/stacks/pr2/pr2_defs/meshes/convex/finger_tip_r_convex.stlb"/>
-        </geometry>
-        <verbose value="Yes"/>
-      </collision>
-    </link><gazebo reference="l_gripper_l_finger_link">
-      <turnGravityOff>true</turnGravityOff>
-      <sensor:contact name="l_gripper_l_finger_contact_sensor">
-        <geom>l_gripper_l_finger_collision</geom>
-        <updateRate>100.0</updateRate>
-        <controller:ros_bumper name="l_gripper_l_finger_ros_bumper_controller" plugin="libros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>l_gripper_l_finger_bumper</bumperTopicName>
-          <interface:bumper name="l_gripper_l_finger_ros_bumper_iface"/>
-        </controller:ros_bumper>
-      </sensor:contact>
-      <mu1 value="500.0"/>
-      <mu2 value="500.0"/>
-      <kp value="1000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/Grey"/>
-      <!-- for "${prefix}_l_finger_joint"-->
-      <stopKd value="1.0"/>
-      <stopKp value="10000000.0"/>
-      <fudgeFactor value="1.0"/>
-      <provideFeedback value="true"/>
-    </gazebo><gazebo reference="l_gripper_r_finger_link">
-      <turnGravityOff>true</turnGravityOff>
-      <sensor:contact name="l_gripper_r_finger_contact_sensor">
-        <geom>l_gripper_r_finger_collision</geom>
-        <updateRate>100.0</updateRate>
-        <controller:ros_bumper name="l_gripper_r_finger_ros_bumper_controller" plugin="libros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>l_gripper_r_finger_bumper</bumperTopicName>
-          <interface:bumper name="l_gripper_r_finger_ros_bumper_iface"/>
-        </controller:ros_bumper>
-      </sensor:contact>
-      <mu1 value="500.0"/>
-      <mu2 value="500.0"/>
-      <kp value="1000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/Grey"/>
-      <stopKd value="1.0"/>
-      <stopKp value="10000000.0"/>
-      <fudgeFactor value="1.0"/>
-      <provideFeedback value="true"/>
-      <!-- for "${prefix}_r_finger_joint"-->
-      <mimicKp value="0.0"/>
-      <mimicKd value="0.0"/>
-      <mimicFMax value="0.0"/>
-    </gazebo><gazebo reference="l_gripper_l_finger_tip_link">
-      <turnGravityOff>true</turnGravityOff>
-      <selfCollide value="false"/>
-      <sensor:contact name="l_gripper_l_finger_tip_contact_sensor">
-        <geom>l_gripper_l_finger_tip_collision</geom>
-        <updateRate>100.0</updateRate>
-        <controller:ros_bumper name="l_gripper_l_finger_tip_ros_bumper_controller" plugin="libros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>l_gripper_l_finger_tip_bumper</bumperTopicName>
-          <interface:bumper name="l_gripper_l_finger_tip_ros_bumper_iface"/>
-        </controller:ros_bumper>
-      </sensor:contact>
-      <mu1 value="500.0"/>
-      <mu2 value="500.0"/>
-      <kp value="10000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/Green"/>
-      <stopKd value="1.0"/>
-      <stopKp value="10000000.0"/>
-      <fudgeFactor value="1.0"/>
-      <provideFeedback value="true"/>
-      <mimicKp value="0.0"/>
-      <mimicKd value="0.0"/>
-      <mimicFMax value="0.0"/>
-    </gazebo><gazebo reference="l_gripper_r_finger_tip_link">
-      <turnGravityOff>true</turnGravityOff>
-      <selfCollide value="false"/>
-      <sensor:contact name="l_gripper_r_finger_tip_contact_sensor">
-        <geom>l_gripper_r_finger_tip_collision</geom>
-        <updateRate>100.0</updateRate>
-        <controller:ros_bumper name="l_gripper_r_finger_tip_ros_bumper_controller" plugin="libros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>l_gripper_r_finger_tip_bumper</bumperTopicName>
-          <interface:bumper name="l_gripper_r_finger_tip_ros_bumper_iface"/>
-        </controller:ros_bumper>
-      </sensor:contact>
-      <mu1 value="500.0"/>
-      <mu2 value="500.0"/>
-      <kp value="10000000.0"/>
-      <kd value="1.0"/>
-      <material value="PR2/Green"/>
-      <stopKd value="1.0"/>
-      <stopKp value="10000000.0"/>
-      <fudgeFactor value="1.0"/>
-      <mimicKp value="0.0"/>
-      <mimicKd value="0.0"/>
-      <mimicFMax value="0.0"/>
-      <provideFeedback value="true"/>
-      <controller:ros_p3d name="p3d_l_gripper_l_finger_controller" plugin="libros_p3d.so">
-        <alwaysOn>true</alwaysOn>
-        <updateRate>100.0</updateRate>
-        <bodyName>l_gripper_l_finger_link</bodyName>
-        <topicName>l_gripper_l_finger_pose_ground_truth</topicName>
-        <gaussianNoise>0.0</gaussianNoise>
-        <frameName>map</frameName>
-        <interface:position name="p3d_l_gripper_l_finger_position_iface"/>
-      </controller:ros_p3d>
-      <controller:ros_f3d name="f3d_l_gripper_l_finger_controller" plugin="libros_f3d.so">
-        <alwaysOn>true</alwaysOn>
-        <updateRate>100.0</updateRate>
-        <bodyName>l_gripper_l_finger_link</bodyName>
-        <topicName>l_gripper_l_finger_force_ground_truth</topicName>
-        <frameName>l_gripper_l_finger_link</frameName>
-        <interface:position name="f3d_l_gripper_l_finger_force_iface"/>
-      </controller:ros_f3d>
-    </gazebo><rviz reference="l_gripper_l_finger_link">
-      <material value="PR2/Grey"/>
-    </rviz><rviz reference="l_gripper_r_finger_link">
-      <material value="PR2/Grey"/>
-    </rviz><rviz reference="l_gripper_l_finger_tip_link">
-      <material value="PR2/Green"/>
-    </rviz><rviz reference="l_gripper_r_finger_tip_link">
-      <material value="PR2/Green"/>
-    </rviz><joint name="l_gripper_joint" type="prismatic">
-      <axis xyz="0 0 1"/>
-      <limit effort="100.0" lower="0.0" upper="0.09" velocity="0.2"/>
-      <safety_controller k_position="20.0" k_velocity="500.0" soft_lower_limit="0.0" soft_upper_limit="0.089"/>
-    </joint><gazebo reference="l_gripper_palm_link">
-      <turnGravityOff>true</turnGravityOff>
-      <sensor:contact name="l_gripper_palm_contact_sensor">
-        <geom>l_gripper_palm_collision</geom>
-        <updateRate>100.0</updateRate>
-        <controller:ros_bumper name="l_gripper_palm_ros_bumper_controller" plugin="libros_bumper.so">
-          <alwaysOn>true</alwaysOn>
-          <updateRate>100.0</updateRate>
-          <bumperTopicName>l_gripper_palm_bumper</bumperTopicName>
-          <interface:bumper name="l_gripper_palm_ros_bumper_iface"/>
-        </controller:ros_bumper>
-      </sensor:contact>
-      <material value="PR2/Red"/>
-    </gazebo><gazebo reference="l_gripper_float_link">
-      <turnGravityOff>true</turnGravityOff>
-      <material value="PR2/White"/>
-      <joint:slider name="l_gripper_l_finger_tip_slider_joint">
-          <body1>l_gripper_l_finger_tip_link</body1>
-          <body2>l_gripper_float_link</body2>
-          <anchor>l_gripper_l_finger_tip_link</anchor>
-          <axis>0 1 0</axis>
-          <anchorOffset>0 0 0</anchorOffset>
-      </joint:slider>
-      <joint:slider name="l_gripper_r_finger_tip_slider_joint">
-          <body1>l_gripper_r_finger_tip_link</body1>
-          <body2>l_gripper_float_link</body2>
-          <anchor>l_gripper_r_finger_tip_link</anchor>
-          <axis>0 1 0</axis>
-          <anchorOffset>0 0 0</anchorOffset>
-      </joint:slider>
-      <!--
-      <joint:slider name="${side}_gripper_finger_tip_slider_joint">
-          <body1>${side}_gripper_l_finger_tip_link</body1>
-          <body2>${side}_gripper_r_finger_tip_link</body2>
-          <anchor>${side}_gripper_l_finger_tip_link</anchor>
-          <axis>0 1 0</axis>
-          <anchorOffset>0 0 0</anchorOffset>
-          <lowStop>0.0</lowStop>
-          <highStop>0.08</highStop>
-      </joint:slider>
-      -->
-    </gazebo><gazebo> <!-- not in a link but in the robot/root element, so no reference attribute -->
-      <controller:ros_p3d name="p3d_l_gripper_palm_controller" plugin="libros_p3d.so">
-        <alwaysOn>true</alwaysOn>
-        <updateRate>100.0</updateRate>
-        <bodyName>l_gripper_palm_link</bodyName>
-        <topicName>l_gripper_palm_pose_ground_truth</topicName>
-        <xyzOffsets>0 0 0</xyzOffsets>
-        <rpyOffsets>0 0 0</rpyOffsets>
-        <gaussianNoise>0.0</gaussianNoise>
-        <frameName>map</frameName>
-        <interface:position name="p3d_l_gripper_palm_position_iface"/>
-      </controller:ros_p3d>
-    </gazebo><gazebo reference="l_gripper_tool_link">
-      <turnGravityOff>true</turnGravityOff>
-      <material value="PR2/Grey"/>
-    </gazebo><rviz reference="l_gripper_palm_link">
-      <material value="PR2/Red"/>
-    </rviz><rviz reference="l_gripper_float_link">
-      <material value="PR2/White"/>
-    </rviz><rviz reference="l_gripper_tool_link">
-      <material value="PR2/Grey"/>
-    </rviz><transmission name="l_gripper_trans" type="PR2GripperTransmission">
-      <actuator name="l_gripper_motor"/>
-      <gap_joint gear_ratio="29.16" mechanical_reduction="1.0" name="l_gripper_joint" screw_reduction="0.002"/>
-      <passive_joint name="l_gripper_l_finger_joint"/>
-      <passive_joint name="l_gripper_r_finger_joint"/>
-      <passive_joint name="l_gripper_r_finger_tip_joint"/>
-      <passive_joint name="l_gripper_l_finger_tip_joint"/>
-    </transmission>
-
-  <!-- Kinematic chains -->
-  <chain root="torso_lift_link" tip="l_wrist_roll_link"/>
-  <chain root="torso_lift_link" tip="r_wrist_roll_link"/>
-
-</robot>