diff --git a/sdf/1.5/CMakeLists.txt b/sdf/1.5/CMakeLists.txt
index 60fd6a15a..be0dc46b4 100644
--- a/sdf/1.5/CMakeLists.txt
+++ b/sdf/1.5/CMakeLists.txt
@@ -1,16 +1,17 @@
 set (sdfs
   actor.sdf
   altimeter.sdf
-  audio_source.sdf
   audio_sink.sdf
+  audio_source.sdf
   battery.sdf
   box_shape.sdf
   camera.sdf
+  collision_engine.sdf
   collision.sdf
   contact.sdf
   cylinder_shape.sdf
-  frame.sdf
   forcetorque.sdf
+  frame.sdf
   geometry.sdf
   gps.sdf
   gripper.sdf
@@ -20,16 +21,16 @@ set (sdfs
   imu.sdf
   inertial.sdf
   joint.sdf
-  light.sdf
   light_state.sdf
-  link.sdf
+  light.sdf
   link_state.sdf
+  link.sdf
   logical_camera.sdf
   magnetometer.sdf
   material.sdf
   mesh_shape.sdf
-  model.sdf
   model_state.sdf
+  model.sdf
   noise.sdf
   physics.sdf
   plane_shape.sdf
@@ -39,38 +40,39 @@ set (sdfs
   pose.sdf
   projector.sdf
   ray.sdf
-  rfidtag.sdf
   rfid.sdf
+  rfidtag.sdf
   road.sdf
   root.sdf
   scene.sdf
   sensor.sdf
-  spherical_coordinates.sdf
-  sphere_shape.sdf
   sonar.sdf
+  sphere_shape.sdf
+  spherical_coordinates.sdf
   state.sdf
   surface.sdf
   transceiver.sdf
+  urdf.sdf
   visual.sdf
   world.sdf
 )
 
 set (SDF_SCHEMA)
 
-foreach(FIL ${sdfs})
-  get_filename_component(ABS_FIL ${FIL} ABSOLUTE)
-  get_filename_component(FIL_WE ${FIL} NAME_WE)
+set(root root.sdf)
+
+get_filename_component(ABS_FIL ${root} ABSOLUTE)
+get_filename_component(FIL_WE ${root} NAME_WE)
 
-  list(APPEND SDF_SCHEMA "${CMAKE_CURRENT_BINARY_DIR}/${FIL_WE}.xsd")
+list(APPEND SDF_SCHEMA "${CMAKE_CURRENT_BINARY_DIR}/${FIL_WE}.xsd")
 
-  add_custom_command(
-    OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/${FIL_WE}.xsd"
-    COMMAND ${RUBY} ${CMAKE_SOURCE_DIR}/tools/xmlschema.rb
-    ARGS -s ${CMAKE_CURRENT_SOURCE_DIR} -i ${ABS_FIL} -o ${CMAKE_CURRENT_BINARY_DIR}
-    DEPENDS ${ABS_FIL}
-    COMMENT "Running xml schema compiler on ${FIL}"
-    VERBATIM)
-endforeach()
+add_custom_command(
+  OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/${FIL_WE}.xsd"
+  COMMAND ${RUBY} ${CMAKE_SOURCE_DIR}/tools/xmlschema.rb
+  ARGS -s ${CMAKE_CURRENT_SOURCE_DIR} -i ${ABS_FIL} -o ${CMAKE_CURRENT_BINARY_DIR}
+  DEPENDS ${ABS_FIL}
+  COMMENT "Running xml schema compiler on ${root}"
+  VERBATIM)
 
 add_custom_target(schema1_5 ALL DEPENDS ${SDF_SCHEMA})
 
diff --git a/sdf/1.5/schema/types.xsd b/sdf/1.5/schema/types.xsd
index 53f3d8db6..7dfd9ca8d 100644
--- a/sdf/1.5/schema/types.xsd
+++ b/sdf/1.5/schema/types.xsd
@@ -2,7 +2,7 @@
 <xsd:schema xmlns:xsd='http://www.w3.org/2001/XMLSchema'>
   <xsd:simpleType name="vector3">
     <xsd:restriction base="xsd:string">
-      <xsd:pattern value="(\s*(-|\+)?(\d+(\.\d*)?|\.\d+|\d+\.\d+[eE][-\+]?[0-9]+)\s+){2}((-|\+)?(\d+(\.\d*)?|\.\d+|\d+\.\d+[eE][-\+]?[0-9]+))\s*"/>
+      <xsd:pattern value="(\s*(-|\+)?(\d+(\.\d*)?|\.\d+|\d+\.\d+)([eE][-\+]?[0-9]+)?\s+){2}(-|\+)?(\d+(\.\d*)?|\.\d+|\d+\.\d+)([eE][-\+]?[0-9]+)?\s*"/>
     </xsd:restriction>
   </xsd:simpleType>
 
@@ -26,7 +26,7 @@
 
   <xsd:simpleType name="pose">
     <xsd:restriction base="xsd:string">
-      <xsd:pattern value="(\s*(-|\+)?(\d+(\.\d*)?|\.\d+|\d+\.\d+[eE][-\+]?[0-9]+)\s+){5}((-|\+)?(\d+(\.\d*)?|\.\d+|\d+\.\d+[eE][-\+]?[0-9]+))\s*"/>
+      <xsd:pattern value="(\s*(-|\+)?(\d+(\.\d*)?|\.\d+|\d+\.\d+)([eE][-\+]?[0-9]+)?\s+){5}(-|\+)?(\d+(\.\d*)?|\.\d+|\d+\.\d+)([eE][-\+]?[0-9]+)?\s*"/>
     </xsd:restriction>
   </xsd:simpleType>
 
diff --git a/sdf/1.6/CMakeLists.txt b/sdf/1.6/CMakeLists.txt
index c77c738ec..5ea3dc8ac 100644
--- a/sdf/1.6/CMakeLists.txt
+++ b/sdf/1.6/CMakeLists.txt
@@ -1,17 +1,19 @@
 set (sdfs
   actor.sdf
+  air_pressure.sdf
   altimeter.sdf
   atmosphere.sdf
-  audio_source.sdf
   audio_sink.sdf
+  audio_source.sdf
   battery.sdf
   box_shape.sdf
   camera.sdf
+  collision_engine.sdf
   collision.sdf
   contact.sdf
   cylinder_shape.sdf
-  frame.sdf
   forcetorque.sdf
+  frame.sdf
   geometry.sdf
   gps.sdf
   gripper.sdf
@@ -21,16 +23,17 @@ set (sdfs
   imu.sdf
   inertial.sdf
   joint.sdf
-  light.sdf
+  lidar.sdf
   light_state.sdf
-  link.sdf
+  light.sdf
   link_state.sdf
+  link.sdf
   logical_camera.sdf
   magnetometer.sdf
   material.sdf
   mesh_shape.sdf
-  model.sdf
   model_state.sdf
+  model.sdf
   noise.sdf
   physics.sdf
   plane_shape.sdf
@@ -40,38 +43,39 @@ set (sdfs
   pose.sdf
   projector.sdf
   ray.sdf
-  rfidtag.sdf
   rfid.sdf
+  rfidtag.sdf
   road.sdf
   root.sdf
   scene.sdf
   sensor.sdf
-  spherical_coordinates.sdf
-  sphere_shape.sdf
   sonar.sdf
+  sphere_shape.sdf
+  spherical_coordinates.sdf
   state.sdf
   surface.sdf
   transceiver.sdf
+  urdf.sdf
   visual.sdf
   world.sdf
 )
 
 set (SDF_SCHEMA)
 
-foreach(FIL ${sdfs})
-  get_filename_component(ABS_FIL ${FIL} ABSOLUTE)
-  get_filename_component(FIL_WE ${FIL} NAME_WE)
+set(root root.sdf)
+
+get_filename_component(ABS_FIL ${root} ABSOLUTE)
+get_filename_component(FIL_WE ${root} NAME_WE)
 
-  list(APPEND SDF_SCHEMA "${CMAKE_CURRENT_BINARY_DIR}/${FIL_WE}.xsd")
+list(APPEND SDF_SCHEMA "${CMAKE_CURRENT_BINARY_DIR}/${FIL_WE}.xsd")
 
-  add_custom_command(
-    OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/${FIL_WE}.xsd"
-    COMMAND ${RUBY} ${CMAKE_SOURCE_DIR}/tools/xmlschema.rb
-    ARGS -s ${CMAKE_CURRENT_SOURCE_DIR} -i ${ABS_FIL} -o ${CMAKE_CURRENT_BINARY_DIR}
-    DEPENDS ${ABS_FIL}
-    COMMENT "Running xml schema compiler on ${FIL}"
-    VERBATIM)
-endforeach()
+add_custom_command(
+  OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/${FIL_WE}.xsd"
+  COMMAND ${RUBY} ${CMAKE_SOURCE_DIR}/tools/xmlschema.rb
+  ARGS -s ${CMAKE_CURRENT_SOURCE_DIR} -i ${ABS_FIL} -o ${CMAKE_CURRENT_BINARY_DIR}
+  DEPENDS ${ABS_FIL}
+  COMMENT "Running xml schema compiler on ${root}"
+  VERBATIM)
 
 add_custom_target(schema1_6 ALL DEPENDS ${SDF_SCHEMA})
 
diff --git a/sdf/1.6/schema/types.xsd b/sdf/1.6/schema/types.xsd
index 53f3d8db6..7dfd9ca8d 100644
--- a/sdf/1.6/schema/types.xsd
+++ b/sdf/1.6/schema/types.xsd
@@ -2,7 +2,7 @@
 <xsd:schema xmlns:xsd='http://www.w3.org/2001/XMLSchema'>
   <xsd:simpleType name="vector3">
     <xsd:restriction base="xsd:string">
-      <xsd:pattern value="(\s*(-|\+)?(\d+(\.\d*)?|\.\d+|\d+\.\d+[eE][-\+]?[0-9]+)\s+){2}((-|\+)?(\d+(\.\d*)?|\.\d+|\d+\.\d+[eE][-\+]?[0-9]+))\s*"/>
+      <xsd:pattern value="(\s*(-|\+)?(\d+(\.\d*)?|\.\d+|\d+\.\d+)([eE][-\+]?[0-9]+)?\s+){2}(-|\+)?(\d+(\.\d*)?|\.\d+|\d+\.\d+)([eE][-\+]?[0-9]+)?\s*"/>
     </xsd:restriction>
   </xsd:simpleType>
 
@@ -26,7 +26,7 @@
 
   <xsd:simpleType name="pose">
     <xsd:restriction base="xsd:string">
-      <xsd:pattern value="(\s*(-|\+)?(\d+(\.\d*)?|\.\d+|\d+\.\d+[eE][-\+]?[0-9]+)\s+){5}((-|\+)?(\d+(\.\d*)?|\.\d+|\d+\.\d+[eE][-\+]?[0-9]+))\s*"/>
+      <xsd:pattern value="(\s*(-|\+)?(\d+(\.\d*)?|\.\d+|\d+\.\d+)([eE][-\+]?[0-9]+)?\s+){5}(-|\+)?(\d+(\.\d*)?|\.\d+|\d+\.\d+)([eE][-\+]?[0-9]+)?\s*"/>
     </xsd:restriction>
   </xsd:simpleType>
 
diff --git a/sdf/1.7/CMakeLists.txt b/sdf/1.7/CMakeLists.txt
index c170977c9..90bd50c40 100644
--- a/sdf/1.7/CMakeLists.txt
+++ b/sdf/1.7/CMakeLists.txt
@@ -1,17 +1,19 @@
 set (sdfs
   actor.sdf
+  air_pressure.sdf
   altimeter.sdf
   atmosphere.sdf
-  audio_source.sdf
   audio_sink.sdf
+  audio_source.sdf
   battery.sdf
   box_shape.sdf
   camera.sdf
+  collision_engine.sdf
   collision.sdf
   contact.sdf
   cylinder_shape.sdf
-  frame.sdf
   forcetorque.sdf
+  frame.sdf
   geometry.sdf
   gps.sdf
   gripper.sdf
@@ -21,16 +23,17 @@ set (sdfs
   imu.sdf
   inertial.sdf
   joint.sdf
-  light.sdf
+  lidar.sdf
   light_state.sdf
-  link.sdf
+  light.sdf
   link_state.sdf
+  link.sdf
   logical_camera.sdf
   magnetometer.sdf
   material.sdf
   mesh_shape.sdf
-  model.sdf
   model_state.sdf
+  model.sdf
   noise.sdf
   physics.sdf
   plane_shape.sdf
@@ -40,38 +43,39 @@ set (sdfs
   pose.sdf
   projector.sdf
   ray.sdf
-  rfidtag.sdf
   rfid.sdf
+  rfidtag.sdf
   road.sdf
   root.sdf
   scene.sdf
   sensor.sdf
-  spherical_coordinates.sdf
-  sphere_shape.sdf
   sonar.sdf
+  sphere_shape.sdf
+  spherical_coordinates.sdf
   state.sdf
   surface.sdf
   transceiver.sdf
+  urdf.sdf
   visual.sdf
   world.sdf
 )
 
 set (SDF_SCHEMA)
 
-foreach(FIL ${sdfs})
-  get_filename_component(ABS_FIL ${FIL} ABSOLUTE)
-  get_filename_component(FIL_WE ${FIL} NAME_WE)
+set(root root.sdf)
+
+get_filename_component(ABS_FIL ${root} ABSOLUTE)
+get_filename_component(FIL_WE ${root} NAME_WE)
 
-  list(APPEND SDF_SCHEMA "${CMAKE_CURRENT_BINARY_DIR}/${FIL_WE}.xsd")
+list(APPEND SDF_SCHEMA "${CMAKE_CURRENT_BINARY_DIR}/${FIL_WE}.xsd")
 
-  add_custom_command(
-    OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/${FIL_WE}.xsd"
-    COMMAND ${RUBY} ${CMAKE_SOURCE_DIR}/tools/xmlschema.rb
-    ARGS -s ${CMAKE_CURRENT_SOURCE_DIR} -i ${ABS_FIL} -o ${CMAKE_CURRENT_BINARY_DIR}
-    DEPENDS ${ABS_FIL}
-    COMMENT "Running xml schema compiler on ${FIL}"
-    VERBATIM)
-endforeach()
+add_custom_command(
+  OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/${FIL_WE}.xsd"
+  COMMAND ${RUBY} ${CMAKE_SOURCE_DIR}/tools/xmlschema.rb
+  ARGS -s ${CMAKE_CURRENT_SOURCE_DIR} -i ${ABS_FIL} -o ${CMAKE_CURRENT_BINARY_DIR}
+  DEPENDS ${ABS_FIL}
+  COMMENT "Running xml schema compiler on ${root}"
+  VERBATIM)
 
 add_custom_target(schema1_7 ALL DEPENDS ${SDF_SCHEMA})
 
diff --git a/sdf/1.7/schema/types.xsd b/sdf/1.7/schema/types.xsd
index 53f3d8db6..cecfdbf98 100644
--- a/sdf/1.7/schema/types.xsd
+++ b/sdf/1.7/schema/types.xsd
@@ -2,7 +2,7 @@
 <xsd:schema xmlns:xsd='http://www.w3.org/2001/XMLSchema'>
   <xsd:simpleType name="vector3">
     <xsd:restriction base="xsd:string">
-      <xsd:pattern value="(\s*(-|\+)?(\d+(\.\d*)?|\.\d+|\d+\.\d+[eE][-\+]?[0-9]+)\s+){2}((-|\+)?(\d+(\.\d*)?|\.\d+|\d+\.\d+[eE][-\+]?[0-9]+))\s*"/>
+      <xsd:pattern value="(\s*(-|\+)?(\d+(\.\d*)?|\.\d+|\d+\.\d+)([eE][-\+]?[0-9]+)?\s+){2}(-|\+)?(\d+(\.\d*)?|\.\d+|\d+\.\d+)([eE][-\+]?[0-9]+)?\s*"/>
     </xsd:restriction>
   </xsd:simpleType>
 
@@ -26,12 +26,13 @@
 
   <xsd:simpleType name="pose">
     <xsd:restriction base="xsd:string">
-      <xsd:pattern value="(\s*(-|\+)?(\d+(\.\d*)?|\.\d+|\d+\.\d+[eE][-\+]?[0-9]+)\s+){5}((-|\+)?(\d+(\.\d*)?|\.\d+|\d+\.\d+[eE][-\+]?[0-9]+))\s*"/>
+      <xsd:pattern value="((\s*(-|\+)?(\d+(\.\d*)?|\.\d+|\d+\.\d+)([eE][-\+]?[0-9]+)?\s+){5}(-|\+)?(\d+(\.\d*)?|\.\d+|\d+\.\d+)([eE][-\+]?[0-9]+)?\s*)?"/>
     </xsd:restriction>
   </xsd:simpleType>
 
   <xsd:simpleType name="time">
-    <xsd:restriction base="xsd:double">
+    <xsd:restriction base="xsd:string">
+      <xsd:pattern value="\s*\d+\s+\d+\s*"/>
     </xsd:restriction>
   </xsd:simpleType>
 
diff --git a/test/integration/model/world_with_state.sdf b/test/integration/model/world_with_state.sdf
new file mode 100644
index 000000000..96cbac93f
--- /dev/null
+++ b/test/integration/model/world_with_state.sdf
@@ -0,0 +1,4335 @@
+<sdf version='1.7'>
+  <world name='default'>
+    <light name='sun' type='directional'>
+      <cast_shadows>1</cast_shadows>
+      <pose>0 0 10 0 -0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+      <spot>
+        <inner_angle>0</inner_angle>
+        <outer_angle>0</outer_angle>
+        <falloff>0</falloff>
+      </spot>
+    </light>
+    <model name='ground_plane'>
+      <static>1</static>
+      <link name='link'>
+        <collision name='collision'>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <surface>
+            <contact>
+              <collide_bitmask>65535</collide_bitmask>
+              <ode/>
+            </contact>
+            <friction>
+              <ode>
+                <mu>100</mu>
+                <mu2>50</mu2>
+              </ode>
+              <torsional>
+                <ode/>
+              </torsional>
+            </friction>
+            <bounce/>
+          </surface>
+          <max_contacts>10</max_contacts>
+        </collision>
+        <visual name='visual'>
+          <cast_shadows>0</cast_shadows>
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <script>
+              <uri>file://media/materials/scripts/gazebo.material</uri>
+              <name>Gazebo/Grey</name>
+            </script>
+          </material>
+        </visual>
+        <self_collide>0</self_collide>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+    </model>
+    <gravity>0 0 -9.8</gravity>
+    <magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
+    <atmosphere type='adiabatic'/>
+    <physics type='ode'>
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1</real_time_factor>
+      <real_time_update_rate>1000</real_time_update_rate>
+    </physics>
+    <scene>
+      <ambient>0.4 0.4 0.4 1</ambient>
+      <background>0.7 0.7 0.7 1</background>
+      <shadows>1</shadows>
+    </scene>
+    <wind/>
+    <spherical_coordinates>
+      <surface_model>EARTH_WGS84</surface_model>
+      <latitude_deg>0</latitude_deg>
+      <longitude_deg>0</longitude_deg>
+      <elevation>0</elevation>
+      <heading_deg>0</heading_deg>
+    </spherical_coordinates>
+    <model name='pr2'>
+      <static>0</static>
+      <link name='base_footprint'>
+        <self_collide>0</self_collide>
+        <inertial>
+          <mass>118.001</mass>
+          <pose>-0.062421 0 0.201365 0 -0 0</pose>
+          <inertia>
+            <ixx>8.43173</ixx>
+            <ixy>-0.00972</ixy>
+            <ixz>1.90157</ixz>
+            <iyy>8.53305</iyy>
+            <iyz>-0.00738</iyz>
+            <izz>3.78747</izz>
+          </inertia>
+        </inertial>
+        <collision name='base_footprint_geom'>
+          <pose>0 0 0.071 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.001 0.001 0.001</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='base_footprint_geom_visual'>
+          <geometry>
+            <box>
+              <size>0.01 0.01 0.01</size>
+            </box>
+          </geometry>
+        </visual>
+        <collision name='base_footprint_geom_base_bellow_link'>
+          <pose>-0.29 0 0.851 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.05 0.37 0.3</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='base_footprint_geom_base_bellow_link_visual'>
+          <pose>-0.29 0 0.851 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.05 0.37 0.3</size>
+            </box>
+          </geometry>
+        </visual>
+        <collision name='base_footprint_geom_base_link'>
+          <pose>0 0 0.051 0 -0 0</pose>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/base_v0/base_L.stl</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='base_footprint_geom_base_link_visual'>
+          <pose>0 0 0.051 0 -0 0</pose>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/base_v0/base.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <sensor name='base_contact_sensor' type='contact'>
+          <update_rate>100</update_rate>
+          <contact>
+            <collision>base_footprint_geom_base_link</collision>
+            <topic>__default_topic__</topic>
+          </contact>
+          <plugin name='gazebo_contact' filename='libContactPlugin.so'/>
+        </sensor>
+        <sensor name='base_laser' type='ray'>
+          <always_on>1</always_on>
+          <visualize>1</visualize>
+          <update_rate>20</update_rate>
+          <pose>0.275 0 0.303 0 -0 0</pose>
+          <ray>
+            <scan>
+              <horizontal>
+                <samples>640</samples>
+                <resolution>1</resolution>
+                <min_angle>-2.2689</min_angle>
+                <max_angle>2.2689</max_angle>
+              </horizontal>
+              <vertical>
+                <samples>1</samples>
+                <min_angle>0</min_angle>
+                <max_angle>0</max_angle>
+              </vertical>
+            </scan>
+            <range>
+              <min>0.08</min>
+              <max>10</max>
+              <resolution>0.01</resolution>
+            </range>
+          </ray>
+          <plugin name='laser' filename='libRayPlugin.so'/>
+        </sensor>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='bl_caster_rotation_link'>
+        <self_collide>0</self_collide>
+        <pose>-0.2246 0.2246 0.0792 0 -0 0</pose>
+        <inertial>
+          <mass>3.47308</mass>
+          <pose>0 0 0.07 0 -0 0</pose>
+          <inertia>
+            <ixx>0.012412</ixx>
+            <ixy>-0.000712</ixy>
+            <ixz>0.000503</ixz>
+            <iyy>0.015218</iyy>
+            <iyz>-4e-06</iyz>
+            <izz>0.011764</izz>
+          </inertia>
+        </inertial>
+        <collision name='bl_caster_rotation_link_geom'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/base_v0/caster_L.stl</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='bl_caster_rotation_link_geom_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/base_v0/caster.stl</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='bl_caster_l_wheel_link'>
+        <self_collide>0</self_collide>
+        <pose>-0.2246 0.2736 0.0792 0 -0 0</pose>
+        <inertial>
+          <mass>0.44036</mass>
+          <inertia>
+            <ixx>0.012412</ixx>
+            <ixy>-0.000712</ixy>
+            <ixz>0.000503</ixz>
+            <iyy>0.015218</iyy>
+            <iyz>-4e-06</iyz>
+            <izz>0.011764</izz>
+          </inertia>
+          <pose>0 0 0 0 -0 0</pose>
+        </inertial>
+        <collision name='bl_caster_l_wheel_link_geom'>
+          <pose>0 0 0 1.5708 -0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.074792</radius>
+              <length>0.034</length>
+            </cylinder>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='bl_caster_l_wheel_link_geom_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/base_v0/wheel.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='bl_caster_r_wheel_link'>
+        <self_collide>0</self_collide>
+        <pose>-0.2246 0.1756 0.0792 0 -0 0</pose>
+        <inertial>
+          <mass>0.44036</mass>
+          <inertia>
+            <ixx>0.012412</ixx>
+            <ixy>0.000712</ixy>
+            <ixz>0.000503</ixz>
+            <iyy>0.015218</iyy>
+            <iyz>4e-06</iyz>
+            <izz>0.011764</izz>
+          </inertia>
+          <pose>0 0 0 0 -0 0</pose>
+        </inertial>
+        <collision name='bl_caster_r_wheel_link_geom'>
+          <pose>0 0 0 1.5708 -0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.074792</radius>
+              <length>0.034</length>
+            </cylinder>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='bl_caster_r_wheel_link_geom_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/base_v0/wheel.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='br_caster_rotation_link'>
+        <self_collide>0</self_collide>
+        <pose>-0.2246 -0.2246 0.0792 0 -0 0</pose>
+        <inertial>
+          <mass>3.47308</mass>
+          <pose>0 0 0.07 0 -0 0</pose>
+          <inertia>
+            <ixx>0.012412</ixx>
+            <ixy>-0.000712</ixy>
+            <ixz>0.000503</ixz>
+            <iyy>0.015218</iyy>
+            <iyz>-4e-06</iyz>
+            <izz>0.011764</izz>
+          </inertia>
+        </inertial>
+        <collision name='br_caster_rotation_link_geom'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/base_v0/caster_L.stl</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='br_caster_rotation_link_geom_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/base_v0/caster.stl</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='br_caster_l_wheel_link'>
+        <self_collide>0</self_collide>
+        <pose>-0.2246 -0.1756 0.0792 0 -0 0</pose>
+        <inertial>
+          <mass>0.44036</mass>
+          <inertia>
+            <ixx>0.012412</ixx>
+            <ixy>-0.000712</ixy>
+            <ixz>0.000503</ixz>
+            <iyy>0.015218</iyy>
+            <iyz>-4e-06</iyz>
+            <izz>0.011764</izz>
+          </inertia>
+          <pose>0 0 0 0 -0 0</pose>
+        </inertial>
+        <collision name='br_caster_l_wheel_link_geom'>
+          <pose>0 0 0 1.5708 -0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.074792</radius>
+              <length>0.034</length>
+            </cylinder>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='br_caster_l_wheel_link_geom_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/base_v0/wheel.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='br_caster_r_wheel_link'>
+        <self_collide>0</self_collide>
+        <pose>-0.2246 -0.2736 0.0792 0 -0 0</pose>
+        <inertial>
+          <mass>0.44036</mass>
+          <inertia>
+            <ixx>0.012412</ixx>
+            <ixy>-0.000712</ixy>
+            <ixz>0.000503</ixz>
+            <iyy>0.015218</iyy>
+            <iyz>-4e-06</iyz>
+            <izz>0.011764</izz>
+          </inertia>
+          <pose>0 0 0 0 -0 0</pose>
+        </inertial>
+        <collision name='br_caster_r_wheel_link_geom'>
+          <pose>0 0 0 1.5708 -0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.074792</radius>
+              <length>0.034</length>
+            </cylinder>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='br_caster_r_wheel_link_geom_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/base_v0/wheel.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='fl_caster_rotation_link'>
+        <self_collide>0</self_collide>
+        <pose>0.2246 0.2246 0.0792 0 -0 0</pose>
+        <inertial>
+          <mass>3.47308</mass>
+          <pose>0 0 0.07 0 -0 0</pose>
+          <inertia>
+            <ixx>0.012412</ixx>
+            <ixy>-0.000712</ixy>
+            <ixz>0.000503</ixz>
+            <iyy>0.015218</iyy>
+            <iyz>-4e-06</iyz>
+            <izz>0.011764</izz>
+          </inertia>
+        </inertial>
+        <collision name='fl_caster_rotation_link_geom'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/base_v0/caster_L.stl</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='fl_caster_rotation_link_geom_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/base_v0/caster.stl</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='fl_caster_l_wheel_link'>
+        <self_collide>0</self_collide>
+        <pose>0.2246 0.2736 0.0792 0 -0 0</pose>
+        <inertial>
+          <mass>0.44036</mass>
+          <inertia>
+            <ixx>0.012412</ixx>
+            <ixy>-0.000712</ixy>
+            <ixz>0.000503</ixz>
+            <iyy>0.015218</iyy>
+            <iyz>-4e-06</iyz>
+            <izz>0.011764</izz>
+          </inertia>
+          <pose>0 0 0 0 -0 0</pose>
+        </inertial>
+        <collision name='fl_caster_l_wheel_link_geom'>
+          <pose>0 0 0 1.5708 -0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.074792</radius>
+              <length>0.034</length>
+            </cylinder>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='fl_caster_l_wheel_link_geom_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/base_v0/wheel.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='fl_caster_r_wheel_link'>
+        <self_collide>0</self_collide>
+        <pose>0.2246 0.1756 0.0792 0 -0 0</pose>
+        <inertial>
+          <mass>0.44036</mass>
+          <inertia>
+            <ixx>0.012412</ixx>
+            <ixy>-0.000712</ixy>
+            <ixz>0.000503</ixz>
+            <iyy>0.015218</iyy>
+            <iyz>-4e-06</iyz>
+            <izz>0.011764</izz>
+          </inertia>
+          <pose>0 0 0 0 -0 0</pose>
+        </inertial>
+        <collision name='fl_caster_r_wheel_link_geom'>
+          <pose>0 0 0 1.5708 -0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.074792</radius>
+              <length>0.034</length>
+            </cylinder>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='fl_caster_r_wheel_link_geom_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/base_v0/wheel.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='fr_caster_rotation_link'>
+        <self_collide>0</self_collide>
+        <pose>0.2246 -0.2246 0.0792 0 -0 0</pose>
+        <inertial>
+          <mass>3.47308</mass>
+          <pose>0 0 0.07 0 -0 0</pose>
+          <inertia>
+            <ixx>0.012412</ixx>
+            <ixy>-0.000712</ixy>
+            <ixz>0.000503</ixz>
+            <iyy>0.015218</iyy>
+            <iyz>-4e-06</iyz>
+            <izz>0.011764</izz>
+          </inertia>
+        </inertial>
+        <collision name='fr_caster_rotation_link_geom'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/base_v0/caster_L.stl</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='fr_caster_rotation_link_geom_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/base_v0/caster.stl</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='fr_caster_l_wheel_link'>
+        <self_collide>0</self_collide>
+        <pose>0.2246 -0.1756 0.0792 0 -0 0</pose>
+        <inertial>
+          <mass>0.44036</mass>
+          <inertia>
+            <ixx>0.012412</ixx>
+            <ixy>-0.000712</ixy>
+            <ixz>0.000503</ixz>
+            <iyy>0.015218</iyy>
+            <iyz>-4e-06</iyz>
+            <izz>0.011764</izz>
+          </inertia>
+          <pose>0 0 0 0 -0 0</pose>
+        </inertial>
+        <collision name='fr_caster_l_wheel_link_geom'>
+          <pose>0 0 0 1.5708 -0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.074792</radius>
+              <length>0.034</length>
+            </cylinder>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='fr_caster_l_wheel_link_geom_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/base_v0/wheel.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='fr_caster_r_wheel_link'>
+        <self_collide>0</self_collide>
+        <pose>0.2246 -0.2736 0.0792 0 -0 0</pose>
+        <inertial>
+          <mass>0.44036</mass>
+          <inertia>
+            <ixx>0.012412</ixx>
+            <ixy>-0.000712</ixy>
+            <ixz>0.000503</ixz>
+            <iyy>0.015218</iyy>
+            <iyz>-4e-06</iyz>
+            <izz>0.011764</izz>
+          </inertia>
+          <pose>0 0 0 0 -0 0</pose>
+        </inertial>
+        <collision name='fr_caster_r_wheel_link_geom'>
+          <pose>0 0 0 1.5708 -0 0</pose>
+          <geometry>
+            <cylinder>
+              <radius>0.074792</radius>
+              <length>0.034</length>
+            </cylinder>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='fr_caster_r_wheel_link_geom_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/base_v0/wheel.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='torso_lift_link'>
+        <self_collide>0</self_collide>
+        <pose>-0.05 0 0.790675 0 -0 0</pose>
+        <inertial>
+          <mass>36.449</mass>
+          <pose>-0.099499 -4e-06 -0.086764 0 -0 0</pose>
+          <inertia>
+            <ixx>2.79233</ixx>
+            <ixy>0.00428</ixy>
+            <ixz>-0.160631</ixz>
+            <iyy>2.52106</iyy>
+            <iyz>0.029689</iyz>
+            <izz>0.536551</izz>
+          </inertia>
+        </inertial>
+        <collision name='torso_lift_link_geom'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/torso_v0/torso_lift_L.stl</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='torso_lift_link_geom_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/torso_v0/torso_lift.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <sensor name='torso_lift_contact_sensor' type='contact'>
+          <update_rate>100</update_rate>
+          <contact>
+            <collision>torso_lift_link_geom</collision>
+            <topic>__default_topic__</topic>
+          </contact>
+        </sensor>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='head_pan_link'>
+        <self_collide>0</self_collide>
+        <pose>-0.06707 0 1.17213 0 -0 0</pose>
+        <inertial>
+          <mass>6.339</mass>
+          <pose>0.010907 0.031693 0.090507 0 -0 0</pose>
+          <inertia>
+            <ixx>0.032498</ixx>
+            <ixy>0.000636</ixy>
+            <ixz>0.002585</ixz>
+            <iyy>0.046546</iyy>
+            <iyz>-0.002453</iyz>
+            <izz>0.057653</izz>
+          </inertia>
+        </inertial>
+        <collision name='head_pan_link_geom'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/head_v0/head_pan_L.stl</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='head_pan_link_geom_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/head_v0/head_pan.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='head_tilt_link'>
+        <projector name='projector_wg6802418_projector_wg6802418'>
+          <texture>stereo_projection_pattern_high_res_red.png</texture>
+          <fov>0.959931</fov>
+          <near_clip>0.1</near_clip>
+          <far_clip>10</far_clip>
+          <pose>0.0232 0.11 0.1191 0 -1.57079 0</pose>
+        </projector>
+        <self_collide>0</self_collide>
+        <pose>0.00093 0 1.17213 0 -0 0</pose>
+        <inertial>
+          <mass>5.319</mass>
+          <pose>-0.005151 -0.014635 0.072819 0 -0 0</pose>
+          <inertia>
+            <ixx>0.126134</ixx>
+            <ixy>0.000237</ixy>
+            <ixz>0.011881</ixz>
+            <iyy>0.165102</iyy>
+            <iyz>0.001606</iyz>
+            <izz>0.166947</izz>
+          </inertia>
+        </inertial>
+        <collision name='head_tilt_link_geom'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/head_v0/head_tilt_L.stl</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='head_tilt_link_geom_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/head_v0/head_tilt.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <collision name='head_tilt_link_geom_head_mount_kinect_ir_link'>
+          <pose>-0.147067 0.0125 0.291953 0 -0 0</pose>
+          <geometry>
+            <sphere>
+              <radius>0.0005</radius>
+            </sphere>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='head_tilt_link_geom_head_mount_kinect_ir_link_visual'>
+          <pose>-0.147067 0.0125 0.291953 0 -0 0</pose>
+          <geometry>
+            <sphere>
+              <radius>0.00025</radius>
+            </sphere>
+          </geometry>
+          <material>
+            <script>
+              <name>Gazebo/Red</name>
+              <uri>__default__</uri>
+            </script>
+          </material>
+        </visual>
+        <collision name='head_tilt_link_geom_head_mount_kinect_rgb_link'>
+          <pose>-0.147067 0.0175 0.291953 0 -0 0</pose>
+          <geometry>
+            <sphere>
+              <radius>0.0005</radius>
+            </sphere>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='head_tilt_link_geom_head_mount_kinect_rgb_link_visual'>
+          <pose>-0.147067 0.0175 0.291953 0 -0 0</pose>
+          <geometry>
+            <sphere>
+              <radius>0.00025</radius>
+            </sphere>
+          </geometry>
+          <material>
+            <script>
+              <name>Gazebo/Red</name>
+              <uri>__default__</uri>
+            </script>
+          </material>
+        </visual>
+        <collision name='head_tilt_link_geom_head_mount_link'>
+          <pose>-0.1148 0 0.1555 0 -0 0</pose>
+          <geometry>
+            <sphere>
+              <radius>0.0005</radius>
+            </sphere>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='head_tilt_link_geom_head_mount_link_visual'>
+          <pose>-0.1148 0 0.1555 0 -0 0</pose>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/sensors/kinect_prosilica_v0/115x100_swept_back--coarse.STL</uri>
+              <scale>0.001 0.001 0.001</scale>
+            </mesh>
+          </geometry>
+        </visual>
+        <collision name='head_tilt_link_geom_head_mount_prosilica_link'>
+          <pose>-0.161257 0.0125 0.244421 0 -0 0</pose>
+          <geometry>
+            <sphere>
+              <radius>0.0005</radius>
+            </sphere>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='head_tilt_link_geom_head_mount_prosilica_link_visual'>
+          <pose>-0.161257 0.0125 0.244421 0 -0 0</pose>
+          <geometry>
+            <sphere>
+              <radius>0.00025</radius>
+            </sphere>
+          </geometry>
+          <material>
+            <script>
+              <name>Gazebo/Red</name>
+              <uri>__default__</uri>
+            </script>
+          </material>
+        </visual>
+        <collision name='head_tilt_link_geom_head_plate_frame'>
+          <pose>0.0232 0 0.0645 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.01 0.01 0.01</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='head_tilt_link_geom_head_plate_frame_visual'>
+          <pose>0.0232 0 0.0645 0 -0 0</pose>
+          <geometry>
+            <box>
+              <size>0.01 0.01 0.01</size>
+            </box>
+          </geometry>
+          <material>
+            <script>
+              <name>Gazebo/Grey</name>
+              <uri>__default__</uri>
+            </script>
+          </material>
+        </visual>
+        <sensor name='head_mount_sensor' type='camera'>
+          <always_on>0</always_on>
+          <visualize>0</visualize>
+          <update_rate>1</update_rate>
+          <pose>-0.147067 0.0125 0.291953 0 -0 0</pose>
+          <camera>
+            <horizontal_fov>0.994838</horizontal_fov>
+            <image>
+              <width>640</width>
+              <height>480</height>
+              <format>L8</format>
+            </image>
+            <clip>
+              <near>0.01</near>
+              <far>5</far>
+            </clip>
+          </camera>
+        </sensor>
+        <sensor name='head_mount_prosilica_link_sensor' type='camera'>
+          <update_rate>1</update_rate>
+          <pose>-0.161257 0.0125 0.244421 0 -0 0</pose>
+          <camera>
+            <horizontal_fov>1.1205</horizontal_fov>
+            <image>
+              <width>320</width>
+              <height>240</height>
+              <format>R8G8B8</format>
+            </image>
+            <clip>
+              <near>0.1</near>
+              <far>100</far>
+            </clip>
+          </camera>
+        </sensor>
+        <sensor name='head_mount_prosilica_link_sim_pcd_sensor' type='camera'>
+          <update_rate>1</update_rate>
+          <pose>-0.161257 0.0125 0.244421 0 -0 0</pose>
+          <camera>
+            <horizontal_fov>1.1205</horizontal_fov>
+            <image>
+              <width>320</width>
+              <height>240</height>
+              <format>R8G8B8</format>
+            </image>
+            <clip>
+              <near>0.1</near>
+              <far>100</far>
+            </clip>
+          </camera>
+        </sensor>
+        <sensor name='narrow_stereo_gazebo_l_stereo_camera_sensor' type='camera'>
+          <always_on>1</always_on>
+          <visualize>1</visualize>
+          <update_rate>25</update_rate>
+          <pose>0.0682 0.06 0.1146 0 -0 0</pose>
+          <camera>
+            <horizontal_fov>0.785398</horizontal_fov>
+            <image>
+              <width>640</width>
+              <height>480</height>
+              <format>L8</format>
+            </image>
+            <clip>
+              <near>0.1</near>
+              <far>100</far>
+            </clip>
+          </camera>
+        </sensor>
+        <sensor name='narrow_stereo_gazebo_r_stereo_camera_sensor' type='camera'>
+          <always_on>1</always_on>
+          <update_rate>25</update_rate>
+          <pose>0.0682 -0.03 0.1146 0 -0 0</pose>
+          <camera>
+            <horizontal_fov>0.785398</horizontal_fov>
+            <image>
+              <width>640</width>
+              <height>480</height>
+              <format>L8</format>
+            </image>
+            <clip>
+              <near>0.1</near>
+              <far>100</far>
+            </clip>
+          </camera>
+        </sensor>
+        <sensor name='wide_stereo_gazebo_l_stereo_camera_sensor' type='camera'>
+          <update_rate>25</update_rate>
+          <pose>0.0682 0.03 0.1146 0 -0 0</pose>
+          <camera>
+            <horizontal_fov>1.5708</horizontal_fov>
+            <image>
+              <width>640</width>
+              <height>480</height>
+              <format>BAYER_BGGR8</format>
+            </image>
+            <clip>
+              <near>0.1</near>
+              <far>100</far>
+            </clip>
+          </camera>
+        </sensor>
+        <sensor name='wide_stereo_gazebo_r_stereo_camera_sensor' type='camera'>
+          <update_rate>25</update_rate>
+          <pose>0.0682 -0.06 0.1146 0 -0 0</pose>
+          <camera>
+            <horizontal_fov>1.5708</horizontal_fov>
+            <image>
+              <width>640</width>
+              <height>480</height>
+              <format>BAYER_BGGR8</format>
+            </image>
+            <clip>
+              <near>0.1</near>
+              <far>100</far>
+            </clip>
+          </camera>
+        </sensor>
+        <sensor name='high_def_sensor' type='camera'>
+          <update_rate>20</update_rate>
+          <pose>0.069657 -0.11 0.1191 0 -0 0</pose>
+          <camera>
+            <horizontal_fov>0.785398</horizontal_fov>
+            <image>
+              <width>2448</width>
+              <height>2050</height>
+              <format>R8G8B8</format>
+            </image>
+            <clip>
+              <near>0.1</near>
+              <far>100</far>
+            </clip>
+          </camera>
+        </sensor>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='l_shoulder_pan_link'>
+        <self_collide>0</self_collide>
+        <gravity>0</gravity>
+        <pose>-0.05 0.188 0.790675 0 -0 0</pose>
+        <inertial>
+          <mass>25.7993</mass>
+          <pose>-0.001201 0.024513 -0.098231 0 -0 0</pose>
+          <inertia>
+            <ixx>0.866179</ixx>
+            <ixy>-0.060865</ixy>
+            <ixz>-0.121181</ixz>
+            <iyy>0.874217</iyy>
+            <iyz>-0.058866</iyz>
+            <izz>0.273538</izz>
+          </inertia>
+        </inertial>
+        <collision name='l_shoulder_pan_link_geom'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/shoulder_v0/shoulder_pan.stl</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='l_shoulder_pan_link_geom_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/shoulder_v0/shoulder_pan.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='l_shoulder_lift_link'>
+        <self_collide>0</self_collide>
+        <gravity>0</gravity>
+        <pose>0.05 0.188 0.790675 0 -0 0</pose>
+        <inertial>
+          <mass>2.74988</mass>
+          <pose>0.02195 -0.02664 -0.03127 0 -0 0</pose>
+          <inertia>
+            <ixx>0.021056</ixx>
+            <ixy>0.004967</ixy>
+            <ixz>-0.001948</ixz>
+            <iyy>0.021272</iyy>
+            <iyz>0.001104</iyz>
+            <izz>0.019758</izz>
+          </inertia>
+        </inertial>
+        <collision name='l_shoulder_lift_link_geom'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/shoulder_v0/shoulder_lift.stl</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='l_shoulder_lift_link_geom_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/shoulder_v0/shoulder_lift.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='l_upper_arm_roll_link'>
+        <self_collide>0</self_collide>
+        <gravity>0</gravity>
+        <pose>0.05 0.188 0.790675 0 -0 0</pose>
+        <inertial>
+          <mass>6.11769</mass>
+          <pose>0.210551 0.016309 -0.000561 0 -0 0</pose>
+          <inertia>
+            <ixx>0.025306</ixx>
+            <ixy>-0.003393</ixy>
+            <ixz>0.000608</ixz>
+            <iyy>0.084737</iyy>
+            <iyz>-0.0002</iyz>
+            <izz>0.086016</izz>
+          </inertia>
+        </inertial>
+        <collision name='l_upper_arm_roll_link_geom'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/shoulder_v0/upper_arm_roll_L.stl</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='l_upper_arm_roll_link_geom_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/shoulder_v0/upper_arm_roll.stl</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <collision name='l_upper_arm_roll_link_geom_l_upper_arm_link'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/upper_arm_v0/upper_arm.stl</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='l_upper_arm_roll_link_geom_l_upper_arm_link_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/upper_arm_v0/upper_arm.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='l_elbow_flex_link'>
+        <self_collide>0</self_collide>
+        <gravity>0</gravity>
+        <pose>0.45 0.188 0.790675 0 -0 0</pose>
+        <inertial>
+          <mass>1.90327</mass>
+          <pose>0.01014 0.00032 -0.01211 0 -0 0</pose>
+          <inertia>
+            <ixx>0.003465</ixx>
+            <ixy>4.1e-05</ixy>
+            <ixz>0.000432</ixz>
+            <iyy>0.004416</iyy>
+            <iyz>-4e-05</iyz>
+            <izz>0.003592</izz>
+          </inertia>
+        </inertial>
+        <collision name='l_elbow_flex_link_geom'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/upper_arm_v0/elbow_flex.stl</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='l_elbow_flex_link_geom_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/upper_arm_v0/elbow_flex.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='l_forearm_roll_link'>
+        <self_collide>0</self_collide>
+        <gravity>0</gravity>
+        <pose>0.45 0.188 0.790675 0 -0 0</pose>
+        <inertial>
+          <mass>2.68968</mass>
+          <pose>0.180727 -0.000163 -0.008583 0 -0 0</pose>
+          <inertia>
+            <ixx>0.014668</ixx>
+            <ixy>5.2e-05</ixy>
+            <ixz>0.000656</ixz>
+            <iyy>0.026279</iyy>
+            <iyz>-1.3e-05</iyz>
+            <izz>0.027775</izz>
+          </inertia>
+        </inertial>
+        <collision name='l_forearm_roll_link_geom'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/upper_arm_v0/forearm_roll_L.stl</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='l_forearm_roll_link_geom_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/upper_arm_v0/forearm_roll.stl</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <collision name='l_forearm_roll_link_geom_l_forearm_link'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/forearm_v0/forearm.stl</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='l_forearm_roll_link_geom_l_forearm_link_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/forearm_v0/forearm.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <sensor name='l_forearm_cam_sensor' type='camera'>
+          <update_rate>25</update_rate>
+          <pose>0.135 0 0.044 -1.5708 -0.562869 0</pose>
+          <camera>
+            <horizontal_fov>1.5708</horizontal_fov>
+            <image>
+              <width>640</width>
+              <height>480</height>
+              <format>R8G8B8</format>
+            </image>
+            <clip>
+              <near>0.1</near>
+              <far>100</far>
+            </clip>
+          </camera>
+        </sensor>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='l_wrist_flex_link'>
+        <self_collide>0</self_collide>
+        <gravity>0</gravity>
+        <pose>0.771 0.188 0.790675 0 -0 0</pose>
+        <inertial>
+          <mass>0.61402</mass>
+          <pose>-0.00157 0 -0.00075 0 -0 0</pose>
+          <inertia>
+            <ixx>0.000652</ixx>
+            <ixy>0</ixy>
+            <ixz>3e-06</ixz>
+            <iyy>0.000198</iyy>
+            <iyz>0</iyz>
+            <izz>0.000645</izz>
+          </inertia>
+        </inertial>
+        <collision name='l_wrist_flex_link_geom'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/forearm_v0/wrist_flex.stl</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='l_wrist_flex_link_geom_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/forearm_v0/wrist_flex.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='l_wrist_roll_link'>
+        <self_collide>0</self_collide>
+        <gravity>0</gravity>
+        <pose>0.771 0.188 0.790675 0 -0 0</pose>
+        <inertial>
+          <mass>0.68107</mass>
+          <pose>0.056408 0.000451 -0.001014 0 -0 0</pose>
+          <inertia>
+            <ixx>0.011352</ixx>
+            <ixy>-1.6e-05</ixy>
+            <ixz>-1e-06</ixz>
+            <iyy>0.011677</iyy>
+            <iyz>-1e-06</iyz>
+            <izz>0.011866</izz>
+          </inertia>
+        </inertial>
+        <collision name='l_wrist_roll_link_geom'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/forearm_v0/wrist_roll_L.stl</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='l_wrist_roll_link_geom_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/forearm_v0/wrist_roll.stl</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <collision name='l_wrist_roll_link_geom_l_gripper_motor_accelerometer_link'>
+          <geometry>
+            <box>
+              <size>0.001 0.001 0.001</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='l_wrist_roll_link_geom_l_gripper_motor_accelerometer_link_visual'>
+          <geometry>
+            <box>
+              <size>0.001 0.001 0.001</size>
+            </box>
+          </geometry>
+        </visual>
+        <collision name='l_wrist_roll_link_geom_l_gripper_palm_link'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/gripper_v0/gripper_palm.stl</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='l_wrist_roll_link_geom_l_gripper_palm_link_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/gripper_v0/gripper_palm.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='l_gripper_l_finger_link'>
+        <self_collide>0</self_collide>
+        <gravity>0</gravity>
+        <pose>0.84791 0.198 0.790675 0 -0 0</pose>
+        <inertial>
+          <mass>0.17126</mass>
+          <pose>0.03598 0.0173 -0.00164 0 -0 0</pose>
+          <inertia>
+            <ixx>7.8e-05</ixx>
+            <ixy>1e-06</ixy>
+            <ixz>-1e-05</ixz>
+            <iyy>0.000197</iyy>
+            <iyz>-3e-06</iyz>
+            <izz>0.000181</izz>
+          </inertia>
+        </inertial>
+        <collision name='l_gripper_l_finger_link_geom'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/gripper_v0/l_finger.stl</uri>
+            </mesh>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>500</mu>
+                <mu2>500</mu2>
+                <slip1>0</slip1>
+                <slip2>0</slip2>
+              </ode>
+              <torsional>
+                <ode/>
+              </torsional>
+            </friction>
+            <contact>
+              <ode>
+                <kp>1e+06</kp>
+                <kd>1</kd>
+                <max_vel>100</max_vel>
+                <min_depth>0</min_depth>
+              </ode>
+            </contact>
+            <bounce/>
+          </surface>
+          <max_contacts>10</max_contacts>
+        </collision>
+        <visual name='l_gripper_l_finger_link_geom_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/gripper_v0/l_finger.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='l_gripper_l_finger_tip_link'>
+        <self_collide>0</self_collide>
+        <gravity>0</gravity>
+        <pose>0.93928 0.20295 0.790675 0 -0 0</pose>
+        <inertial>
+          <mass>0.04419</mass>
+          <pose>0.00423 0.00284 0 0 -0 0</pose>
+          <inertia>
+            <ixx>8e-06</ixx>
+            <ixy>6e-06</ixy>
+            <ixz>0</ixz>
+            <iyy>1e-05</iyy>
+            <iyz>0</iyz>
+            <izz>1.5e-05</izz>
+          </inertia>
+        </inertial>
+        <collision name='l_gripper_l_finger_tip_link_geom'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/gripper_v0/l_finger_tip.stl</uri>
+            </mesh>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>500</mu>
+                <mu2>500</mu2>
+                <slip1>0</slip1>
+                <slip2>0</slip2>
+              </ode>
+              <torsional>
+                <ode/>
+              </torsional>
+            </friction>
+            <contact>
+              <ode>
+                <kp>1e+07</kp>
+                <kd>1</kd>
+                <max_vel>100</max_vel>
+                <min_depth>0</min_depth>
+              </ode>
+            </contact>
+            <bounce/>
+          </surface>
+          <max_contacts>10</max_contacts>
+        </collision>
+        <visual name='l_gripper_l_finger_tip_link_geom_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/gripper_v0/l_finger_tip.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <sensor name='l_gripper_l_finger_tip_contact_sensor' type='contact'>
+          <update_rate>100</update_rate>
+          <contact>
+            <collision>l_gripper_l_finger_tip_link_geom</collision>
+            <topic>__default_topic__</topic>
+          </contact>
+        </sensor>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='l_gripper_motor_slider_link'>
+        <self_collide>0</self_collide>
+        <gravity>0</gravity>
+        <pose>0.93928 0.188 0.790675 0 -0 0</pose>
+        <inertial>
+          <mass>0.01</mass>
+          <inertia>
+            <ixx>0.001</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.001</iyy>
+            <iyz>0</iyz>
+            <izz>0.001</izz>
+          </inertia>
+          <pose>0 0 0 0 -0 0</pose>
+        </inertial>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='l_gripper_motor_screw_link'>
+        <self_collide>0</self_collide>
+        <gravity>0</gravity>
+        <pose>0.93928 0.188 0.790675 0 -0 0</pose>
+        <inertial>
+          <mass>0.01</mass>
+          <inertia>
+            <ixx>0.0001</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.0001</iyy>
+            <iyz>0</iyz>
+            <izz>0.0001</izz>
+          </inertia>
+          <pose>0 0 0 0 -0 0</pose>
+        </inertial>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='l_gripper_r_finger_link'>
+        <self_collide>0</self_collide>
+        <gravity>0</gravity>
+        <pose>0.84791 0.178 0.790675 0 -0 0</pose>
+        <inertial>
+          <mass>0.17389</mass>
+          <pose>0.03576 -0.01736 -0.00095 0 -0 0</pose>
+          <inertia>
+            <ixx>7.7e-05</ixx>
+            <ixy>-2e-06</ixy>
+            <ixz>-8e-06</ixz>
+            <iyy>0.000198</iyy>
+            <iyz>2e-06</iyz>
+            <izz>0.000181</izz>
+          </inertia>
+        </inertial>
+        <collision name='l_gripper_r_finger_link_geom'>
+          <pose>0 0 0 -3.14159 0 0</pose>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/gripper_v0/l_finger.stl</uri>
+            </mesh>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>500</mu>
+                <mu2>500</mu2>
+                <slip1>0</slip1>
+                <slip2>0</slip2>
+              </ode>
+              <torsional>
+                <ode/>
+              </torsional>
+            </friction>
+            <contact>
+              <ode>
+                <kp>1e+06</kp>
+                <kd>1</kd>
+                <max_vel>100</max_vel>
+                <min_depth>0</min_depth>
+              </ode>
+            </contact>
+            <bounce/>
+          </surface>
+          <max_contacts>10</max_contacts>
+        </collision>
+        <visual name='l_gripper_r_finger_link_geom_visual'>
+          <pose>0 0 0 -3.14159 0 0</pose>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/gripper_v0/l_finger.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='l_gripper_r_finger_tip_link'>
+        <self_collide>0</self_collide>
+        <gravity>0</gravity>
+        <pose>0.93928 0.17305 0.790675 0 -0 0</pose>
+        <inertial>
+          <mass>0.04419</mass>
+          <pose>0.00423 -0.00284 0 0 -0 0</pose>
+          <inertia>
+            <ixx>8e-06</ixx>
+            <ixy>-6e-06</ixy>
+            <ixz>0</ixz>
+            <iyy>1e-05</iyy>
+            <iyz>0</iyz>
+            <izz>1.5e-05</izz>
+          </inertia>
+        </inertial>
+        <collision name='l_gripper_r_finger_tip_link_geom'>
+          <pose>0 0 0 -3.14159 0 0</pose>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/gripper_v0/l_finger_tip.stl</uri>
+            </mesh>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>500</mu>
+                <mu2>500</mu2>
+                <slip1>0</slip1>
+                <slip2>0</slip2>
+              </ode>
+              <torsional>
+                <ode/>
+              </torsional>
+            </friction>
+            <contact>
+              <ode>
+                <kp>1e+07</kp>
+                <kd>1</kd>
+                <max_vel>100</max_vel>
+                <min_depth>0</min_depth>
+              </ode>
+            </contact>
+            <bounce/>
+          </surface>
+          <max_contacts>10</max_contacts>
+        </collision>
+        <visual name='l_gripper_r_finger_tip_link_geom_visual'>
+          <pose>0 0 0 -3.14159 0 0</pose>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/gripper_v0/l_finger_tip.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <sensor name='l_gripper_r_finger_tip_contact_sensor' type='contact'>
+          <update_rate>100</update_rate>
+          <contact>
+            <collision>l_gripper_r_finger_tip_link_geom</collision>
+            <topic>__default_topic__</topic>
+          </contact>
+        </sensor>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='laser_tilt_mount_link'>
+        <self_collide>0</self_collide>
+        <pose>0.04893 0 1.01768 0 -0 0</pose>
+        <inertial>
+          <mass>0.592</mass>
+          <pose>-0.001134 0.001667 -0.007067 0 -0 0</pose>
+          <inertia>
+            <ixx>0.001296</ixx>
+            <ixy>2.3e-05</ixy>
+            <ixz>3.7e-05</ixz>
+            <iyy>0.001086</iyy>
+            <iyz>3.5e-05</iyz>
+            <izz>0.000895</izz>
+          </inertia>
+        </inertial>
+        <collision name='laser_tilt_mount_link_geom'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/tilting_laser_v0/tilting_hokuyo_L.stl</uri>
+            </mesh>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>0</mu>
+                <mu2>0</mu2>
+                <slip1>0</slip1>
+                <slip2>0</slip2>
+              </ode>
+              <torsional>
+                <ode/>
+              </torsional>
+            </friction>
+            <contact>
+              <ode>
+                <kp>1e+12</kp>
+                <kd>10000</kd>
+                <max_vel>100</max_vel>
+                <min_depth>0</min_depth>
+              </ode>
+            </contact>
+            <bounce/>
+          </surface>
+          <max_contacts>10</max_contacts>
+        </collision>
+        <visual name='laser_tilt_mount_link_geom_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/tilting_laser_v0/tilting_hokuyo.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <sensor name='laser_tilt' type='ray'>
+          <update_rate>40</update_rate>
+          <pose>0 0 0.03 0 -0 0</pose>
+          <ray>
+            <scan>
+              <horizontal>
+                <samples>640</samples>
+                <resolution>1</resolution>
+                <min_angle>-1.39626</min_angle>
+                <max_angle>1.39626</max_angle>
+              </horizontal>
+              <vertical>
+                <samples>1</samples>
+                <min_angle>0</min_angle>
+                <max_angle>0</max_angle>
+              </vertical>
+            </scan>
+            <range>
+              <min>0.08</min>
+              <max>10</max>
+              <resolution>0.01</resolution>
+            </range>
+          </ray>
+        </sensor>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='r_shoulder_pan_link'>
+        <self_collide>0</self_collide>
+        <gravity>0</gravity>
+        <pose>-0.05 -0.188 0.790675 0 -0 0</pose>
+        <inertial>
+          <mass>25.7993</mass>
+          <pose>-0.001201 0.024513 -0.098231 0 -0 0</pose>
+          <inertia>
+            <ixx>0.866179</ixx>
+            <ixy>-0.060865</ixy>
+            <ixz>-0.121181</ixz>
+            <iyy>0.874217</iyy>
+            <iyz>-0.058866</iyz>
+            <izz>0.273538</izz>
+          </inertia>
+        </inertial>
+        <collision name='r_shoulder_pan_link_geom'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/shoulder_v0/shoulder_pan.stl</uri>
+            </mesh>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>0</mu>
+                <mu2>0</mu2>
+                <slip1>0</slip1>
+                <slip2>0</slip2>
+              </ode>
+              <torsional>
+                <ode/>
+              </torsional>
+            </friction>
+            <contact>
+              <ode>
+                <kp>1e+12</kp>
+                <kd>10000</kd>
+                <max_vel>100</max_vel>
+                <min_depth>0</min_depth>
+              </ode>
+            </contact>
+            <bounce/>
+          </surface>
+          <max_contacts>10</max_contacts>
+        </collision>
+        <visual name='r_shoulder_pan_link_geom_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/shoulder_v0/shoulder_pan.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='r_shoulder_lift_link'>
+        <self_collide>0</self_collide>
+        <gravity>0</gravity>
+        <pose>0.05 -0.188 0.790675 0 -0 0</pose>
+        <inertial>
+          <mass>2.74988</mass>
+          <pose>0.02195 -0.02664 -0.03127 0 -0 0</pose>
+          <inertia>
+            <ixx>0.021056</ixx>
+            <ixy>0.004967</ixy>
+            <ixz>-0.001948</ixz>
+            <iyy>0.021272</iyy>
+            <iyz>0.001104</iyz>
+            <izz>0.019758</izz>
+          </inertia>
+        </inertial>
+        <collision name='r_shoulder_lift_link_geom'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/shoulder_v0/shoulder_lift.stl</uri>
+            </mesh>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>0</mu>
+                <mu2>0</mu2>
+                <slip1>0</slip1>
+                <slip2>0</slip2>
+              </ode>
+              <torsional>
+                <ode/>
+              </torsional>
+            </friction>
+            <contact>
+              <ode>
+                <kp>1e+12</kp>
+                <kd>10000</kd>
+                <max_vel>100</max_vel>
+                <min_depth>0</min_depth>
+              </ode>
+            </contact>
+            <bounce/>
+          </surface>
+          <max_contacts>10</max_contacts>
+        </collision>
+        <visual name='r_shoulder_lift_link_geom_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/shoulder_v0/shoulder_lift.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='r_upper_arm_roll_link'>
+        <self_collide>0</self_collide>
+        <gravity>0</gravity>
+        <pose>0.05 -0.188 0.790675 0 -0 0</pose>
+        <inertial>
+          <mass>6.11769</mass>
+          <pose>0.210482 -0.015945 -0.000197 0 -0 0</pose>
+          <inertia>
+            <ixx>0.025378</ixx>
+            <ixy>0.003757</ixy>
+            <ixz>-0.000709</ixz>
+            <iyy>0.084737</iyy>
+            <iyz>-0.000179</iyz>
+            <izz>0.086088</izz>
+          </inertia>
+        </inertial>
+        <collision name='r_upper_arm_roll_link_geom'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/shoulder_v0/upper_arm_roll_L.stl</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='r_upper_arm_roll_link_geom_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/shoulder_v0/upper_arm_roll.stl</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <collision name='r_upper_arm_roll_link_geom_r_upper_arm_link'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/upper_arm_v0/upper_arm.stl</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='r_upper_arm_roll_link_geom_r_upper_arm_link_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/upper_arm_v0/upper_arm.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='r_elbow_flex_link'>
+        <self_collide>0</self_collide>
+        <gravity>0</gravity>
+        <pose>0.45 -0.188 0.790675 0 -0 0</pose>
+        <inertial>
+          <mass>1.90327</mass>
+          <pose>0.01014 0.00032 -0.01211 0 -0 0</pose>
+          <inertia>
+            <ixx>0.003465</ixx>
+            <ixy>4.1e-05</ixy>
+            <ixz>0.000432</ixz>
+            <iyy>0.004416</iyy>
+            <iyz>-4e-05</iyz>
+            <izz>0.003592</izz>
+          </inertia>
+        </inertial>
+        <collision name='r_elbow_flex_link_geom'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/upper_arm_v0/elbow_flex.stl</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='r_elbow_flex_link_geom_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/upper_arm_v0/elbow_flex.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='r_forearm_roll_link'>
+        <self_collide>0</self_collide>
+        <gravity>0</gravity>
+        <pose>0.45 -0.188 0.790675 0 -0 0</pose>
+        <inertial>
+          <mass>2.68968</mass>
+          <pose>0.180727 -0.000163 -0.008583 0 -0 0</pose>
+          <inertia>
+            <ixx>0.014668</ixx>
+            <ixy>5.2e-05</ixy>
+            <ixz>0.000656</ixz>
+            <iyy>0.026279</iyy>
+            <iyz>-1.3e-05</iyz>
+            <izz>0.027775</izz>
+          </inertia>
+        </inertial>
+        <collision name='r_forearm_roll_link_geom'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/upper_arm_v0/forearm_roll_L.stl</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='r_forearm_roll_link_geom_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/upper_arm_v0/forearm_roll.stl</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <collision name='r_forearm_roll_link_geom_r_forearm_link'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/forearm_v0/forearm.stl</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='r_forearm_roll_link_geom_r_forearm_link_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/forearm_v0/forearm.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <sensor name='r_forearm_cam_sensor' type='camera'>
+          <update_rate>25</update_rate>
+          <pose>0.135 0 0.044 1.5708 -0.562869 -0</pose>
+          <camera>
+            <horizontal_fov>1.5708</horizontal_fov>
+            <image>
+              <width>640</width>
+              <height>480</height>
+              <format>R8G8B8</format>
+            </image>
+            <clip>
+              <near>0.1</near>
+              <far>100</far>
+            </clip>
+          </camera>
+        </sensor>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='r_wrist_flex_link'>
+        <self_collide>0</self_collide>
+        <gravity>0</gravity>
+        <pose>0.771 -0.188 0.790675 0 -0 0</pose>
+        <inertial>
+          <mass>0.61402</mass>
+          <pose>-0.00157 0 -0.00075 0 -0 0</pose>
+          <inertia>
+            <ixx>0.000652</ixx>
+            <ixy>0</ixy>
+            <ixz>3e-06</ixz>
+            <iyy>0.000198</iyy>
+            <iyz>0</iyz>
+            <izz>0.000645</izz>
+          </inertia>
+        </inertial>
+        <collision name='r_wrist_flex_link_geom'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/forearm_v0/wrist_flex.stl</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='r_wrist_flex_link_geom_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/forearm_v0/wrist_flex.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='r_wrist_roll_link'>
+        <self_collide>0</self_collide>
+        <gravity>0</gravity>
+        <pose>0.771 -0.188 0.790675 0 -0 0</pose>
+        <inertial>
+          <mass>0.68107</mass>
+          <pose>0.056408 0.000451 -0.001014 0 -0 0</pose>
+          <inertia>
+            <ixx>0.011352</ixx>
+            <ixy>-1.6e-05</ixy>
+            <ixz>-1e-06</ixz>
+            <iyy>0.011677</iyy>
+            <iyz>-1e-06</iyz>
+            <izz>0.011866</izz>
+          </inertia>
+        </inertial>
+        <collision name='r_wrist_roll_link_geom'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/forearm_v0/wrist_roll_L.stl</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='r_wrist_roll_link_geom_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/forearm_v0/wrist_roll.stl</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <collision name='r_wrist_roll_link_geom_r_gripper_motor_accelerometer_link'>
+          <geometry>
+            <box>
+              <size>0.001 0.001 0.001</size>
+            </box>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='r_wrist_roll_link_geom_r_gripper_motor_accelerometer_link_visual'>
+          <geometry>
+            <box>
+              <size>0.001 0.001 0.001</size>
+            </box>
+          </geometry>
+        </visual>
+        <collision name='r_wrist_roll_link_geom_r_gripper_palm_link'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/gripper_v0/gripper_palm.stl</uri>
+            </mesh>
+          </geometry>
+          <max_contacts>10</max_contacts>
+          <surface>
+            <contact>
+              <ode/>
+            </contact>
+            <bounce/>
+            <friction>
+              <torsional>
+                <ode/>
+              </torsional>
+              <ode/>
+            </friction>
+          </surface>
+        </collision>
+        <visual name='r_wrist_roll_link_geom_r_gripper_palm_link_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/gripper_v0/gripper_palm.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='r_gripper_l_finger_link'>
+        <self_collide>0</self_collide>
+        <gravity>0</gravity>
+        <pose>0.84791 -0.178 0.790675 0 -0 0</pose>
+        <inertial>
+          <mass>0.17126</mass>
+          <pose>0.03598 0.0173 -0.00164 0 -0 0</pose>
+          <inertia>
+            <ixx>7.8e-05</ixx>
+            <ixy>1e-06</ixy>
+            <ixz>1e-05</ixz>
+            <iyy>0.000197</iyy>
+            <iyz>-3e-06</iyz>
+            <izz>0.000181</izz>
+          </inertia>
+        </inertial>
+        <collision name='r_gripper_l_finger_link_geom'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/gripper_v0/l_finger.stl</uri>
+            </mesh>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>500</mu>
+                <mu2>500</mu2>
+                <slip1>0</slip1>
+                <slip2>0</slip2>
+              </ode>
+              <torsional>
+                <ode/>
+              </torsional>
+            </friction>
+            <contact>
+              <ode>
+                <kp>1e+06</kp>
+                <kd>1</kd>
+                <max_vel>100</max_vel>
+                <min_depth>0</min_depth>
+              </ode>
+            </contact>
+            <bounce/>
+          </surface>
+          <max_contacts>10</max_contacts>
+        </collision>
+        <visual name='r_gripper_l_finger_link_geom_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/gripper_v0/l_finger.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='r_gripper_l_finger_tip_link'>
+        <self_collide>0</self_collide>
+        <gravity>0</gravity>
+        <pose>0.93928 -0.17305 0.790675 0 -0 0</pose>
+        <inertial>
+          <mass>0.04419</mass>
+          <pose>0.00423 0.00284 0 0 -0 0</pose>
+          <inertia>
+            <ixx>8e-06</ixx>
+            <ixy>6e-06</ixy>
+            <ixz>0</ixz>
+            <iyy>1e-05</iyy>
+            <iyz>0</iyz>
+            <izz>1.5e-05</izz>
+          </inertia>
+        </inertial>
+        <collision name='r_gripper_l_finger_tip_link_geom'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/gripper_v0/l_finger_tip.stl</uri>
+            </mesh>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>500</mu>
+                <mu2>500</mu2>
+                <slip1>0</slip1>
+                <slip2>0</slip2>
+              </ode>
+              <torsional>
+                <ode/>
+              </torsional>
+            </friction>
+            <contact>
+              <ode>
+                <kp>1e+07</kp>
+                <kd>1</kd>
+                <max_vel>100</max_vel>
+                <min_depth>0</min_depth>
+              </ode>
+            </contact>
+            <bounce/>
+          </surface>
+          <max_contacts>10</max_contacts>
+        </collision>
+        <visual name='r_gripper_l_finger_tip_link_geom_visual'>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/gripper_v0/l_finger_tip.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <sensor name='r_gripper_l_finger_tip_contact_sensor' type='contact'>
+          <update_rate>100</update_rate>
+          <contact>
+            <collision>r_gripper_l_finger_tip_link_geom</collision>
+            <topic>__default_topic__</topic>
+          </contact>
+        </sensor>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='r_gripper_motor_slider_link'>
+        <self_collide>0</self_collide>
+        <gravity>0</gravity>
+        <pose>0.93928 -0.188 0.790675 0 -0 0</pose>
+        <inertial>
+          <mass>0.01</mass>
+          <inertia>
+            <ixx>0.001</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.001</iyy>
+            <iyz>0</iyz>
+            <izz>0.001</izz>
+          </inertia>
+          <pose>0 0 0 0 -0 0</pose>
+        </inertial>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='r_gripper_motor_screw_link'>
+        <self_collide>0</self_collide>
+        <gravity>0</gravity>
+        <pose>0.93928 -0.188 0.790675 0 -0 0</pose>
+        <inertial>
+          <mass>0.01</mass>
+          <inertia>
+            <ixx>0.0001</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.0001</iyy>
+            <iyz>0</iyz>
+            <izz>0.0001</izz>
+          </inertia>
+          <pose>0 0 0 0 -0 0</pose>
+        </inertial>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='r_gripper_r_finger_link'>
+        <self_collide>0</self_collide>
+        <gravity>0</gravity>
+        <pose>0.84791 -0.198 0.790675 0 -0 0</pose>
+        <inertial>
+          <mass>0.17389</mass>
+          <pose>0.03576 -0.01736 -0.00095 0 -0 0</pose>
+          <inertia>
+            <ixx>7.7e-05</ixx>
+            <ixy>-2e-06</ixy>
+            <ixz>-8e-06</ixz>
+            <iyy>0.000198</iyy>
+            <iyz>2e-06</iyz>
+            <izz>0.000181</izz>
+          </inertia>
+        </inertial>
+        <collision name='r_gripper_r_finger_link_geom'>
+          <pose>0 0 0 -3.14159 0 0</pose>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/gripper_v0/l_finger.stl</uri>
+            </mesh>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>500</mu>
+                <mu2>500</mu2>
+                <slip1>0</slip1>
+                <slip2>0</slip2>
+              </ode>
+              <torsional>
+                <ode/>
+              </torsional>
+            </friction>
+            <contact>
+              <ode>
+                <kp>1e+06</kp>
+                <kd>1</kd>
+                <max_vel>100</max_vel>
+                <min_depth>0</min_depth>
+              </ode>
+            </contact>
+            <bounce/>
+          </surface>
+          <max_contacts>10</max_contacts>
+        </collision>
+        <visual name='r_gripper_r_finger_link_geom_visual'>
+          <pose>0 0 0 -3.14159 0 0</pose>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/gripper_v0/l_finger.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='r_gripper_r_finger_tip_link'>
+        <self_collide>0</self_collide>
+        <gravity>0</gravity>
+        <pose>0.93928 -0.20295 0.790675 0 -0 0</pose>
+        <inertial>
+          <mass>0.04419</mass>
+          <pose>0.00423 -0.00284 0 0 -0 0</pose>
+          <inertia>
+            <ixx>8e-06</ixx>
+            <ixy>-6e-06</ixy>
+            <ixz>0</ixz>
+            <iyy>1e-05</iyy>
+            <iyz>0</iyz>
+            <izz>1.5e-05</izz>
+          </inertia>
+        </inertial>
+        <collision name='r_gripper_r_finger_tip_link_geom'>
+          <pose>0 0 0 -3.14159 0 0</pose>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/gripper_v0/l_finger_tip.stl</uri>
+            </mesh>
+          </geometry>
+          <surface>
+            <friction>
+              <ode>
+                <mu>500</mu>
+                <mu2>500</mu2>
+                <slip1>0</slip1>
+                <slip2>0</slip2>
+              </ode>
+              <torsional>
+                <ode/>
+              </torsional>
+            </friction>
+            <contact>
+              <ode>
+                <kp>1e+07</kp>
+                <kd>1</kd>
+                <max_vel>100</max_vel>
+                <min_depth>0</min_depth>
+              </ode>
+            </contact>
+            <bounce/>
+          </surface>
+          <max_contacts>10</max_contacts>
+        </collision>
+        <visual name='r_gripper_r_finger_tip_link_geom_visual'>
+          <pose>0 0 0 -3.14159 0 0</pose>
+          <geometry>
+            <mesh>
+              <uri>model://pr2/meshes/gripper_v0/l_finger_tip.dae</uri>
+            </mesh>
+          </geometry>
+        </visual>
+        <sensor name='r_gripper_r_finger_tip_contact_sensor' type='contact'>
+          <update_rate>100</update_rate>
+          <contact>
+            <collision>r_gripper_r_finger_tip_link_geom</collision>
+            <topic>__default_topic__</topic>
+          </contact>
+        </sensor>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='torso_lift_motor_screw_link'>
+        <self_collide>0</self_collide>
+        <pose>-0.15 0 0.751 0 -0 0</pose>
+        <inertial>
+          <mass>1</mass>
+          <inertia>
+            <ixx>0.001</ixx>
+            <ixy>0</ixy>
+            <ixz>0</ixz>
+            <iyy>0.001</iyy>
+            <iyz>0</iyz>
+            <izz>0.001</izz>
+          </inertia>
+          <pose>0 0 0 0 -0 0</pose>
+        </inertial>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='r_gripper_l_parallel_link'>
+        <self_collide>0</self_collide>
+        <gravity>0</gravity>
+        <pose>0.82991 -0.157 0.790675 0 -0 0</pose>
+        <inertial>
+          <mass>0.17126</mass>
+          <pose>0.03598 0.0173 -0.00164 0 -0 0</pose>
+          <inertia>
+            <ixx>7.8e-05</ixx>
+            <ixy>1e-06</ixy>
+            <ixz>-1e-05</ixz>
+            <iyy>0.000197</iyy>
+            <iyz>-3e-06</iyz>
+            <izz>0.000181</izz>
+          </inertia>
+        </inertial>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='r_gripper_r_parallel_link'>
+        <self_collide>0</self_collide>
+        <gravity>0</gravity>
+        <pose>0.82991 -0.219 0.790675 0 -0 0</pose>
+        <inertial>
+          <mass>0.17389</mass>
+          <pose>0.03576 -0.01736 -0.00095 0 -0 0</pose>
+          <inertia>
+            <ixx>7.7e-05</ixx>
+            <ixy>-2e-06</ixy>
+            <ixz>-8e-06</ixz>
+            <iyy>0.000198</iyy>
+            <iyz>2e-06</iyz>
+            <izz>0.000181</izz>
+          </inertia>
+        </inertial>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='l_gripper_l_parallel_link'>
+        <self_collide>0</self_collide>
+        <gravity>0</gravity>
+        <pose>0.82991 0.219 0.790675 0 -0 0</pose>
+        <inertial>
+          <mass>0.17126</mass>
+          <pose>0.03598 0.0173 -0.00164 0 -0 0</pose>
+          <inertia>
+            <ixx>7.8e-05</ixx>
+            <ixy>1e-06</ixy>
+            <ixz>1e-05</ixz>
+            <iyy>0.000197</iyy>
+            <iyz>-3e-06</iyz>
+            <izz>0.000181</izz>
+          </inertia>
+        </inertial>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <link name='l_gripper_r_parallel_link'>
+        <self_collide>0</self_collide>
+        <gravity>0</gravity>
+        <pose>0.82991 0.157 0.790675 0 -0 0</pose>
+        <inertial>
+          <mass>0.17389</mass>
+          <pose>0.03576 -0.01736 -0.00095 0 -0 0</pose>
+          <inertia>
+            <ixx>7.7e-05</ixx>
+            <ixy>-2e-06</ixy>
+            <ixz>8e-06</ixz>
+            <iyy>0.000198</iyy>
+            <iyz>2e-06</iyz>
+            <izz>0.000181</izz>
+          </inertia>
+        </inertial>
+        <velocity_decay/>
+        <enable_wind>0</enable_wind>
+        <kinematic>0</kinematic>
+      </link>
+      <joint name='bl_caster_rotation_joint' type='revolute'>
+        <parent>base_footprint</parent>
+        <child>bl_caster_rotation_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 0 1</xyz>
+          <limit>
+            <lower>-1e+16</lower>
+            <upper>1e+16</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+          <dynamics>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+        </axis>
+      </joint>
+      <joint name='bl_caster_l_wheel_joint' type='revolute'>
+        <parent>bl_caster_rotation_link</parent>
+        <child>bl_caster_l_wheel_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 1 0</xyz>
+          <limit>
+            <lower>-1e+16</lower>
+            <upper>1e+16</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+          <dynamics>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+        </axis>
+      </joint>
+      <joint name='bl_caster_r_wheel_joint' type='revolute'>
+        <parent>bl_caster_rotation_link</parent>
+        <child>bl_caster_r_wheel_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 1 0</xyz>
+          <limit>
+            <lower>-1e+16</lower>
+            <upper>1e+16</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+          <dynamics>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+        </axis>
+      </joint>
+      <joint name='br_caster_rotation_joint' type='revolute'>
+        <parent>base_footprint</parent>
+        <child>br_caster_rotation_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 0 1</xyz>
+          <limit>
+            <lower>-1e+16</lower>
+            <upper>1e+16</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+          <dynamics>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+        </axis>
+      </joint>
+      <joint name='br_caster_l_wheel_joint' type='revolute'>
+        <parent>br_caster_rotation_link</parent>
+        <child>br_caster_l_wheel_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 1 0</xyz>
+          <limit>
+            <lower>-1e+16</lower>
+            <upper>1e+16</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+          <dynamics>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+        </axis>
+      </joint>
+      <joint name='br_caster_r_wheel_joint' type='revolute'>
+        <parent>br_caster_rotation_link</parent>
+        <child>br_caster_r_wheel_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 1 0</xyz>
+          <limit>
+            <lower>-1e+16</lower>
+            <upper>1e+16</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+          <dynamics>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+        </axis>
+      </joint>
+      <joint name='fl_caster_rotation_joint' type='revolute'>
+        <parent>base_footprint</parent>
+        <child>fl_caster_rotation_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 0 1</xyz>
+          <limit>
+            <lower>-1e+16</lower>
+            <upper>1e+16</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+          <dynamics>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+        </axis>
+      </joint>
+      <joint name='fl_caster_l_wheel_joint' type='revolute'>
+        <parent>fl_caster_rotation_link</parent>
+        <child>fl_caster_l_wheel_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 1 0</xyz>
+          <limit>
+            <lower>-1e+16</lower>
+            <upper>1e+16</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+          <dynamics>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+        </axis>
+      </joint>
+      <joint name='fl_caster_r_wheel_joint' type='revolute'>
+        <parent>fl_caster_rotation_link</parent>
+        <child>fl_caster_r_wheel_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 1 0</xyz>
+          <limit>
+            <lower>-1e+16</lower>
+            <upper>1e+16</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+          <dynamics>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+        </axis>
+      </joint>
+      <joint name='fr_caster_rotation_joint' type='revolute'>
+        <parent>base_footprint</parent>
+        <child>fr_caster_rotation_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 0 1</xyz>
+          <limit>
+            <lower>-1e+16</lower>
+            <upper>1e+16</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+          <dynamics>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+        </axis>
+      </joint>
+      <joint name='fr_caster_l_wheel_joint' type='revolute'>
+        <parent>fr_caster_rotation_link</parent>
+        <child>fr_caster_l_wheel_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 1 0</xyz>
+          <limit>
+            <lower>-1e+16</lower>
+            <upper>1e+16</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+          <dynamics>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+        </axis>
+      </joint>
+      <joint name='fr_caster_r_wheel_joint' type='revolute'>
+        <parent>fr_caster_rotation_link</parent>
+        <child>fr_caster_r_wheel_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 1 0</xyz>
+          <limit>
+            <lower>-1e+16</lower>
+            <upper>1e+16</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+          <dynamics>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+        </axis>
+      </joint>
+      <joint name='torso_lift_joint' type='prismatic'>
+        <parent>base_footprint</parent>
+        <child>torso_lift_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 0 1</xyz>
+          <limit>
+            <lower>0</lower>
+            <upper>0.33</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+          <dynamics>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+        </axis>
+      </joint>
+      <joint name='head_pan_joint' type='revolute'>
+        <parent>torso_lift_link</parent>
+        <child>head_pan_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 0 1</xyz>
+          <dynamics>
+            <damping>2</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>-3.00699</lower>
+            <upper>3.00699</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='head_tilt_joint' type='revolute'>
+        <parent>head_pan_link</parent>
+        <child>head_tilt_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 1 0</xyz>
+          <dynamics>
+            <damping>10</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>-0.471237</lower>
+            <upper>1.39626</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='l_shoulder_pan_joint' type='revolute'>
+        <parent>torso_lift_link</parent>
+        <child>l_shoulder_pan_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 0 1</xyz>
+          <dynamics>
+            <damping>10</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>-0.714602</lower>
+            <upper>2.2854</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='l_shoulder_lift_joint' type='revolute'>
+        <parent>l_shoulder_pan_link</parent>
+        <child>l_shoulder_lift_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 1 0</xyz>
+          <dynamics>
+            <damping>10</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>-0.523601</lower>
+            <upper>1.3963</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='l_upper_arm_roll_joint' type='revolute'>
+        <parent>l_shoulder_lift_link</parent>
+        <child>l_upper_arm_roll_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>1 0 0</xyz>
+          <dynamics>
+            <damping>0.1</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>-0.8</lower>
+            <upper>3.90001</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='l_elbow_flex_joint' type='revolute'>
+        <parent>l_upper_arm_roll_link</parent>
+        <child>l_elbow_flex_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 1 0</xyz>
+          <dynamics>
+            <damping>1</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>-2.32131</lower>
+            <upper>0</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='l_forearm_roll_joint' type='revolute'>
+        <parent>l_elbow_flex_link</parent>
+        <child>l_forearm_roll_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>1 0 0</xyz>
+          <dynamics>
+            <damping>0.1</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>-1e+16</lower>
+            <upper>1e+16</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='l_wrist_flex_joint' type='revolute'>
+        <parent>l_forearm_roll_link</parent>
+        <child>l_wrist_flex_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 1 0</xyz>
+          <dynamics>
+            <damping>0.1</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>-2.18</lower>
+            <upper>0</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='l_wrist_roll_joint' type='revolute'>
+        <parent>l_wrist_flex_link</parent>
+        <child>l_wrist_roll_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>1 0 0</xyz>
+          <dynamics>
+            <damping>0.1</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>-1e+16</lower>
+            <upper>1e+16</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='l_gripper_l_finger_joint' type='revolute'>
+        <parent>l_wrist_roll_link</parent>
+        <child>l_gripper_l_finger_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 0 1</xyz>
+          <dynamics>
+            <damping>0.02</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>0</lower>
+            <upper>0.548</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='l_gripper_l_finger_tip_joint' type='revolute'>
+        <parent>l_gripper_l_finger_link</parent>
+        <child>l_gripper_l_finger_tip_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 0 -1</xyz>
+          <dynamics>
+            <damping>0.001</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>0</lower>
+            <upper>0.548</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='l_gripper_motor_slider_joint' type='prismatic'>
+        <parent>l_wrist_roll_link</parent>
+        <child>l_gripper_motor_slider_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>1 0 0</xyz>
+          <limit>
+            <lower>-0.1</lower>
+            <upper>0.1</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+          <dynamics>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+        </axis>
+      </joint>
+      <joint name='l_gripper_motor_screw_joint' type='revolute'>
+        <parent>l_gripper_motor_slider_link</parent>
+        <child>l_gripper_motor_screw_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 1 0</xyz>
+          <dynamics>
+            <damping>0.0001</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>-1e+16</lower>
+            <upper>1e+16</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='l_gripper_r_finger_joint' type='revolute'>
+        <parent>l_wrist_roll_link</parent>
+        <child>l_gripper_r_finger_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 0 -1</xyz>
+          <dynamics>
+            <damping>0.02</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>0</lower>
+            <upper>0.548</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='l_gripper_r_finger_tip_joint' type='revolute'>
+        <parent>l_gripper_r_finger_link</parent>
+        <child>l_gripper_r_finger_tip_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 0 1</xyz>
+          <dynamics>
+            <damping>0.001</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>0</lower>
+            <upper>0.548</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='laser_tilt_mount_joint' type='revolute'>
+        <parent>torso_lift_link</parent>
+        <child>laser_tilt_mount_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 1 0</xyz>
+          <dynamics>
+            <damping>0.008</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>-0.7854</lower>
+            <upper>1.48353</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='r_shoulder_pan_joint' type='revolute'>
+        <parent>torso_lift_link</parent>
+        <child>r_shoulder_pan_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 0 1</xyz>
+          <dynamics>
+            <damping>10</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>-2.2854</lower>
+            <upper>0.714602</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='r_shoulder_lift_joint' type='revolute'>
+        <parent>r_shoulder_pan_link</parent>
+        <child>r_shoulder_lift_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 1 0</xyz>
+          <dynamics>
+            <damping>10</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>-0.523601</lower>
+            <upper>1.3963</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='r_upper_arm_roll_joint' type='revolute'>
+        <parent>r_shoulder_lift_link</parent>
+        <child>r_upper_arm_roll_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>1 0 0</xyz>
+          <dynamics>
+            <damping>0.1</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>-3.90001</lower>
+            <upper>0.8</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='r_elbow_flex_joint' type='revolute'>
+        <parent>r_upper_arm_roll_link</parent>
+        <child>r_elbow_flex_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 1 0</xyz>
+          <dynamics>
+            <damping>1</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>-2.32131</lower>
+            <upper>0</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='r_forearm_roll_joint' type='revolute'>
+        <parent>r_elbow_flex_link</parent>
+        <child>r_forearm_roll_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>1 0 0</xyz>
+          <dynamics>
+            <damping>0.1</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>-1e+16</lower>
+            <upper>1e+16</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='r_wrist_flex_joint' type='revolute'>
+        <parent>r_forearm_roll_link</parent>
+        <child>r_wrist_flex_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 1 0</xyz>
+          <dynamics>
+            <damping>0.1</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>-2.18</lower>
+            <upper>0</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='r_wrist_roll_joint' type='revolute'>
+        <parent>r_wrist_flex_link</parent>
+        <child>r_wrist_roll_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>1 0 0</xyz>
+          <dynamics>
+            <damping>0.1</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>-1e+16</lower>
+            <upper>1e+16</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='r_gripper_l_finger_joint' type='revolute'>
+        <parent>r_wrist_roll_link</parent>
+        <child>r_gripper_l_finger_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 0 1</xyz>
+          <dynamics>
+            <damping>0.02</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>0</lower>
+            <upper>0.548</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='r_gripper_l_finger_tip_joint' type='revolute'>
+        <parent>r_gripper_l_finger_link</parent>
+        <child>r_gripper_l_finger_tip_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 0 -1</xyz>
+          <dynamics>
+            <damping>0.001</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>0</lower>
+            <upper>0.548</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='r_gripper_motor_slider_joint' type='prismatic'>
+        <parent>r_wrist_roll_link</parent>
+        <child>r_gripper_motor_slider_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>1 0 0</xyz>
+          <dynamics>
+            <damping>0</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>-0.1</lower>
+            <upper>0.1</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='r_gripper_motor_screw_joint' type='revolute'>
+        <parent>r_gripper_motor_slider_link</parent>
+        <child>r_gripper_motor_screw_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 1 0</xyz>
+          <dynamics>
+            <damping>0.0001</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>-1e+16</lower>
+            <upper>1e+16</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='r_gripper_r_finger_joint' type='revolute'>
+        <parent>r_wrist_roll_link</parent>
+        <child>r_gripper_r_finger_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 0 -1</xyz>
+          <dynamics>
+            <damping>0.02</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>0</lower>
+            <upper>0.548</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='r_gripper_r_finger_tip_joint' type='revolute'>
+        <parent>r_gripper_r_finger_link</parent>
+        <child>r_gripper_r_finger_tip_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 0 1</xyz>
+          <dynamics>
+            <damping>0.001</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>0</lower>
+            <upper>0.548</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='torso_lift_motor_screw_joint' type='revolute'>
+        <parent>base_footprint</parent>
+        <child>torso_lift_motor_screw_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 0 1</xyz>
+          <dynamics>
+            <damping>0.0001</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>-1e+16</lower>
+            <upper>1e+16</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='torso_lift_screw_torso_lift_joint' type='screw'>
+        <parent>torso_lift_link</parent>
+        <child>torso_lift_motor_screw_link</child>
+        <thread_pitch>3141.6</thread_pitch>
+        <axis>
+          <xyz expressed_in='__model__'>0 0 1</xyz>
+          <dynamics>
+            <damping>0</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>-1e+16</lower>
+            <upper>1e+16</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='r_gripper_r_screw_screw_joint' type='screw'>
+        <parent>r_gripper_r_finger_tip_link</parent>
+        <child>r_gripper_motor_screw_link</child>
+        <thread_pitch>-3141.6</thread_pitch>
+        <axis>
+          <xyz expressed_in='__model__'>0 1 0</xyz>
+          <dynamics>
+            <damping>0</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>-1e+16</lower>
+            <upper>1e+16</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='r_gripper_l_screw_screw_joint' type='screw'>
+        <parent>r_gripper_l_finger_tip_link</parent>
+        <child>r_gripper_motor_screw_link</child>
+        <thread_pitch>3141.6</thread_pitch>
+        <axis>
+          <xyz expressed_in='__model__'>0 1 0</xyz>
+          <dynamics>
+            <damping>0</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>-1e+16</lower>
+            <upper>1e+16</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='r_gripper_r_parallel_root_joint' type='revolute'>
+        <pose>0.05891 -0.031 0 0 -0 0</pose>
+        <parent>r_gripper_r_parallel_link</parent>
+        <child>r_wrist_roll_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 0 -1</xyz>
+          <dynamics>
+            <damping>0.2</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>-1e+16</lower>
+            <upper>1e+16</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='r_gripper_l_parallel_root_joint' type='revolute'>
+        <pose>0.05891 0.031 0 0 -0 0</pose>
+        <parent>r_gripper_l_parallel_link</parent>
+        <child>r_wrist_roll_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 0 1</xyz>
+          <dynamics>
+            <damping>0.2</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>-1e+16</lower>
+            <upper>1e+16</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='r_gripper_r_parallel_tip_joint' type='revolute'>
+        <pose>-0.018 -0.021 0 0 -0 0</pose>
+        <parent>r_gripper_r_parallel_link</parent>
+        <child>r_gripper_r_finger_tip_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 0 1</xyz>
+          <dynamics>
+            <damping>0</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>-1e+16</lower>
+            <upper>1e+16</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='r_gripper_l_parallel_tip_joint' type='revolute'>
+        <pose>-0.018 0.021 0 0 -0 0</pose>
+        <parent>r_gripper_l_parallel_link</parent>
+        <child>r_gripper_l_finger_tip_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 0 1</xyz>
+          <dynamics>
+            <damping>0</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>-1e+16</lower>
+            <upper>1e+16</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='r_gripper_joint' type='prismatic'>
+        <parent>r_gripper_r_finger_tip_link</parent>
+        <child>r_gripper_l_finger_tip_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 1 0</xyz>
+          <dynamics>
+            <damping>0</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>-1e+16</lower>
+            <upper>1e+16</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='l_gripper_r_screw_screw_joint' type='screw'>
+        <parent>l_gripper_r_finger_tip_link</parent>
+        <child>l_gripper_motor_screw_link</child>
+        <thread_pitch>-3141.6</thread_pitch>
+        <axis>
+          <xyz expressed_in='__model__'>0 1 0</xyz>
+          <dynamics>
+            <damping>0</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>-1e+16</lower>
+            <upper>1e+16</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='l_gripper_l_screw_screw_joint' type='screw'>
+        <parent>l_gripper_l_finger_tip_link</parent>
+        <child>l_gripper_motor_screw_link</child>
+        <thread_pitch>3141.6</thread_pitch>
+        <axis>
+          <xyz expressed_in='__model__'>0 1 0</xyz>
+          <dynamics>
+            <damping>0</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>-1e+16</lower>
+            <upper>1e+16</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='l_gripper_r_parallel_root_joint' type='revolute'>
+        <pose>0.05891 -0.031 0 0 -0 0</pose>
+        <parent>l_gripper_r_parallel_link</parent>
+        <child>l_wrist_roll_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 0 -1</xyz>
+          <dynamics>
+            <damping>0.2</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>-1e+16</lower>
+            <upper>1e+16</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='l_gripper_l_parallel_root_joint' type='revolute'>
+        <pose>0.05891 0.031 0 0 -0 0</pose>
+        <parent>l_gripper_l_parallel_link</parent>
+        <child>l_wrist_roll_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 0 1</xyz>
+          <dynamics>
+            <damping>0.2</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>-1e+16</lower>
+            <upper>1e+16</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='l_gripper_r_parallel_tip_joint' type='revolute'>
+        <pose>-0.018 -0.021 0 0 -0 0</pose>
+        <parent>l_gripper_r_parallel_link</parent>
+        <child>l_gripper_r_finger_tip_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 0 1</xyz>
+          <dynamics>
+            <damping>0</damping>
+            <friction>0</friction>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+          <limit>
+            <lower>-1e+16</lower>
+            <upper>1e+16</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+        </axis>
+      </joint>
+      <joint name='l_gripper_l_parallel_tip_joint' type='revolute'>
+        <pose>-0.018 0.021 0 0 -0 0</pose>
+        <parent>l_gripper_l_parallel_link</parent>
+        <child>l_gripper_l_finger_tip_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 0 1</xyz>
+          <limit>
+            <lower>-1e+16</lower>
+            <upper>1e+16</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+          <dynamics>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+        </axis>
+      </joint>
+      <joint name='l_gripper_joint' type='prismatic'>
+        <parent>l_gripper_r_finger_tip_link</parent>
+        <child>l_gripper_l_finger_tip_link</child>
+        <axis>
+          <xyz expressed_in='__model__'>0 1 0</xyz>
+          <limit>
+            <lower>-1e+16</lower>
+            <upper>1e+16</upper>
+            <effort>0</effort>
+            <velocity>4</velocity>
+          </limit>
+          <dynamics>
+            <spring_reference>0</spring_reference>
+            <spring_stiffness>0</spring_stiffness>
+          </dynamics>
+        </axis>
+      </joint>
+      <pose>0.315091 -0.936677 0 0 -0 0</pose>
+    </model>
+    <state world_name='default'>
+      <sim_time>10 786000000</sim_time>
+      <real_time>11 947006679</real_time>
+      <wall_time>1622235518 286654524</wall_time>
+      <iterations>10786</iterations>
+      <model name='ground_plane'>
+        <pose>0 0 0 0 -0 0</pose>
+        <scale>1 1 1</scale>
+        <link name='link'>
+          <pose>0 0 0 0 -0 0</pose>
+          <velocity>0 0 0 0 -0 0</velocity>
+          <acceleration>0 0 0 0 -0 0</acceleration>
+          <wrench>0 0 0 0 -0 0</wrench>
+        </link>
+      </model>
+      <model name='pr2'>
+        <pose>0.315176 -0.935399 -0.004433 1.1e-05 -6.6e-05 -0.002005</pose>
+        <scale>1 1 1</scale>
+        <link name='base_footprint'>
+          <pose>0.315176 -0.935399 -0.004433 1.1e-05 -6.6e-05 -0.002005</pose>
+          <velocity>-0.000494 0.000941 0.005745 -0.000108 0.001269 -0.001778</velocity>
+          <acceleration>-0.316785 -0.020765 9.66742 2.97548 1.52824 2.98363</acceleration>
+          <wrench>-37.381 -2.45029 1140.77 0 -0 0</wrench>
+        </link>
+        <link name='bl_caster_l_wheel_link'>
+          <pose>0.091437 -0.661362 0.074794 4.7e-05 0.014445 -0.008465</pose>
+          <velocity>0.000893 -0.000284 0.011557 0.004041 0.015162 -0.013907</velocity>
+          <acceleration>-0.09763 -3.66412 23.7448 2.46642 -0.417537 1.92389</acceleration>
+          <wrench>-0.042992 -1.61353 10.4562 0 -0 0</wrench>
+        </link>
+        <link name='bl_caster_r_wheel_link'>
+          <pose>0.090608 -0.759352 0.074796 -3.8e-05 0.003169 -0.008462</pose>
+          <velocity>-0.000409 0.000621 0.011968 -0.008215 -0.002386 -0.013253</velocity>
+          <acceleration>-0.09042 -1.40583 24.8073 -2.9307 -0.453226 -2.85725</acceleration>
+          <wrench>-0.039817 -0.619069 10.9241 0 -0 0</wrench>
+        </link>
+        <link name='bl_caster_rotation_link'>
+          <pose>0.091022 -0.710351 0.074763 4e-05 -6.8e-05 -0.008465</pose>
+          <velocity>4.1e-05 0.001238 0.007231 0.003419 0.000813 -0.013704</velocity>
+          <acceleration>-0.368207 -0.918835 12.9551 -2.77437 0.816835 -0.785324</acceleration>
+          <wrench>-1.27881 -3.19119 44.994 0 -0 0</wrench>
+        </link>
+        <link name='br_caster_l_wheel_link'>
+          <pose>0.090074 -1.11056 0.074795 5.4e-05 -0.006014 0.000988</pose>
+          <velocity>-0.000213 -0.000383 0.012621 0.005326 -0.001341 -0.006338</velocity>
+          <acceleration>-0.006706 -3.87315 25.8854 -0.97738 -0.097283 1.69264</acceleration>
+          <wrench>-0.002953 -1.70558 11.3989 0 -0 0</wrench>
+        </link>
+        <link name='br_caster_r_wheel_link'>
+          <pose>0.090171 -1.20855 0.074797 -4.5e-05 -0.003271 0.000993</pose>
+          <velocity>-0.000762 0.00069 0.013072 -0.009221 -0.008832 -0.005573</velocity>
+          <acceleration>0.003382 -1.24863 27.0279 -1.96956 0.613258 0.302598</acceleration>
+          <wrench>0.001489 -0.549847 11.902 0 -0 0</wrench>
+        </link>
+        <link name='br_caster_rotation_link'>
+          <pose>0.090121 -1.15955 0.07476 4.5e-05 -6.9e-05 0.00099</pose>
+          <velocity>-0.000723 0.001289 0.007484 0.004399 0.000705 -0.006092</velocity>
+          <acceleration>-0.370033 -0.939142 13.4379 -0.844373 0.583862 -0.9109</acceleration>
+          <wrench>-1.28515 -3.26171 46.671 0 -0 0</wrench>
+        </link>
+        <link name='fl_caster_l_wheel_link'>
+          <pose>0.540015 -0.662254 0.074792 9e-06 0.006992 0.004207</pose>
+          <velocity>-3.7e-05 0.000227 0.005273 -0.002952 -0.000687 0.004346</velocity>
+          <acceleration>-0.120673 -1.08289 10.8297 0.497021 0.391896 2.44511</acceleration>
+          <wrench>-0.05314 -0.476862 4.76898 0 -0 0</wrench>
+        </link>
+        <link name='fl_caster_r_wheel_link'>
+          <pose>0.540428 -0.760252 0.074792 -6e-06 0.012939 0.004209</pose>
+          <velocity>0.000496 0.000141 0.005148 -0.001724 0.005667 0.004586</velocity>
+          <acceleration>-0.050141 -0.872943 11.0198 -2.23461 1.07099 2.95317</acceleration>
+          <wrench>-0.02208 -0.384409 4.8527 0 -0 0</wrench>
+        </link>
+        <link name='fl_caster_rotation_link'>
+          <pose>0.540221 -0.711251 0.074786 1e-05 -7e-05 0.004208</pose>
+          <velocity>8.1e-05 0.000449 0.005372 -0.002088 0.000529 0.00456</velocity>
+          <acceleration>-0.3316 -0.132848 9.49632 -1.64107 0.255375 -0.389772</acceleration>
+          <wrench>-1.15167 -0.461391 32.9815 0 -0 0</wrench>
+        </link>
+        <link name='fr_caster_l_wheel_link'>
+          <pose>0.539416 -1.11145 0.074792 1.8e-05 -0.000149 -0.001942</pose>
+          <velocity>-0.000292 0.000102 0.006102 -0.001247 -0.002643 -0.005283</velocity>
+          <acceleration>-0.117797 -1.33909 12.7143 -2.42928 0.337896 2.44083</acceleration>
+          <wrench>-0.051873 -0.589684 5.59887 0 -0 0</wrench>
+        </link>
+        <link name='fr_caster_r_wheel_link'>
+          <pose>0.539226 -1.20945 0.074793 -9e-06 -0.001991 -0.00194</pose>
+          <velocity>-0.00078 0.000189 0.006248 -0.002428 -0.009182 -0.005118</velocity>
+          <acceleration>-0.117994 -0.772934 13.2387 2.49886 1.50371 2.9685</acceleration>
+          <wrench>-0.05196 -0.340369 5.82979 0 -0 0</wrench>
+        </link>
+        <link name='fr_caster_rotation_link'>
+          <pose>0.53932 -1.16045 0.074782 1.7e-05 -6.9e-05 -0.001942</pose>
+          <velocity>-0.000731 0.000502 0.005606 -0.000665 0.000604 -0.005217</velocity>
+          <acceleration>-0.413371 -0.246026 9.96379 0.974373 0.389397 -0.514096</acceleration>
+          <wrench>-1.43567 -0.854469 34.605 0 -0 0</wrench>
+        </link>
+        <link name='head_pan_link'>
+          <pose>0.248028 -0.935278 1.16768 1.5e-05 -6.4e-05 -0.00177</pose>
+          <velocity>0.000826 0.001108 -0.002874 0.0005 0.001956 -0.000905</velocity>
+          <acceleration>1.10731 -0.110699 -0.728335 -1.76977 0.244182 -2.22991</acceleration>
+          <wrench>7.01922 -0.701718 -4.61692 0 -0 0</wrench>
+        </link>
+        <link name='head_tilt_link'>
+          <pose>0.316028 -0.935398 1.16769 1.4e-05 -0.040081 -0.001771</pose>
+          <velocity>0.000889 0.000993 -0.002959 0.000322 -0.043022 -0.000891</velocity>
+          <acceleration>0.88604 -0.16483 -0.84976 -1.9887 -1.16178 -2.20679</acceleration>
+          <wrench>4.71285 -0.87673 -4.51987 0 -0 0</wrench>
+        </link>
+        <link name='l_elbow_flex_link'>
+          <pose>0.765497 -0.748288 0.784938 0.000677 -0.069018 -0.002009</pose>
+          <velocity>0.000663 0.000332 0.002204 0.001659 -0.017607 -0.001436</velocity>
+          <acceleration>0.25866 0.094457 0.222301 1.32706 1.09935 0.291863</acceleration>
+          <wrench>0.492299 0.179776 0.423098 0 -0 0</wrench>
+        </link>
+        <link name='l_forearm_roll_link'>
+          <pose>0.765497 -0.748288 0.784938 0.001744 -0.069018 -0.00201</pose>
+          <velocity>0.000629 0.000326 0.002218 -0.002783 -0.017729 -0.001888</velocity>
+          <acceleration>0.216438 0.015178 0.097462 -0.064023 0.954685 -0.243821</acceleration>
+          <wrench>0.582148 0.040823 0.262142 0 -0 0</wrench>
+        </link>
+        <link name='l_gripper_l_finger_link'>
+          <pose>1.15994 -0.739108 0.827358 0.002187 -0.266574 -0.001944</pose>
+          <velocity>-0.000864 -0.000298 0.011698 -0.003469 -0.051815 -0.001704</velocity>
+          <acceleration>0.107235 -0.011168 -0.031693 0.062104 -0.327375 0.649681</acceleration>
+          <wrench>0.018365 -0.001913 -0.005428 0 -0 0</wrench>
+        </link>
+        <link name='l_gripper_l_finger_tip_link'>
+          <pose>1.24809 -0.734329 0.851438 0.0023 -0.266573 -0.002365</pose>
+          <velocity>-0.002098 -0.000369 0.016249 -0.002896 -0.052114 -0.001872</velocity>
+          <acceleration>0.115792 0.016594 -0.007567 0.816641 -0.822057 0.284281</acceleration>
+          <wrench>0.005117 0.000733 -0.000334 0 -0 0</wrench>
+        </link>
+        <link name='l_gripper_l_parallel_link'>
+          <pose>1.14261 -0.718067 0.822663 0.002187 -0.266574 -0.001943</pose>
+          <velocity>-0.00057 -0.000279 0.010733 -0.003544 -0.051846 -0.001454</velocity>
+          <acceleration>0.118024 0.004099 -0.03091 -0.092835 -0.373451 0.969838</acceleration>
+          <wrench>0.020213 0.000702 -0.005294 0 -0 0</wrench>
+        </link>
+        <link name='l_gripper_motor_screw_link'>
+          <pose>1.24806 -0.749316 0.851405 0.002242 -0.150218 -0.002095</pose>
+          <velocity>-0.002139 -0.0004 0.016296 -0.003509 -0.119967 -0.002174</velocity>
+          <acceleration>0.095115 -0.054125 -0.017433 0.048036 0.084428 -0.135426</acceleration>
+          <wrench>0.000951 -0.000541 -0.000174 0 -0 0</wrench>
+        </link>
+        <link name='l_gripper_motor_slider_link'>
+          <pose>1.24806 -0.749316 0.851405 0.002298 -0.266573 -0.002365</pose>
+          <velocity>-0.002134 -0.000399 0.016298 -0.003387 -0.051819 -0.002043</velocity>
+          <acceleration>0.103481 -0.054226 -0.01501 0.050508 -0.324168 -0.166618</acceleration>
+          <wrench>0.001035 -0.000542 -0.00015 0 -0 0</wrench>
+        </link>
+        <link name='l_gripper_r_finger_link'>
+          <pose>1.15991 -0.759108 0.827314 0.002409 -0.266572 -0.002785</pose>
+          <velocity>-0.000898 -0.000306 0.011766 -0.00351 -0.051814 -0.001577</velocity>
+          <acceleration>0.124016 -0.039776 -0.02672 0.019496 -0.322431 0.144948</acceleration>
+          <wrench>0.021565 -0.006917 -0.004646 0 -0 0</wrench>
+        </link>
+        <link name='l_gripper_r_finger_tip_link'>
+          <pose>1.24804 -0.764303 0.851371 0.002298 -0.266572 -0.002365</pose>
+          <velocity>-0.00215 -0.000365 0.016353 -0.003332 -0.051711 -0.002097</velocity>
+          <acceleration>0.119465 -0.039714 -0.005775 -0.04803 -0.330939 -0.301219</acceleration>
+          <wrench>0.005279 -0.001755 -0.000255 0 -0 0</wrench>
+        </link>
+        <link name='l_gripper_r_parallel_link'>
+          <pose>1.14251 -0.780066 0.822525 0.002409 -0.266572 -0.002787</pose>
+          <velocity>-0.000682 -0.000283 0.010939 -0.003408 -0.051807 -0.001938</velocity>
+          <acceleration>0.129669 -0.051328 -0.030404 0.097129 -0.318013 -0.40457</acceleration>
+          <wrench>0.022548 -0.008925 -0.005287 0 -0 0</wrench>
+        </link>
+        <link name='l_shoulder_lift_link'>
+          <pose>0.3655 -0.747504 0.786239 1e-05 0.003253 -0.001961</pose>
+          <velocity>0.00073 0.00094 -0.003385 -0.000202 -0.013524 -0.001563</velocity>
+          <acceleration>0.500051 0.036846 -1.12587 2.74254 -0.413629 -3.04653</acceleration>
+          <wrench>1.37508 0.101322 -3.096 0 -0 0</wrench>
+        </link>
+        <link name='l_shoulder_pan_link'>
+          <pose>0.2655 -0.747308 0.786232 1e-05 -6.8e-05 -0.001961</pose>
+          <velocity>0.00073 0.001113 -0.003206 -0.000186 0.001224 -0.001592</velocity>
+          <acceleration>0.280483 0.03564 -0.935371 -0.403685 1.31699 0.01783</acceleration>
+          <wrench>7.23626 0.919482 -24.1319 0 -0 0</wrench>
+        </link>
+        <link name='l_upper_arm_roll_link'>
+          <pose>0.3655 -0.747504 0.786238 0.000675 0.00325 -0.00196</pose>
+          <velocity>0.000661 0.000915 -0.003475 0.001671 -0.014136 -0.001442</velocity>
+          <acceleration>0.293931 0.048984 -0.562492 -1.80703 0.837911 -2.9126</acceleration>
+          <wrench>1.79818 0.299667 -3.44115 0 -0 0</wrench>
+        </link>
+        <link name='l_wrist_flex_link'>
+          <pose>1.08573 -0.748932 0.807076 0.001804 -0.266572 -0.002365</pose>
+          <velocity>0.000225 -0.000219 0.007896 -0.002839 -0.051717 -0.001969</velocity>
+          <acceleration>0.216605 -0.021285 -0.042276 -0.05574 -0.171862 -0.303311</acceleration>
+          <wrench>0.133 -0.01307 -0.025958 0 -0 0</wrench>
+        </link>
+        <link name='l_wrist_roll_link'>
+          <pose>1.08573 -0.748932 0.807076 0.002298 -0.266573 -0.002365</pose>
+          <velocity>0.000196 -0.000219 0.007891 -0.003386 -0.051821 -0.002041</velocity>
+          <acceleration>0.165671 -0.034614 -0.036138 0.049816 -0.320616 -0.165986</acceleration>
+          <wrench>0.112834 -0.023574 -0.024613 0 -0 0</wrench>
+        </link>
+        <link name='laser_tilt_mount_link'>
+          <pose>0.364038 -0.935508 1.01324 1.1e-05 -0.157613 -0.002006</pose>
+          <velocity>0.000726 0.000929 -0.002963 -2.8e-05 0.008291 -0.001786</velocity>
+          <acceleration>0.783894 0.04155 -0.743148 3.10679 0.167844 2.9638</acceleration>
+          <wrench>0.464065 0.024598 -0.439943 0 -0 0</wrench>
+        </link>
+        <link name='r_elbow_flex_link'>
+          <pose>0.764743 -1.1243 0.784976 -0.000242 -0.068517 -0.001965</pose>
+          <velocity>1.9e-05 0.000286 0.002063 -0.000864 -0.017879 -0.001689</velocity>
+          <acceleration>0.263858 0.020419 0.064408 -0.488586 0.162445 0.026805</acceleration>
+          <wrench>0.502193 0.038862 0.122587 0 -0 0</wrench>
+        </link>
+        <link name='r_forearm_roll_link'>
+          <pose>0.764743 -1.1243 0.784976 -0.001187 -0.068517 -0.001965</pose>
+          <velocity>2.4e-05 0.000282 0.00206 0.002792 -0.017859 -0.001394</velocity>
+          <acceleration>0.274427 0.03992 0.020533 0.012061 0.21608 0.091746</acceleration>
+          <wrench>0.73812 0.107371 0.055228 0 -0 0</wrench>
+        </link>
+        <link name='r_gripper_l_finger_link'>
+          <pose>1.15933 -1.11506 0.826724 -0.001766 -0.260208 -0.001318</pose>
+          <velocity>-0.001385 -0.000386 0.011588 0.003469 -0.050844 -0.001309</velocity>
+          <acceleration>0.221496 0.085527 0.005105 0.120116 -0.217345 0.664043</acceleration>
+          <wrench>0.037933 0.014647 0.000874 0 -0 0</wrench>
+        </link>
+        <link name='r_gripper_l_finger_tip_link'>
+          <pose>1.24764 -1.11022 0.850224 -0.001658 -0.26021 -0.00173</pose>
+          <velocity>-0.002568 -0.000585 0.016094 0.004034 -0.051137 -0.00113</velocity>
+          <acceleration>0.233711 0.117398 0.02299 1.19193 -0.91397 0.604863</acceleration>
+          <wrench>0.010328 0.005188 0.001016 0 -0 0</wrench>
+        </link>
+        <link name='r_gripper_l_parallel_link'>
+          <pose>1.14198 -1.09403 0.82206 -0.001767 -0.260208 -0.001317</pose>
+          <velocity>-0.00111 -0.000346 0.010783 0.003317 -0.050881 -0.001052</velocity>
+          <acceleration>0.230536 0.103534 0.008266 -0.263079 -0.274764 1.08733</acceleration>
+          <wrench>0.039482 0.017731 0.001416 0 -0 0</wrench>
+        </link>
+        <link name='r_gripper_motor_screw_link'>
+          <pose>1.2476 -1.12521 0.850248 -0.001622 -0.146503 -0.001921</pose>
+          <velocity>-0.002605 -0.000605 0.016039 0.003272 -0.117217 -0.001201</velocity>
+          <acceleration>0.208535 0.03935 0.013751 -0.01945 0.10212 0.047932</acceleration>
+          <wrench>0.002085 0.000393 0.000138 0 -0 0</wrench>
+        </link>
+        <link name='r_gripper_motor_slider_link'>
+          <pose>1.2476 -1.12521 0.850248 -0.00166 -0.260209 -0.001731</pose>
+          <velocity>-0.002599 -0.000634 0.01604 0.003417 -0.050852 -0.001338</velocity>
+          <acceleration>0.218615 0.008017 0.015892 -0.016998 -0.222179 -0.00033</acceleration>
+          <wrench>0.002186 8e-05 0.000159 0 -0 0</wrench>
+        </link>
+        <link name='r_gripper_r_finger_link'>
+          <pose>1.15929 -1.13506 0.826756 -0.001555 -0.26021 -0.002141</pose>
+          <velocity>-0.001407 -0.000393 0.011521 0.003562 -0.050847 -0.001824</velocity>
+          <acceleration>0.229567 0.024459 0.009729 -0.051233 -0.222414 -0.776766</acceleration>
+          <wrench>0.039919 0.004253 0.001692 0 -0 0</wrench>
+        </link>
+        <link name='r_gripper_r_finger_tip_link'>
+          <pose>1.24757 -1.1402 0.850272 -0.001661 -0.260209 -0.001727</pose>
+          <velocity>-0.002611 -0.000644 0.015991 0.003125 -0.050856 -0.000173</velocity>
+          <acceleration>0.242657 -0.017646 0.026256 0.597344 0.703969 2.02441</acceleration>
+          <wrench>0.010723 -0.00078 0.00116 0 -0 0</wrench>
+        </link>
+        <link name='r_gripper_r_parallel_link'>
+          <pose>1.14185 -1.15603 0.822159 -0.001555 -0.26021 -0.002141</pose>
+          <velocity>-0.001199 -0.000356 0.010564 0.003578 -0.050869 -0.001811</velocity>
+          <acceleration>0.234789 0.021068 0.010017 0.057767 -0.257685 -0.739776</acceleration>
+          <wrench>0.040827 0.003663 0.001742 0 -0 0</wrench>
+        </link>
+        <link name='r_shoulder_lift_link'>
+          <pose>0.364746 -1.1235 0.786235 1e-05 0.003148 -0.001982</pose>
+          <velocity>2.2e-05 0.000923 -0.002964 -0.000109 -0.012426 -0.001621</velocity>
+          <acceleration>0.344708 0.016851 -0.710132 2.88586 -1.42171 3.09413</acceleration>
+          <wrench>0.947906 0.046338 -1.95278 0 -0 0</wrench>
+        </link>
+        <link name='r_shoulder_pan_link'>
+          <pose>0.264747 -1.12331 0.786228 1.1e-05 -6.7e-05 -0.001982</pose>
+          <velocity>8.1e-05 0.001103 -0.002807 -3.5e-05 0.001319 -0.00168</velocity>
+          <acceleration>0.239283 0.05645 -0.536878 3.10741 1.56249 3.01159</acceleration>
+          <wrench>6.17333 1.45636 -13.8511 0 -0 0</wrench>
+        </link>
+        <link name='r_upper_arm_roll_link'>
+          <pose>0.364746 -1.1235 0.786235 -0.000241 0.003147 -0.001982</pose>
+          <velocity>2.8e-05 0.000899 -0.002943 -0.000891 -0.01253 -0.001531</velocity>
+          <acceleration>0.305529 0.00314 -0.282676 2.60529 -1.17603 -3.02284</acceleration>
+          <wrench>1.86913 0.01921 -1.72932 0 -0 0</wrench>
+        </link>
+        <link name='r_wrist_flex_link'>
+          <pose>1.08499 -1.12493 0.806953 -0.001225 -0.260209 -0.00173</pose>
+          <velocity>-0.000389 -0.000226 0.007775 0.002771 -0.050857 -0.001525</velocity>
+          <acceleration>0.245796 0.052158 -0.015529 -0.006504 -0.23327 0.060183</acceleration>
+          <wrench>0.150924 0.032026 -0.009535 0 -0 0</wrench>
+        </link>
+        <link name='r_wrist_roll_link'>
+          <pose>1.08499 -1.12493 0.806953 -0.00166 -0.260209 -0.00173</pose>
+          <velocity>-0.000391 -0.000226 0.007774 0.00341 -0.05085 -0.001305</velocity>
+          <acceleration>0.239559 0.054476 -0.003411 -0.02472 -0.221027 0.041962</acceleration>
+          <wrench>0.163156 0.037102 -0.002323 0 -0 0</wrench>
+        </link>
+        <link name='torso_lift_link'>
+          <pose>0.265124 -0.935307 0.786231 1.1e-05 -6.8e-05 -0.002004</pose>
+          <velocity>0.000448 0.001098 -0.003066 -6.1e-05 0.00119 -0.00175</velocity>
+          <acceleration>0.377251 0.049813 -0.484714 -0.081474 1.20156 -0.091569</acceleration>
+          <wrench>13.7504 1.81563 -17.6673 0 -0 0</wrench>
+        </link>
+        <link name='torso_lift_motor_screw_link'>
+          <pose>0.165126 -0.935107 0.746561 1.1e-05 -6.7e-05 0.006142</pose>
+          <velocity>0.000451 0.001268 0.009531 -9e-06 0.000985 0.001385</velocity>
+          <acceleration>0.484245 0.050239 13.7618 0.058529 1.1909 -2.49675</acceleration>
+          <wrench>0.484245 0.050239 13.7618 0 -0 0</wrench>
+        </link>
+      </model>
+      <light name='sun'>
+        <pose>0 0 10 0 -0 0</pose>
+      </light>
+    </state>
+    <gui fullscreen='0'>
+      <camera name='user_camera'>
+        <pose>5 -5 2 0 0.275643 2.35619</pose>
+        <view_controller>orbit</view_controller>
+        <projection_type>perspective</projection_type>
+      </camera>
+    </gui>
+  </world>
+</sdf>
diff --git a/test/integration/schema_test.cc b/test/integration/schema_test.cc
index 028548471..721ef4456 100644
--- a/test/integration/schema_test.cc
+++ b/test/integration/schema_test.cc
@@ -41,6 +41,14 @@ const std::string SDF_TEST_PENDULUM =
   sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "integration", "model",
                           "double_pendulum.sdf");
 
+const std::string SDF_TEST_NESTED_MODEL =
+  sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf",
+                          "nested_canonical_link.sdf");
+
+const std::string SDF_TEST_STATE =
+  sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "integration", "model",
+                          "world_with_state.sdf");
+
 
 class SDFSchemaGenerator : public testing::Test
 {
@@ -73,9 +81,20 @@ TEST_F(SDFSchemaGenerator, TestPR2Model)
   runXMLlint(SDF_TEST_PR2);
 }
 
-
 /////////////////////////////////////////////////
 TEST_F(SDFSchemaGenerator, TestTurtleBotModel)
 {
   runXMLlint(SDF_TEST_TURTLEBOT);
 }
+
+/////////////////////////////////////////////////
+TEST_F(SDFSchemaGenerator, TestNestedModel)
+{
+  runXMLlint(SDF_TEST_NESTED_MODEL);
+}
+
+/////////////////////////////////////////////////
+TEST_F(SDFSchemaGenerator, TestState)
+{
+  runXMLlint(SDF_TEST_STATE);
+}
diff --git a/tools/xmlschema.rb b/tools/xmlschema.rb
old mode 100755
new mode 100644
index 221a5bc9f..0e98e207c
--- a/tools/xmlschema.rb
+++ b/tools/xmlschema.rb
@@ -2,6 +2,7 @@
 
 require "rexml/document"
 require "optparse"
+require "set"
 
 $path = nil
 
@@ -79,6 +80,11 @@ def printElem(_file, _spaces, _elem)
     _elem.get_elements("element").each do |elem|
       printElem(_file, _spaces+6, elem)
     end
+
+    _elem.get_elements("include").each do |inc|
+      printInclude(_file, _spaces+6, inc)
+    end
+
     _file.printf("%*s</xsd:choice>\n", _spaces+4, "")
 
     # Print the attributes for the complex type
@@ -89,18 +95,43 @@ def printElem(_file, _spaces, _elem)
 
     _file.printf("%*s</xsd:complexType>\n", _spaces+2, "")
   else
-    _file.printf("%*s<xsd:element name='%s' type='%s'>\n",
-                 _spaces, "", _elem.attributes["name"], type)
+
+    attributes = _elem.get_elements("attribute")
+    hasAttributes = attributes.size > 0
+
+    if hasAttributes
+      _file.printf("%*s<xsd:element name='%s'>\n",
+                    _spaces, "", _elem.attributes["name"])
+    else
+      _file.printf("%*s<xsd:element name='%s' type='%s'>\n",
+                    _spaces, "", _elem.attributes["name"], type)
+    end
 
     if !_elem.elements["description"].nil? &&
        !_elem.elements["description"].text.nil?
       printDocumentation(_file, _spaces+2, _elem.elements["description"].text)
     end
+
+    if hasAttributes
+      _file.printf("%*s<xsd:complexType>\n", _spaces+2, "")
+      _file.printf("%*s<xsd:simpleContent>\n", _spaces+4, "")
+      _file.printf("%*s<xsd:extension base='%s'>\n", _spaces+6, "", type)
+
+      # Print the attributes for the complex type
+      # Attributes must go at the end of the complex type.
+      attributes.each do |attr|
+        printAttribute(_file, _spaces+8, attr);
+      end
+
+      _file.printf("%*s</xsd:extension>\n", _spaces+6, "")
+      _file.printf("%*s</xsd:simpleContent>\n", _spaces+4, "")
+      _file.printf("%*s</xsd:complexType>\n", _spaces+2, "")
+    end
+
   end
 
   _file.printf("%*s</xsd:element>\n", _spaces, "")
   _file.printf("%*s</xsd:choice>\n", _spaces, "")
-
 end
 
 #################################################
@@ -121,18 +152,14 @@ def printDocumentation(_file, _spaces, _doc)
 end
 
 #################################################
-def printIncludeRef(_file, _spaces, _inc)
-  path = File.join($path, _inc.attributes["filename"])
-  doc = REXML::Document.new File.new(path)
-  incElemName = doc.root.attributes['name']
-  _file.printf("%*s<xsd:element ref='%s'/>\n", _spaces, "", incElemName)
-end
-
-#################################################
+# Prints XSD contents of <include>
 def printInclude(_file, _spaces, _attr)
-  loc = "http://sdformat.org/schemas/"
-  loc += _attr.attributes['filename'].sub("\.sdf","\.xsd")
-  _file.printf("%*s<xsd:include schemaLocation='%s'/>\n", _spaces, "", loc)
+  loc = $path + "/" + _attr.attributes['filename']
+  doc = REXML::Document.new File.new(loc)
+
+  doc.elements.each_with_index("element") do |elem, i|
+    printElem(_file, _spaces+2, elem)
+  end
 end
 
 #################################################
@@ -176,19 +203,6 @@ def printAttribute(_file, _spaces, _attr)
 # \param[in] _elem The SDF element to convert to an xml schema.
 def printXSD(_file, _spaces, _elem)
 
-  if !_elem.elements["description"].nil? &&
-     !_elem.elements["description"].text.nil?
-    printDocumentation(_file, _spaces, _elem.elements["description"].text)
-  end
-
-  _file.printf("%*s<xsd:include schemaLocation='http://sdformat.org/schemas/types.xsd'/>\n", _spaces, "")
-
-  # Print the inclues for the complex type
-  # The includes must appear first
-  _elem.get_elements("include").each do |inc|
-    printInclude(_file, _spaces, inc);
-  end
-
   if _elem.get_elements("element").size > 0 ||
      _elem.get_elements("attribute").size > 0 ||
      _elem.get_elements("include").size > 0
@@ -196,6 +210,12 @@ def printXSD(_file, _spaces, _elem)
     # Print the complex type with a name
     _file.printf("%*s<xsd:element name='%s'>\n", _spaces, "",
                  _elem.attributes["name"])
+
+    if !_elem.elements["description"].nil? &&
+      !_elem.elements["description"].text.nil?
+     printDocumentation(_file, _spaces+2, _elem.elements["description"].text)
+    end
+
     _file.printf("%*s<xsd:complexType>\n", _spaces+2, "")
 
     if _elem.attributes['name'] != "plugin" &&
@@ -209,10 +229,11 @@ def printXSD(_file, _spaces, _elem)
       printElem(_file, _spaces+6, elem);
     end
 
-    # Print all the included sdf's root elements
+    # print XSD of <include> element
     _elem.get_elements("include").each do |inc|
-      printIncludeRef(_file, _spaces+6, inc);
+      printInclude(_file, _spaces+4, inc);
     end
+
     if _elem.attributes['name'] != "plugin" &&
       (_elem.get_elements("element").size > 0 ||
        _elem.get_elements("include").size > 0)
@@ -295,6 +316,14 @@ def printXSD(_file, _spaces, _elem)
   file.print("<?xml version='1.0' encoding='UTF-8'?>\n")
   file.print("<xsd:schema xmlns:xsd='http://www.w3.org/2001/XMLSchema'>\n")
 
+  # drop xml header
+  File.readlines("#{$path}/schema/types.xsd").drop(1).each do |line|
+    # skip root <schema> element
+    if !line.include? "xsd:schema"
+      file.printf("%*s#{line}", spaces-2, "")
+    end
+  end
+
   printXSD(file, spaces, elem)
 
   file.print("</xsd:schema>\n")