diff --git a/test/atlas_description/atlas_foot.urdf b/test/atlas_description/atlas_foot.urdf
new file mode 100644
index 0000000..9f32e04
--- /dev/null
+++ b/test/atlas_description/atlas_foot.urdf
@@ -0,0 +1,1952 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from atlas.urdf.xacro               | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="atlas" xmlns:xacro="http://ros.org/wiki/xacro">
+  <link name="l_clav">
+    <inertial>
+      <mass value="3.45"/>
+      <origin rpy="0 -0 0" xyz="0 0.048 0.084"/>
+      <inertia ixx="0.011" ixy="0" ixz="0" iyy="0.009" iyz="-0.004" izz="0.004"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 -0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://atlas_description/meshes_v3/l_clav.dae" scale="1 1 1"/>
+      </geometry>
+    </visual>
+    <!--<collision>
+      <origin rpy="0 1.5707963267949 0" xyz="0 0.0697 0.0261"/>
+      <geometry>
+        <cylinder length="0.1525" radius="0.045"/>
+      </geometry>
+    </collision>-->
+    <collision>
+      <origin rpy="1.0472 0 0" xyz="-0.005 -0.0074 -0.0538"/>
+      <geometry>
+        <box size="0.13 0.15 0.05"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="l_farm">
+    <inertial>
+      <mass value="3.388"/>
+      <origin rpy="0 -0 0" xyz="0 -0.065 0"/>
+      <inertia ixx="0.00656" ixy="0" ixz="0" iyy="0.00358" iyz="0" izz="0.00656"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 -0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://atlas_description/meshes_v3/l_farm.dae" scale="1 1 1"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 1.5708 0" xyz="0.0 0.065 0.004"/>
+      <geometry>
+        <cylinder length="0.125" radius="0.045"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.001 0.015 -0.002"/>
+      <geometry>
+        <box size="0.135 0.1 0.045"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="l_foot">
+    <inertial>
+      <mass value="2.05"/>
+      <origin rpy="0 -0 0" xyz="0.027 0 -0.067"/>
+      <inertia ixx="0.002" ixy="0" ixz="0" iyy="0.007" iyz="0" izz="0.008"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 -0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://atlas_description/meshes_v3/l_foot.dae" scale="1 1 1"/>
+      </geometry>
+    </visual>
+    <!-- bottom -->
+    <collision>
+      <origin rpy="0 0 0" xyz="0.048 0 -0.056119"/>
+      <geometry>
+        <box size="0.26 0.124887 0.05"/>
+      </geometry>
+    </collision>
+    <!-- ankle FIXME: adding this collision body destabilizes robot, shouldn't
+        <collision>
+          <origin xyz="0.0 0 -0.0528735" rpy="0 0 0"/>
+          <geometry>
+            <cylinder radius="0.0373" length="0.037827"/>
+          </geometry>
+        </collision>
+        -->
+  </link>
+  <link name="l_hand">
+    <inertial>
+      <mass value="2.509"/>
+      <origin rpy="0 -0 0" xyz="0 0 0"/>
+      <inertia ixx="0.00265" ixy="0" ixz="0" iyy="0.00446" iyz="0" izz="0.00446"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 -0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://atlas_description/meshes_v3/l_hand.dae" scale="1 1 1"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267949 0 0" xyz="0.001 0.1 0.01"/>
+      <geometry>
+        <cylinder length="0.05" radius="0.03"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.001 0.05 0.01"/>
+      <geometry>
+        <box size="0.13 0.09 0.07"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="l_larm">
+    <inertial>
+      <mass value="2.509"/>
+      <origin rpy="0 -0 0" xyz="0 0 0"/>
+      <inertia ixx="0.00265" ixy="0" ixz="0" iyy="0.00446" iyz="0" izz="0.00446"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 -0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://atlas_description/meshes_v3/l_larm.dae" scale="1 1 1"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267949 0 0" xyz="0.001 0.127 0.00"/>
+      <geometry>
+        <cylinder length="0.125" radius="0.045"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.001 0.051 0.0"/>
+      <geometry>
+        <box size="0.135 0.15 0.06"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="l_lglut">
+    <inertial>
+      <mass value="0.866"/>
+      <origin rpy="0 -0 0" xyz="0.0133341 0.0170484 -0.0312052"/>
+      <inertia ixx="0.000691326" ixy="-2.24344e-05" ixz="2.50508e-06" iyy="0.00126856" iyz="0.000137862" izz="0.00106487"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 -0 0" xyz="-0.05 0 0.05"/><!--xyz="0 0 0"/>-->
+      <geometry>
+        <mesh filename="package://atlas_description/meshes_v3/l_lglut.dae" scale="1 1 1"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267949 0 0" xyz="0.049968 0.022488 -0.0501165"/>
+      <geometry>
+        <cylinder length="0.060306" radius="0.02009687"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 1.5707963267949 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.12" radius="0.02"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="l_lleg">
+    <inertial>
+      <mass value="5.479"/>
+      <origin rpy="0 -0 0" xyz="0.001 0 -0.187"/>
+      <inertia ixx="0.077" ixy="0" ixz="-0.003" iyy="0.076" iyz="0" izz="0.01"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 -0 0" xyz="0 0 0.422"/><!--xyz="0 0 0"/>-->
+      <geometry>
+        <mesh filename="package://atlas_description/meshes_v3/l_lleg.dae" scale="1 1 1"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0.10 0" xyz="0.02 0.00295 -0.18"/>
+      <geometry>
+        <cylinder length="0.40" radius="0.07"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="l_scap">
+    <inertial>
+      <mass value="3.012"/>
+      <origin rpy="0 -0 0" xyz="0 0 0"/>
+      <inertia ixx="0.00319" ixy="0" ixz="0" iyy="0.00583" iyz="0" izz="0.00583"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 -0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://atlas_description/meshes_v3/l_scap.dae" scale="1 1 1"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267949 0 0" xyz="0.001 0.12651 0.01521"/>
+      <geometry>
+        <cylinder length="0.125" radius="0.045"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.001 0.05 0.016"/>
+      <geometry>
+        <box size="0.15 0.15 0.06"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="l_talus">
+    <inertial>
+      <mass value="0.125"/>
+      <origin rpy="0 -0 0" xyz="0 0 0"/>
+      <inertia ixx="1.01674e-05" ixy="0" ixz="0" iyy="8.42775e-06" iyz="0" izz="1.30101e-05"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 -0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://atlas_description/meshes_v3/l_talus.dae" scale="1 1 1"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 1.5707963267949 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.029542" radius="0.010181"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="1.5707963267949 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.010059" radius="0.010348"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="l_uarm">
+    <inertial>
+      <mass value="3.388"/>
+      <origin rpy="0 -0 0" xyz="0 -0.065 0"/>
+      <inertia ixx="0.00656" ixy="0" ixz="0" iyy="0.00358" iyz="0" izz="0.00656"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 -0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://atlas_description/meshes_v3/l_uarm.dae" scale="1 1 1"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 1.5708 0" xyz="0.001 0.111 0.0"/>
+      <geometry>
+        <cylinder length="0.125" radius="0.045"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 1.5707963267949 0" xyz="0.001 0.043 0.01"/>
+      <geometry>
+        <box size="0.04 0.07 0.13"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="l_uglut">
+    <inertial>
+      <mass value="0.648"/>
+      <origin rpy="0 -0 0" xyz="0.00529262 -0.00344732 0.00313046"/>
+      <inertia ixx="0.00074276" ixy="-3.79607e-08" ixz="-2.79549e-05" iyy="0.000688179" iyz="-3.2735e-08" izz="0.00041242"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 -0 0" xyz="0 -0.089 0"/><!--xyz="0 0 0"/>-->
+      <geometry>
+        <mesh filename="package://atlas_description/meshes_v3/l_uglut.dae" scale="1 1 1"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 1.5707963267949 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.078396" radius="0.019096"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.080842" radius="0.019841"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="l_uleg">
+    <inertial>
+      <mass value="9.209"/>
+      <origin rpy="0 -0 0" xyz="0 0 -0.21"/>
+      <inertia ixx="0.09" ixy="0" ixz="0" iyy="0.09" iyz="0" izz="0.02"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 -0 0" xyz="0.05 0 0.374"/><!--xyz="0 0 0"/>-->
+      <geometry>
+        <mesh filename="package://atlas_description/meshes_v3/l_uleg.dae" scale="1 1 1"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.0179 0.02085 -0.13"/>
+      <geometry>
+        <cylinder length="0.10" radius="0.1"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 -0.3 0" xyz="-0.03 0.01 -0.23"/>
+      <geometry>
+        <cylinder length="0.15" radius="0.07"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0.3 0" xyz="-0.005 0.01 -0.23"/>
+      <geometry>
+        <cylinder length="0.15" radius="0.07"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="-0.25 0 0" xyz="-0.02 0.03 -0.23"/>
+      <geometry>
+        <cylinder length="0.15" radius="0.07"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="ltorso">
+    <inertial>
+      <mass value="2.409"/>
+      <origin rpy="0 -0 0" xyz="-0.0112984 -3.15366e-06 0.0746835"/>
+      <inertia ixx="0.0039092" ixy="-5.04491e-08" ixz="-0.000342157" iyy="0.00341694" iyz="4.87119e-07" izz="0.00174492"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 -0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://atlas_description/meshes_v3/ltorso.dae" scale="1 1 1"/>
+      </geometry>
+    </visual>
+    <!-- this part is completely internal
+        <collision>
+          <origin rpy="0 0 0" xyz="0 0 0.039762"/>
+          <geometry>
+            <cylinder radius="0.025918" length="0.076922"/>
+          </geometry>
+        </collision>
+        <collision>
+          <origin rpy="1.5707963267949 0 0" xyz="0 0.0250065 0.09025"/>
+          <geometry>
+            <cylinder radius="0.014828" length="0.020367"/>
+          </geometry>
+        </collision>
+        <collision>
+          <origin rpy="1.5707963267949 0 0" xyz="0 -0.0248895 0.09025"/>
+          <geometry>
+            <cylinder radius="0.014828" length="0.020169"/>
+          </geometry>
+        </collision>
+        -->
+  </link>
+  <link name="mtorso">
+    <inertial>
+      <mass value="0.69"/>
+      <origin rpy="0 -0 0" xyz="-0.00816266 -0.0131245 0.0305974"/>
+      <inertia ixx="0.000454181" ixy="-6.10764e-05" ixz="3.94009e-05" iyy="0.000483282" iyz="5.27463e-05" izz="0.000444215"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 -0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://atlas_description/meshes_v3/mtorso.dae" scale="1 1 1"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267949 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.03" radius="0.017"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 1.5707963267949 0" xyz="0.0285 0 0.05"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.015"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 1.5707963267949 0" xyz="-0.0285 0 0.05"/>
+      <geometry>
+        <cylinder length="0.02" radius="0.015"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="pelvis">
+    <inertial>
+      <mass value="17.882"/>
+      <origin rpy="0 -0 0" xyz="0.0111 0 0.0271"/>
+      <inertia ixx="0.1244" ixy="0.0008" ixz="-0.0007" iyy="0.0958" iyz="-0.0005" izz="0.1167"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 -0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://atlas_description/meshes_v3/pelvis.dae" scale="1 1 1"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267949 0 0" xyz="0.046 0.0 -0.02"/>
+      <geometry>
+        <cylinder length="0.06" radius="0.11"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="1.5707963267949 0 0" xyz="-0.03 0.0 -0.02"/>
+      <geometry>
+        <cylinder length="0.06" radius="0.11"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.01 0.042 0.09"/>
+      <geometry>
+        <cylinder length="0.05" radius="0.16"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.01 -0.042 0.09"/>
+      <geometry>
+        <cylinder length="0.05" radius="0.16"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.1 0 -0.05"/>
+      <geometry>
+        <box size="0.1 0.15 0.2"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="r_clav">
+    <inertial>
+      <mass value="3.45"/>
+      <origin rpy="0 -0 0" xyz="0 -0.048 0.084"/>
+      <inertia ixx="0.011" ixy="0" ixz="0" iyy="0.009" iyz="0.004" izz="0.004"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 -0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://atlas_description/meshes_v3/r_clav.dae" scale="1 1 1"/>
+      </geometry>
+    </visual>
+    <!--<collision>
+      <origin rpy="0 1.5707963267949 0" xyz="0 -0.0697 0.0261"/>
+      <geometry>
+        <cylinder length="0.1525" radius="0.045"/>
+      </geometry>
+    </collision>-->
+    <collision>
+      <origin rpy="-1.0472 0 0" xyz="-0.005 0.0074 -0.0538"/>
+      <geometry>
+        <box size="0.13 0.15 0.05"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="r_farm">
+    <inertial>
+      <mass value="3.388"/>
+      <origin rpy="0 -0 0" xyz="0 0.065 0"/>
+      <inertia ixx="0.00656" ixy="0" ixz="0" iyy="0.00358" iyz="0" izz="0.00656"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 -0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://atlas_description/meshes_v3/r_farm.dae" scale="1 1 1"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 1.5708 0" xyz="0.0 -0.065 0.004"/>
+      <geometry>
+        <cylinder length="0.125" radius="0.045"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.001 -0.015 -0.002"/>
+      <geometry>
+        <box size="0.135 0.1 0.045"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="r_foot">
+    <inertial>
+      <mass value="2.05"/>
+      <origin rpy="0 -0 0" xyz="0.027 0 -0.067"/>
+      <inertia ixx="0.002" ixy="0" ixz="0" iyy="0.007" iyz="0" izz="0.008"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 -0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://atlas_description/meshes_v3/r_foot.dae" scale="1 1 1"/>
+      </geometry>
+    </visual>
+    <!-- bottom -->
+    <collision>
+      <origin rpy="0 0 0" xyz="0.048 0 -0.056119"/>
+      <geometry>
+        <box size="0.26 0.124887 0.05"/>
+      </geometry>
+    </collision>
+    <!-- ankle FIXME: adding this collision body destabilizes robot, shouldn't
+        <collision>
+          <origin xyz="0.0 0 -0.0528735" rpy="0 0 0"/>
+          <geometry>
+            <cylinder radius="0.0373" length="0.037827"/>
+          </geometry>
+        </collision>
+        -->
+  </link>
+  <link name="r_hand">
+    <inertial>
+      <mass value="2.509"/>
+      <origin rpy="0 -0 0" xyz="0 0 0"/>
+      <inertia ixx="0.00265" ixy="0" ixz="0" iyy="0.00446" iyz="0" izz="0.00446"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 -0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://atlas_description/meshes_v3/r_hand.dae" scale="1 1 1"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267949 0 0" xyz="0.001 -0.1 0.01"/>
+      <geometry>
+        <cylinder length="0.05" radius="0.03"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.001 -0.05 0.01"/>
+      <geometry>
+        <box size="0.13 0.09 0.07"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="r_larm">
+    <inertial>
+      <mass value="2.509"/>
+      <origin rpy="0 -0 0" xyz="0 0 0"/>
+      <inertia ixx="0.00265" ixy="0" ixz="0" iyy="0.00446" iyz="0" izz="0.00446"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 -0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://atlas_description/meshes_v3/r_larm.dae" scale="1 1 1"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267949 0 0" xyz="0.001 -0.127 0.00"/>
+      <geometry>
+        <cylinder length="0.125" radius="0.045"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.001 -0.051 0.0"/>
+      <geometry>
+        <box size="0.135 0.15 0.06"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="r_lglut">
+    <inertial>
+      <mass value="0.866"/>
+      <origin rpy="0 -0 0" xyz="0.0133341 -0.0170484 -0.0312052"/>
+      <inertia ixx="0.000691326" ixy="2.24344e-05" ixz="2.50508e-06" iyy="0.00126856" iyz="-0.000137862" izz="0.00106487"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 -0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://atlas_description/meshes_v3/r_lglut.dae" scale="1 1 1"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267949 0 0" xyz="0.049968 -0.022488 -0.0501165"/>
+      <geometry>
+        <cylinder length="0.060306" radius="0.02009687"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 1.5707963267949 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.12" radius="0.02"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="r_lleg">
+    <inertial>
+      <mass value="5.479"/>
+      <origin rpy="0 -0 0" xyz="0.001 0 -0.187"/>
+      <inertia ixx="0.077" ixy="-0" ixz="-0.003" iyy="0.076" iyz="-0" izz="0.01"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 -0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://atlas_description/meshes_v3/r_lleg.dae" scale="1 1 1"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0.10 0" xyz="0.02 -0.00295 -0.18"/>
+      <geometry>
+        <cylinder length="0.40" radius="0.07"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="r_scap">
+    <inertial>
+      <mass value="3.012"/>
+      <origin rpy="0 -0 0" xyz="0 0 0"/>
+      <inertia ixx="0.00319" ixy="0" ixz="0" iyy="0.00583" iyz="0" izz="0.00583"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 -0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://atlas_description/meshes_v3/r_scap.dae" scale="1 1 1"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="1.5707963267949 0 0" xyz="0.001 -0.12651 0.01521"/>
+      <geometry>
+        <cylinder length="0.125" radius="0.045"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.001 -0.05 0.016"/>
+      <geometry>
+        <box size="0.15 0.15 0.06"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="r_talus">
+    <inertial>
+      <mass value="0.125"/>
+      <origin rpy="0 -0 0" xyz="0 0 0"/>
+      <inertia ixx="1.01674e-05" ixy="0" ixz="0" iyy="8.42775e-06" iyz="0" izz="1.30101e-05"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 -0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://atlas_description/meshes_v3/r_talus.dae" scale="1 1 1"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 1.5707963267949 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.029542" radius="0.010181"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="1.5707963267949 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.010059" radius="0.010348"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="r_uarm">
+    <inertial>
+      <mass value="3.388"/>
+      <origin rpy="0 -0 0" xyz="0 0.065 0"/>
+      <inertia ixx="0.00656" ixy="0" ixz="0" iyy="0.00358" iyz="0" izz="0.00656"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 -0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://atlas_description/meshes_v3/r_uarm.dae" scale="1 1 1"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 1.5708 0" xyz="0.001 -0.111 0.0"/>
+      <geometry>
+        <cylinder length="0.125" radius="0.045"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 1.5707963267949 0" xyz="0.001 -0.043 0.01"/>
+      <geometry>
+        <box size="0.04 0.07 0.13"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="r_uglut">
+    <inertial>
+      <mass value="0.648"/>
+      <origin rpy="0 -0 0" xyz="0.00529262 0.00344732 0.00313046"/>
+      <inertia ixx="0.00074276" ixy="3.79607e-08" ixz="-2.79549e-05" iyy="0.000688179" iyz="3.2735e-08" izz="0.00041242"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 -0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://atlas_description/meshes_v3/r_uglut.dae" scale="1 1 1"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 1.5707963267949 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.078396" radius="0.019096"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <cylinder length="0.080842" radius="0.019841"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="r_uleg">
+    <inertial>
+      <mass value="9.209"/>
+      <origin rpy="0 -0 0" xyz="0 0 -0.21"/>
+      <inertia ixx="0.09" ixy="0" ixz="0" iyy="0.09" iyz="0" izz="0.02"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 -0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://atlas_description/meshes_v3/r_uleg.dae" scale="1 1 1"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.0179 -0.02085 -0.13"/>
+      <geometry>
+        <cylinder length="0.10" radius="0.1"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 -0.3 0" xyz="-0.03 -0.01 -0.23"/>
+      <geometry>
+        <cylinder length="0.15" radius="0.07"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0.3 0" xyz="-0.005 -0.01 -0.23"/>
+      <geometry>
+        <cylinder length="0.15" radius="0.07"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0.25 0 0" xyz="-0.02 -0.03 -0.23"/>
+      <geometry>
+        <cylinder length="0.15" radius="0.07"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="utorso">
+    <inertial>
+      <mass value="52.007"/>
+      <origin rpy="0 -0 0" xyz="-0.0923 0 0.3"/>
+      <inertia ixx="1.466" ixy="0.00362" ixz="0.336" iyy="1.51" iyz="0.001" izz="1.3"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 -0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://atlas_description/meshes_v3/utorso.dae" scale="1 1 1"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 -0 0" xyz="-0.04 0 0.38"/>
+      <geometry>
+        <box size="0.58 0.35 0.68"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="-0.5236 0 0" xyz="0.065 0.16 0.18"/>
+      <geometry>
+        <cylinder length="0.22375" radius="0.048"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0.5236 0 0" xyz="0.065 -0.16 0.18"/>
+      <geometry>
+        <cylinder length="0.22375" radius="0.048"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin rpy="0 -0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://atlas_description/meshes_v3/utorso_pack.dae" scale="1 1 1"/>
+      </geometry>
+    </visual>
+    <!--<collision>
+      <origin rpy="0 -0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://atlas_description/meshes_v3/utorso_pack.dae" scale="1 1 1"/>
+      </geometry>
+    </collision>-->
+  </link>
+  <joint name="back_bkx" type="revolute">
+    <origin rpy="0 -0 0" xyz="0 0 0.05"/>
+    <axis xyz="1 0 0"/>
+    <parent link="mtorso"/>
+    <child link="utorso"/>
+    <dynamics damping="0.1" friction="0"/>
+    <limit effort="200" lower="-0.72" upper="0.69" velocity="12"/>
+    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.6981" soft_upper_limit="10.6981"/>
+  </joint>
+  <joint name="back_bky" type="revolute">
+    <origin rpy="0 -0 0" xyz="0 0 0.16197"/>
+    <axis xyz="0 1 0"/>
+    <parent link="ltorso"/>
+    <child link="mtorso"/>
+    <dynamics damping="0.1" friction="0"/>
+    <limit effort="206.843" lower="-0.64" upper="0.45" velocity="12"/>
+    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.6107" soft_upper_limit="10.4384"/>
+  </joint>
+  <joint name="back_bkz" type="revolute">
+    <origin rpy="0 -0 0" xyz="-0.0125 0 0"/>
+    <axis xyz="0 0 1"/>
+    <parent link="pelvis"/>
+    <child link="ltorso"/>
+    <dynamics damping="0.1" friction="0"/>
+    <limit effort="124.016" lower="-0.64" upper="0.62" velocity="12"/>
+    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.6632" soft_upper_limit="10.6632"/>
+  </joint>
+  <joint name="l_arm_elx" type="revolute">
+    <origin rpy="0 -0 0" xyz="0 0.119 0.00921"/>
+    <axis xyz="1 0 0"/>
+    <parent link="l_uarm"/>
+    <child link="l_larm"/>
+    <dynamics damping="0.1" friction="0"/>
+    <limit effort="114" lower="-0.04" upper="2.31" velocity="12"/>
+    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10" soft_upper_limit="12.3562"/>
+  </joint>
+  <joint name="l_arm_ely" type="revolute">
+    <origin rpy="0 -0 0" xyz="0 0.187 0.016"/>
+    <axis xyz="0 1 0"/>
+    <parent link="l_scap"/>
+    <child link="l_uarm"/>
+    <dynamics damping="0.1" friction="0"/>
+    <limit effort="114" lower="0" upper="3.14" velocity="12"/>
+    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10" soft_upper_limit="13.1416"/>
+  </joint>
+  <joint name="l_arm_shx" type="revolute">
+    <origin rpy="0 -0 0" xyz="0 0.14035 0.19609"/>
+    <axis xyz="1 0 0"/>
+    <parent link="l_clav"/>
+    <child link="l_scap"/>
+    <dynamics damping="0.1" friction="0"/>
+    <limit effort="170" lower="-1.57" upper="1.57" velocity="12"/>
+    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11.5708" soft_upper_limit="11.5708"/>
+  </joint>
+  <joint name="l_arm_shy" type="revolute">
+    <origin rpy="0 -0 0" xyz="0.06441 0.13866 0.10718"/>
+    <axis xyz="0 0.5 0.866025"/>
+    <parent link="utorso"/>
+    <child link="l_clav"/>
+    <dynamics damping="0.1" friction="0"/>
+    <limit effort="212" lower="-1.57" upper="0.78" velocity="12"/>
+    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11.5708" soft_upper_limit="10.7854"/>
+  </joint>
+  <joint name="l_arm_wrx" type="revolute">
+    <origin rpy="0 -0 0" xyz="0 0.119 0.00921"/>
+    <axis xyz="1 0 0"/>
+    <parent link="l_farm"/>
+    <child link="l_hand"/>
+    <dynamics damping="0.1" friction="0"/>
+    <limit effort="60" lower="-1.18" upper="1.17" velocity="12"/>
+    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11.1781" soft_upper_limit="11.1781"/>
+  </joint>
+  <joint name="l_arm_wry" type="revolute">
+    <origin rpy="0 -0 0" xyz="0 0.187 -0.00921"/>
+    <axis xyz="0 1 0"/>
+    <parent link="l_larm"/>
+    <child link="l_farm"/>
+    <dynamics damping="0.1" friction="0"/>
+    <limit effort="114" lower="0.01" upper="3.15" velocity="12"/>
+    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10" soft_upper_limit="13.1416"/>
+  </joint>
+  <joint name="l_leg_akx" type="revolute">
+    <origin rpy="0 -0 0" xyz="0 0 0"/>
+    <axis xyz="-1 0 0"/>
+    <child link="l_talus"/>
+    <parent link="l_foot"/>
+    <dynamics damping="0.1" friction="0"/>
+    <limit effort="90" lower="-0.61" upper="0.61" velocity="12"/>
+    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.8" soft_upper_limit="10.8"/>
+  </joint>
+  <joint name="l_leg_aky" type="revolute">
+    <origin rpy="0 -0 0" xyz="0 0 0"/> <!-- xyz="0 0 0.422"/> -->
+    <axis xyz="0 -1 0"/>
+    <child link="l_lleg"/>
+    <parent link="l_talus"/>
+    <dynamics damping="0.1" friction="0"/>
+    <limit effort="700" lower="-1" upper="0.72" velocity="12"/>
+    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11" soft_upper_limit="10.7"/>
+  </joint>
+  <joint name="l_leg_hpx" type="revolute">
+    <origin rpy="0 -0 0" xyz="-0.05 0 0.05"/><!--xyz="0 0 0"/>-->
+    <axis xyz="-1 0 0"/>
+    <child link="l_uglut"/>
+    <parent link="l_lglut"/>
+    <dynamics damping="0.1" friction="0"/>
+    <limit effort="180" lower="-0.34" upper="0.53" velocity="12"/>
+    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.5236" soft_upper_limit="10.5236"/>
+  </joint>
+  <joint name="l_leg_hpy" type="revolute">
+    <origin rpy="0 -0 0" xyz="0.05 0 0.374"/><!--xyz="-0.05 0 0.05"/>-->
+    <axis xyz="0 -1 0"/>
+    <child link="l_lglut"/>
+    <parent link="l_uleg"/>
+    <dynamics damping="0.1" friction="0"/>
+    <limit effort="260" lower="-1.75" upper="0.54" velocity="12"/>
+    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11.7207" soft_upper_limit="10.5248"/>
+  </joint>
+  <joint name="l_leg_hpz" type="revolute">
+    <origin rpy="0 -0 0" xyz="0 -0.089 0"/><!--xyz="0 -0.089 0"/>-->
+    <axis xyz="0 0 -1"/>
+    <child link="pelvis"/>
+    <parent link="l_uglut"/>
+    <dynamics damping="0.1" friction="0"/>
+    <limit effort="110" lower="-0.17" upper="1.22" velocity="12"/>
+    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.1745" soft_upper_limit="11.2217"/>
+  </joint>
+  <joint name="l_leg_kny" type="revolute">
+    <origin rpy="0 -0 0" xyz="0 0 0.422"/> <!--xyz="0.05 0 0.374"/>-->
+    <axis xyz="0 -1 0"/>
+    <child link="l_uleg"/>
+    <parent link="l_lleg"/>
+    <dynamics damping="0.1" friction="0"/>
+    <limit effort="220" lower="0" upper="2.35" velocity="12"/>
+    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10" soft_upper_limit="12.3857"/>
+  </joint>
+  <joint name="neck_ay" type="revolute">
+    <origin rpy="0 -0 0" xyz="0.21672 0 0.53796"/>
+    <axis xyz="0 1 0"/>
+    <parent link="utorso"/>
+    <child link="head_root"/>
+    <dynamics damping="0.1" friction="0"/>
+    <limit effort="5" lower="-0.61" upper="1.15" velocity="12"/>
+    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.6021" soft_upper_limit="11.1449"/>
+  </joint>
+  <joint name="r_arm_elx" type="revolute">
+    <origin rpy="0 -0 0" xyz="0 -0.119 0.00921"/>
+    <axis xyz="1 0 0"/>
+    <parent link="r_uarm"/>
+    <child link="r_larm"/>
+    <dynamics damping="0.1" friction="0"/>
+    <limit effort="114" lower="-2.32" upper="0" velocity="12"/>
+    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-12.3562" soft_upper_limit="10"/>
+  </joint>
+  <joint name="r_arm_ely" type="revolute">
+    <origin rpy="0 -0 0" xyz="0 -0.187 0.016"/>
+    <axis xyz="0 1 0"/>
+    <parent link="r_scap"/>
+    <child link="r_uarm"/>
+    <dynamics damping="0.1" friction="0"/>
+    <limit effort="114" lower="0.01" upper="3.15" velocity="12"/>
+    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10" soft_upper_limit="13.1416"/>
+  </joint>
+  <joint name="r_arm_shx" type="revolute">
+    <origin rpy="0 -0 0" xyz="0 -0.14035 0.19609"/>
+    <axis xyz="1 0 0"/>
+    <parent link="r_clav"/>
+    <child link="r_scap"/>
+    <dynamics damping="0.1" friction="0"/>
+    <limit effort="170" lower="-1.57" upper="1.57" velocity="12"/>
+    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11.5708" soft_upper_limit="11.5708"/>
+  </joint>
+  <joint name="r_arm_shy" type="revolute">
+    <origin rpy="0 -0 0" xyz="0.06441 -0.13866 0.10718"/>
+    <axis xyz="0 0.5 -0.866025"/>
+    <parent link="utorso"/>
+    <child link="r_clav"/>
+    <dynamics damping="0.1" friction="0"/>
+    <limit effort="212" lower="-1.56" upper="0.79" velocity="12"/>
+    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11.5708" soft_upper_limit="10.7854"/>
+  </joint>
+  <joint name="r_arm_wrx" type="revolute">
+    <origin rpy="0 -0 0" xyz="0 -0.119 0.00921"/>
+    <axis xyz="1 0 0"/>
+    <parent link="r_farm"/>
+    <child link="r_hand"/>
+    <dynamics damping="0.1" friction="0"/>
+    <limit effort="60" lower="-1.18" upper="1.18" velocity="12"/>
+    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11.1781" soft_upper_limit="11.1781"/>
+  </joint>
+  <joint name="r_arm_wry" type="revolute">
+    <origin rpy="0 -0 0" xyz="0 -0.187 -0.00921"/>
+    <axis xyz="0 1 0"/>
+    <parent link="r_larm"/>
+    <child link="r_farm"/>
+    <dynamics damping="0.1" friction="0"/>
+    <limit effort="114" lower="0" upper="3.15" velocity="12"/>
+    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10" soft_upper_limit="13.1416"/>
+  </joint>
+  <joint name="r_leg_akx" type="revolute">
+    <origin rpy="0 -0 0" xyz="0 0 0"/>
+    <axis xyz="1 0 0"/>
+    <parent link="r_talus"/>
+    <child link="r_foot"/>
+    <dynamics damping="0.1" friction="0"/>
+    <limit effort="90" lower="-0.61" upper="0.61" velocity="12"/>
+    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.8" soft_upper_limit="10.8"/>
+  </joint>
+  <joint name="r_leg_aky" type="revolute">
+    <origin rpy="0 -0 0" xyz="0 0 -0.422"/>
+    <axis xyz="0 1 0"/>
+    <parent link="r_lleg"/>
+    <child link="r_talus"/>
+    <dynamics damping="0.1" friction="0"/>
+    <limit effort="700" lower="-1" upper="0.72" velocity="12"/>
+    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11" soft_upper_limit="10.7"/>
+  </joint>
+  <joint name="r_leg_hpx" type="revolute">
+    <origin rpy="0 -0 0" xyz="0 0 0"/>
+    <axis xyz="1 0 0"/>
+    <parent link="r_uglut"/>
+    <child link="r_lglut"/>
+    <dynamics damping="0.1" friction="0"/>
+    <limit effort="180" lower="-0.46" upper="0.35" velocity="12"/>
+    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.5236" soft_upper_limit="10.5236"/>
+  </joint>
+  <joint name="r_leg_hpy" type="revolute">
+    <origin rpy="0 -0 0" xyz="0.05 0 -0.05"/>
+    <axis xyz="0 1 0"/>
+    <parent link="r_lglut"/>
+    <child link="r_uleg"/>
+    <dynamics damping="0.1" friction="0"/>
+    <limit effort="260" lower="-1.75" upper="0.52" velocity="12"/>
+    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11.7207" soft_upper_limit="10.5248"/>
+  </joint>
+  <joint name="r_leg_hpz" type="revolute">
+    <origin rpy="0 -0 0" xyz="0 -0.089 0"/>
+    <axis xyz="0 0 1"/>
+    <parent link="pelvis"/>
+    <child link="r_uglut"/>
+    <dynamics damping="0.1" friction="0"/>
+    <limit effort="110" lower="-1.21" upper="0.18" velocity="12"/>
+    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11.2217" soft_upper_limit="10.1745"/>
+  </joint>
+  <joint name="r_leg_kny" type="revolute">
+    <origin rpy="0 -0 0" xyz="-0.05 0 -0.374"/>
+    <axis xyz="0 1 0"/>
+    <parent link="r_uleg"/>
+    <child link="r_lleg"/>
+    <dynamics damping="0.1" friction="0"/>
+    <limit effort="220" lower="0" upper="2.39" velocity="12"/>
+    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10" soft_upper_limit="12.3857"/>
+  </joint>
+  <link name="r_situational_awareness_camera_link">
+    <inertial>
+      <mass value="0.1"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+    </visual>
+  </link>
+  <joint name="r_situational_awareness_camera_joint" type="fixed">
+    <parent link="utorso"/>
+    <child link="r_situational_awareness_camera_link"/>
+    <!-- if camera points x-forward, bottom towards front of robot, 15 degree yaw forwards from
+           lateral (+y) view -->
+    <origin rpy="1.57079633 0 -1.30899694" xyz="0.216409 -0.121799 0.406759"/>
+  </joint>
+  <joint name="r_situational_awareness_camera_optical_frame_joint" type="fixed">
+    <origin rpy="-1.5708 0.0 -1.5708" xyz="0 0 0"/>
+    <parent link="r_situational_awareness_camera_link"/>
+    <child link="r_situational_awareness_camera_optical_frame"/>
+  </joint>
+  <link name="r_situational_awareness_camera_optical_frame"/>
+  <link name="l_situational_awareness_camera_link">
+    <inertial>
+      <mass value="0.1"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00001" iyz="0" izz="0.00001"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+    </visual>
+  </link>
+  <joint name="l_situational_awareness_camera_joint" type="fixed">
+    <parent link="utorso"/>
+    <child link="l_situational_awareness_camera_link"/>
+    <!-- if camera points x-forward, bottom towards front of robot, 15 degree yaw forwards from
+           lateral (+y) view -->
+    <origin rpy="-1.57079633 0 1.30899694" xyz="0.216409 0.121799 0.406759"/>
+  </joint>
+  <joint name="l_situational_awareness_camera_optical_frame_joint" type="fixed">
+    <origin rpy="-1.5708 0.0 -1.5708" xyz="0 0 0"/>
+    <parent link="l_situational_awareness_camera_link"/>
+    <child link="l_situational_awareness_camera_optical_frame"/>
+  </joint>
+  <link name="l_situational_awareness_camera_optical_frame"/>
+  <link name="imu_link">
+    <inertial>
+      <mass value="0.1"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+    </inertial>
+    <visual>
+      <origin rpy="0.00 -0.00 0.785413" xyz="-0.0905 -0.0000 -0.0120"/>
+      <geometry>
+        <box size="0.01 0.01 0.01"/>
+      </geometry>
+    </visual>
+  </link>
+  <joint name="imu_joint" type="fixed">
+    <parent link="pelvis"/>
+    <child link="imu_link"/>
+    <origin rpy="0 0 0" xyz="0.05991527603 0 -0.01415"/>
+  </joint>
+  <link name="head_root"/>
+  <joint name="head_root_joint" type="fixed">
+    <parent link="head_root"/>
+    <child link="head"/>
+  </joint>
+  <link name="head">
+    <inertial>
+      <origin rpy="0 0 0" xyz="-0.075493 3.3383E-05 0.02774"/>
+      <mass value="1.4199"/>
+      <inertia ixx="0.0039688" ixy="-1.5797E-06" ixz="-0.00089293" iyy="0.0041178" iyz="-6.8415E-07" izz="0.0035243"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0  0"/>
+      <geometry>
+        <mesh filename="package://multisense_description/meshes/head.dae"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.9098 0.44314 0.031373 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.0503 0 -0.00195"/>
+      <geometry>
+        <box size="0.1311 0.12 0.0591"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="left_camera_optical_joint" type="continuous">
+    <parent link="left_camera_optical_frame"/>
+    <child link="motor"/>
+  </joint>
+  <link name="motor"/>
+  <link name="spindle"/>
+  <joint name="motor_joint" type="continuous">
+    <parent link="motor"/>
+    <child link="spindle"/>
+  </joint>
+  <joint name="spindle_joint" type="continuous">
+    <parent link="spindle"/>
+    <child link="hokuyo_link"/>
+  </joint>
+  <link name="hokuyo_link">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0.012428 0.0004084 -0.0041835"/>
+      <mass value="0.057664"/>
+      <inertia ixx="3.7174E-05" ixy="4.9927E-08" ixz="1.1015E-05" iyy="4.2412E-05" iyz="-9.8165E-09" izz="4.167E-05"/>
+    </inertial>
+    <visual>
+      <origin rpy="-1.9179632679 -1.57079632679 0" xyz="-.03 -.1 0"/>
+      <geometry>
+        <mesh filename="package://multisense_description/meshes/head_camera.dae"/>
+      </geometry>
+      <material name="">
+        <color rgba="0.72941 0.35686 0.023529 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.03 0 0.0235"/>
+      <geometry>
+        <cylinder length="0.047" radius="0.024425"/>
+      </geometry>
+    </collision>
+  </link>
+  <link name="head_hokuyo_frame"/>
+  <joint name="head_hokuyo_frame_joint" type="fixed">
+    <!--
+     This transform accounts for the fact ROS expects laser data in the x forward,
+     y left, z up frame and the calibraton has the data in x left, y up, z forward.
+     Thus a roll of -90 * a pitch of -90 will make the appropriate transform. Note
+     Roll and pitch are both with respect to the origional x left, y up, z forward frame
+     -->
+    <origin rpy="-1.57079632679 -1.57079632679 0.0" xyz="0 0 0"/>
+    <parent link="hokuyo_link"/>
+    <child link="head_hokuyo_frame"/>
+  </joint>
+  <joint name="left_camera_frame_joint" type="fixed">
+    <!-- optical frame collocated with tilting DOF -->
+    <origin xyz="0.0 0.035 -0.002"/>
+    <parent link="head"/>
+    <child link="left_camera_frame"/>
+  </joint>
+  <link name="left_camera_frame">
+    <inertial>
+      <mass value="0.1"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
+    </inertial>
+  </link>
+  <joint name="left_camera_optical_frame_joint" type="fixed">
+    <origin rpy="-1.57079632679 0.0 -1.57079632679" xyz="0 0 0"/>
+    <parent link="left_camera_frame"/>
+    <child link="left_camera_optical_frame"/>
+  </joint>
+  <link name="left_camera_optical_frame"/>
+  <joint name="right_camera_frame_joint" type="fixed">
+    <origin xyz="0.0 -0.035 -0.002"/>
+    <parent link="head"/>
+    <child link="right_camera_frame"/>
+  </joint>
+  <link name="right_camera_frame">
+    <inertial>
+      <mass value="0.1"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
+    </inertial>
+  </link>
+  <joint name="right_camera_optical_frame_joint" type="fixed">
+    <origin rpy="-1.5708 0.0 -1.5708" xyz="0 0 0"/>
+    <parent link="right_camera_frame"/>
+    <child link="right_camera_optical_frame"/>
+  </joint>
+  <link name="right_camera_optical_frame"/>
+  <joint name="center_top_led_frame_joint" type="fixed">
+    <origin xyz="0.01125 0.0 0.0105"/>
+    <parent link="head"/>
+    <child link="center_top_led_frame"/>
+  </joint>
+  <link name="center_top_led_frame">
+    <inertial>
+      <mass value="0.1"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
+    </inertial>
+  </link>
+  <joint name="center_bottom_led_frame_joint" type="fixed">
+    <origin xyz="0.01125 0.0 -0.0155"/>
+    <parent link="head"/>
+    <child link="center_bottom_led_frame"/>
+  </joint>
+  <link name="center_bottom_led_frame">
+    <inertial>
+      <mass value="0.1"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
+    </inertial>
+  </link>
+  <joint name="left_led_frame_joint" type="fixed">
+    <origin rpy="0 -0.15 0.53" xyz="-0.01443 0.07452 0.050346"/>
+    <parent link="head"/>
+    <child link="left_led_frame"/>
+  </joint>
+  <link name="left_led_frame">
+    <inertial>
+      <mass value="0.1"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
+    </inertial>
+  </link>
+  <joint name="right_led_frame_joint" type="fixed">
+    <origin rpy="0 -0.15 -0.53" xyz="-0.01443 -0.07452 0.050346"/>
+    <parent link="head"/>
+    <child link="right_led_frame"/>
+  </joint>
+  <link name="right_led_frame">
+    <inertial>
+      <mass value="0.1"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.001" ixy="0.0" ixz="0.0" iyy="0.001" iyz="0.0" izz="0.001"/>
+    </inertial>
+  </link>
+  <link name="accel"/>
+  <joint name="accel_joint" type="fixed">
+    <origin rpy="0 1.5707 0" xyz="-.0475 .0302 -.00275"/>
+    <parent link="head_root"/>
+    <child link="accel"/>
+  </joint>
+  <link name="mag"/>
+  <joint name="mag_joint" type="fixed">
+    <origin rpy="0 1.5707 0" xyz="-.0475 .0302 -.00275"/>
+    <parent link="head_root"/>
+    <child link="mag"/>
+  </joint>
+  <link name="gyro"/>
+  <joint name="gyro_joint" type="fixed">
+    <origin rpy="1.5707 3.14159 1.5707 " xyz="-.0476 .0388 -.00075"/>
+    <parent link="head_root"/>
+    <child link="gyro"/>
+  </joint>
+  <link name="r_finger_1_link_0">
+    <visual>
+      <origin rpy="0 0 0" xyz="0.020 0 0"/>
+      <geometry>
+        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/robotiq_link_0.dae"/>
+      </geometry>
+      <material name="green">
+        <color rgba="0 1 0 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="r_finger_1_link_1">
+    <visual>
+      <origin rpy="0 0 -0.52" xyz="0.050 -.028 0"/>
+      <geometry>
+        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/robotiq_link_1.dae"/>
+      </geometry>
+      <material name="green"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 -0.5" xyz="0.000 -0.015 0"/>
+      <geometry>
+        <box size="0.1 0.045 0.035"/>
+      </geometry>
+      <material name="yellow">
+        <color rgba="0 1 1 1"/>
+      </material>
+    </collision>
+  </link>
+  <link name="r_finger_1_link_2">
+    <!--
+      NOTE: This link has an offset in the z-direction based on the models that were
+      used to generate grap-it poses.  With the offset below the model appears visually
+      correct, but it is suspect.
+      -->
+    <visual>
+      <origin rpy="0 0 0" xyz="0.039 0 0.0075"/>
+      <geometry>
+        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/robotiq_link_2.dae"/>
+      </geometry>
+      <material name="green"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.02 -0.01 0"/>
+      <geometry>
+        <box size="0.05 0.04 0.035"/>
+      </geometry>
+      <material name="yellow"/>
+    </collision>
+  </link>
+  <link name="r_finger_1_link_3">
+    <visual>
+      <origin rpy="0 0 0.52" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/robotiq_link_3.dae"/>
+      </geometry>
+      <material name="green"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0.5" xyz="0.018 0.003 0"/>
+      <geometry>
+        <box size="0.045 0.03 0.035"/>
+      </geometry>
+      <material name="yellow"/>
+    </collision>
+  </link>
+  <joint name="r_finger_1_joint_1" type="fixed">
+    <parent link="r_finger_1_link_0"/>
+    <child link="r_finger_1_link_1"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="0.020 0 0"/>
+    <limit effort="100" lower="0" upper="3.1416" velocity="100"/>
+  </joint>
+  <joint name="r_finger_1_joint_2" type="fixed">
+    <parent link="r_finger_1_link_1"/>
+    <child link="r_finger_1_link_2"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 -0.52" xyz="0.050 -.028 0"/>
+    <limit effort="100" lower="0" upper="3.1416" velocity="100"/>
+  </joint>
+  <joint name="r_finger_1_joint_3" type="fixed">
+    <parent link="r_finger_1_link_2"/>
+    <child link="r_finger_1_link_3"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="0.039 0 0"/>
+    <limit effort="100" lower="0" upper="3.1416" velocity="100"/>
+  </joint>
+  <link name="r_finger_2_link_0">
+    <visual>
+      <origin rpy="0 0 0" xyz="0.020 0 0"/>
+      <geometry>
+        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/robotiq_link_0.dae"/>
+      </geometry>
+      <material name="green">
+        <color rgba="0 1 0 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="r_finger_2_link_1">
+    <visual>
+      <origin rpy="0 0 -0.52" xyz="0.050 -.028 0"/>
+      <geometry>
+        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/robotiq_link_1.dae"/>
+      </geometry>
+      <material name="green"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 -0.5" xyz="0.000 -0.015 0"/>
+      <geometry>
+        <box size="0.1 0.045 0.035"/>
+      </geometry>
+      <material name="yellow">
+        <color rgba="0 1 1 1"/>
+      </material>
+    </collision>
+  </link>
+  <link name="r_finger_2_link_2">
+    <!--
+      NOTE: This link has an offset in the z-direction based on the models that were
+      used to generate grap-it poses.  With the offset below the model appears visually
+      correct, but it is suspect.
+      -->
+    <visual>
+      <origin rpy="0 0 0" xyz="0.039 0 0.0075"/>
+      <geometry>
+        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/robotiq_link_2.dae"/>
+      </geometry>
+      <material name="green"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.02 -0.01 0"/>
+      <geometry>
+        <box size="0.05 0.04 0.035"/>
+      </geometry>
+      <material name="yellow"/>
+    </collision>
+  </link>
+  <link name="r_finger_2_link_3">
+    <visual>
+      <origin rpy="0 0 0.52" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/robotiq_link_3.dae"/>
+      </geometry>
+      <material name="green"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0.5" xyz="0.018 0.003 0"/>
+      <geometry>
+        <box size="0.045 0.03 0.035"/>
+      </geometry>
+      <material name="yellow"/>
+    </collision>
+  </link>
+  <joint name="r_finger_2_joint_1" type="fixed">
+    <parent link="r_finger_2_link_0"/>
+    <child link="r_finger_2_link_1"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="0.020 0 0"/>
+    <limit effort="100" lower="0" upper="3.1416" velocity="100"/>
+  </joint>
+  <joint name="r_finger_2_joint_2" type="fixed">
+    <parent link="r_finger_2_link_1"/>
+    <child link="r_finger_2_link_2"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 -0.52" xyz="0.050 -.028 0"/>
+    <limit effort="100" lower="0" upper="3.1416" velocity="100"/>
+  </joint>
+  <joint name="r_finger_2_joint_3" type="fixed">
+    <parent link="r_finger_2_link_2"/>
+    <child link="r_finger_2_link_3"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="0.039 0 0"/>
+    <limit effort="100" lower="0" upper="3.1416" velocity="100"/>
+  </joint>
+  <link name="r_finger_middle_link_0">
+    <visual>
+      <origin rpy="0 0 0" xyz="0.020 0 0"/>
+      <geometry>
+        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/robotiq_link_0.dae"/>
+      </geometry>
+      <material name="green">
+        <color rgba="0 1 0 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="r_finger_middle_link_1">
+    <visual>
+      <origin rpy="0 0 -0.52" xyz="0.050 -.028 0"/>
+      <geometry>
+        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/robotiq_link_1.dae"/>
+      </geometry>
+      <material name="green"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 -0.5" xyz="0.000 -0.015 0"/>
+      <geometry>
+        <box size="0.1 0.045 0.035"/>
+      </geometry>
+      <material name="yellow">
+        <color rgba="0 1 1 1"/>
+      </material>
+    </collision>
+  </link>
+  <link name="r_finger_middle_link_2">
+    <!--
+      NOTE: This link has an offset in the z-direction based on the models that were
+      used to generate grap-it poses.  With the offset below the model appears visually
+      correct, but it is suspect.
+      -->
+    <visual>
+      <origin rpy="0 0 0" xyz="0.039 0 0.0075"/>
+      <geometry>
+        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/robotiq_link_2.dae"/>
+      </geometry>
+      <material name="green"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.02 -0.01 0"/>
+      <geometry>
+        <box size="0.05 0.04 0.035"/>
+      </geometry>
+      <material name="yellow"/>
+    </collision>
+  </link>
+  <link name="r_finger_middle_link_3">
+    <visual>
+      <origin rpy="0 0 0.52" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/robotiq_link_3.dae"/>
+      </geometry>
+      <material name="green"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0.5" xyz="0.018 0.003 0"/>
+      <geometry>
+        <box size="0.045 0.03 0.035"/>
+      </geometry>
+      <material name="yellow"/>
+    </collision>
+  </link>
+  <joint name="r_finger_middle_joint_1" type="fixed">
+    <parent link="r_finger_middle_link_0"/>
+    <child link="r_finger_middle_link_1"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="0.020 0 0"/>
+    <limit effort="100" lower="0" upper="3.1416" velocity="100"/>
+  </joint>
+  <joint name="r_finger_middle_joint_2" type="fixed">
+    <parent link="r_finger_middle_link_1"/>
+    <child link="r_finger_middle_link_2"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 -0.52" xyz="0.050 -.028 0"/>
+    <limit effort="100" lower="0" upper="3.1416" velocity="100"/>
+  </joint>
+  <joint name="r_finger_middle_joint_3" type="fixed">
+    <parent link="r_finger_middle_link_2"/>
+    <child link="r_finger_middle_link_3"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="0.039 0 0"/>
+    <limit effort="100" lower="0" upper="3.1416" velocity="100"/>
+  </joint>
+  <link name="r_palm">
+    <visual>
+      <geometry>
+        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/robotiq_palm.dae"/>
+      </geometry>
+      <material name="green">
+        <color rgba="0 1 0 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 -0.033 0"/>
+      <geometry>
+        <box size="0.13 0.17 0.13"/>
+      </geometry>
+      <material name="yellow">
+        <color rgba="0 1 1 1"/>
+      </material>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.085 -0.02 0"/>
+      <geometry>
+        <box size="0.08 0.08 0.07"/>
+      </geometry>
+      <material name="yellow">
+        <color rgba="0 1 1 1"/>
+      </material>
+    </collision>
+  </link>
+  <link name="r_wrist">
+    <visual>
+      <geometry>
+        <cylinder length="0.02614" radius="0.04"/>
+      </geometry>
+      <material name="green">
+        <color rgba="0 1 0 1"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <cylinder length="0.02614" radius="0.04"/>
+      </geometry>
+      <material name="yellow">
+        <color rgba="0 1 1 1"/>
+      </material>
+    </collision>
+  </link>
+  <joint name="r_palm_finger_1_joint" type="fixed">
+    <parent link="r_palm"/>
+    <child link="r_finger_1_link_0"/>
+    <axis xyz="0 1 0"/>
+    <origin rpy="0 3.1416 -1.57" xyz="-0.0455 0.0214 0.036"/>
+    <limit effort="100" lower="-0.16" upper="0.25" velocity="100"/>
+  </joint>
+  <joint name="r_wrist_joint" type="fixed">
+    <parent link="r_palm"/>
+    <child link="r_wrist"/>
+    <axis xyz="0 1 0"/>
+    <origin rpy="1.57 0 0" xyz="0 -0.0575 0"/>
+  </joint>
+  <joint name="r_palm_finger_2_joint" type="fixed">
+    <parent link="r_palm"/>
+    <child link="r_finger_2_link_0"/>
+    <axis xyz="0 1 0"/>
+    <origin rpy="0 3.1416 -1.57" xyz="-0.0455 0.0214 -0.036"/>
+    <limit effort="100" lower="-0.25" upper="0.16" velocity="100"/>
+  </joint>
+  <joint name="r_palm_finger_middle_joint" type="fixed">
+    <parent link="r_palm"/>
+    <child link="r_finger_middle_link_0"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 1.57" xyz="0.0455 0.0214 0"/>
+  </joint>
+  <joint name="r_hand_joint" type="fixed">
+    <origin rpy="3.14 0 0" xyz="0 -0.212725 0.013"/>
+    <parent link="r_hand"/>
+    <child link="r_palm"/>
+  </joint>
+  <link name="l_finger_1_link_0">
+    <visual>
+      <origin rpy="0 0 0" xyz="0.020 0 0"/>
+      <geometry>
+        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/robotiq_link_0.dae"/>
+      </geometry>
+      <material name="green">
+        <color rgba="0 1 0 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="l_finger_1_link_1">
+    <visual>
+      <origin rpy="0 0 -0.52" xyz="0.050 -.028 0"/>
+      <geometry>
+        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/robotiq_link_1.dae"/>
+      </geometry>
+      <material name="green"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 -0.5" xyz="0.000 -0.015 0"/>
+      <geometry>
+        <box size="0.1 0.045 0.035"/>
+      </geometry>
+      <material name="yellow">
+        <color rgba="0 1 1 1"/>
+      </material>
+    </collision>
+  </link>
+  <link name="l_finger_1_link_2">
+    <!--
+      NOTE: This link has an offset in the z-direction based on the models that were
+      used to generate grap-it poses.  With the offset below the model appears visually
+      correct, but it is suspect.
+      -->
+    <visual>
+      <origin rpy="0 0 0" xyz="0.039 0 0.0075"/>
+      <geometry>
+        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/robotiq_link_2.dae"/>
+      </geometry>
+      <material name="green"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.02 -0.01 0"/>
+      <geometry>
+        <box size="0.05 0.04 0.035"/>
+      </geometry>
+      <material name="yellow"/>
+    </collision>
+  </link>
+  <link name="l_finger_1_link_3">
+    <visual>
+      <origin rpy="0 0 0.52" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/robotiq_link_3.dae"/>
+      </geometry>
+      <material name="green"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0.5" xyz="0.018 0.003 0"/>
+      <geometry>
+        <box size="0.045 0.03 0.035"/>
+      </geometry>
+      <material name="yellow"/>
+    </collision>
+  </link>
+  <joint name="l_finger_1_joint_1" type="fixed">
+    <parent link="l_finger_1_link_0"/>
+    <child link="l_finger_1_link_1"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="0.020 0 0"/>
+    <limit effort="100" lower="0" upper="3.1416" velocity="100"/>
+  </joint>
+  <joint name="l_finger_1_joint_2" type="fixed">
+    <parent link="l_finger_1_link_1"/>
+    <child link="l_finger_1_link_2"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 -0.52" xyz="0.050 -.028 0"/>
+    <limit effort="100" lower="0" upper="3.1416" velocity="100"/>
+  </joint>
+  <joint name="l_finger_1_joint_3" type="fixed">
+    <parent link="l_finger_1_link_2"/>
+    <child link="l_finger_1_link_3"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="0.039 0 0"/>
+    <limit effort="100" lower="0" upper="3.1416" velocity="100"/>
+  </joint>
+  <link name="l_finger_2_link_0">
+    <visual>
+      <origin rpy="0 0 0" xyz="0.020 0 0"/>
+      <geometry>
+        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/robotiq_link_0.dae"/>
+      </geometry>
+      <material name="green">
+        <color rgba="0 1 0 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="l_finger_2_link_1">
+    <visual>
+      <origin rpy="0 0 -0.52" xyz="0.050 -.028 0"/>
+      <geometry>
+        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/robotiq_link_1.dae"/>
+      </geometry>
+      <material name="green"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 -0.5" xyz="0.000 -0.015 0"/>
+      <geometry>
+        <box size="0.1 0.045 0.035"/>
+      </geometry>
+      <material name="yellow">
+        <color rgba="0 1 1 1"/>
+      </material>
+    </collision>
+  </link>
+  <link name="l_finger_2_link_2">
+    <!--
+      NOTE: This link has an offset in the z-direction based on the models that were
+      used to generate grap-it poses.  With the offset below the model appears visually
+      correct, but it is suspect.
+      -->
+    <visual>
+      <origin rpy="0 0 0" xyz="0.039 0 0.0075"/>
+      <geometry>
+        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/robotiq_link_2.dae"/>
+      </geometry>
+      <material name="green"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.02 -0.01 0"/>
+      <geometry>
+        <box size="0.05 0.04 0.035"/>
+      </geometry>
+      <material name="yellow"/>
+    </collision>
+  </link>
+  <link name="l_finger_2_link_3">
+    <visual>
+      <origin rpy="0 0 0.52" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/robotiq_link_3.dae"/>
+      </geometry>
+      <material name="green"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0.5" xyz="0.018 0.003 0"/>
+      <geometry>
+        <box size="0.045 0.03 0.035"/>
+      </geometry>
+      <material name="yellow"/>
+    </collision>
+  </link>
+  <joint name="l_finger_2_joint_1" type="fixed">
+    <parent link="l_finger_2_link_0"/>
+    <child link="l_finger_2_link_1"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="0.020 0 0"/>
+    <limit effort="100" lower="0" upper="3.1416" velocity="100"/>
+  </joint>
+  <joint name="l_finger_2_joint_2" type="fixed">
+    <parent link="l_finger_2_link_1"/>
+    <child link="l_finger_2_link_2"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 -0.52" xyz="0.050 -.028 0"/>
+    <limit effort="100" lower="0" upper="3.1416" velocity="100"/>
+  </joint>
+  <joint name="l_finger_2_joint_3" type="fixed">
+    <parent link="l_finger_2_link_2"/>
+    <child link="l_finger_2_link_3"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="0.039 0 0"/>
+    <limit effort="100" lower="0" upper="3.1416" velocity="100"/>
+  </joint>
+  <link name="l_finger_middle_link_0">
+    <visual>
+      <origin rpy="0 0 0" xyz="0.020 0 0"/>
+      <geometry>
+        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/robotiq_link_0.dae"/>
+      </geometry>
+      <material name="green">
+        <color rgba="0 1 0 1"/>
+      </material>
+    </visual>
+  </link>
+  <link name="l_finger_middle_link_1">
+    <visual>
+      <origin rpy="0 0 -0.52" xyz="0.050 -.028 0"/>
+      <geometry>
+        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/robotiq_link_1.dae"/>
+      </geometry>
+      <material name="green"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 -0.5" xyz="0.000 -0.015 0"/>
+      <geometry>
+        <box size="0.1 0.045 0.035"/>
+      </geometry>
+      <material name="yellow">
+        <color rgba="0 1 1 1"/>
+      </material>
+    </collision>
+  </link>
+  <link name="l_finger_middle_link_2">
+    <!--
+      NOTE: This link has an offset in the z-direction based on the models that were
+      used to generate grap-it poses.  With the offset below the model appears visually
+      correct, but it is suspect.
+      -->
+    <visual>
+      <origin rpy="0 0 0" xyz="0.039 0 0.0075"/>
+      <geometry>
+        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/robotiq_link_2.dae"/>
+      </geometry>
+      <material name="green"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.02 -0.01 0"/>
+      <geometry>
+        <box size="0.05 0.04 0.035"/>
+      </geometry>
+      <material name="yellow"/>
+    </collision>
+  </link>
+  <link name="l_finger_middle_link_3">
+    <visual>
+      <origin rpy="0 0 0.52" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/robotiq_link_3.dae"/>
+      </geometry>
+      <material name="green"/>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0.5" xyz="0.018 0.003 0"/>
+      <geometry>
+        <box size="0.045 0.03 0.035"/>
+      </geometry>
+      <material name="yellow"/>
+    </collision>
+  </link>
+  <joint name="l_finger_middle_joint_1" type="fixed">
+    <parent link="l_finger_middle_link_0"/>
+    <child link="l_finger_middle_link_1"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="0.020 0 0"/>
+    <limit effort="100" lower="0" upper="3.1416" velocity="100"/>
+  </joint>
+  <joint name="l_finger_middle_joint_2" type="fixed">
+    <parent link="l_finger_middle_link_1"/>
+    <child link="l_finger_middle_link_2"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 -0.52" xyz="0.050 -.028 0"/>
+    <limit effort="100" lower="0" upper="3.1416" velocity="100"/>
+  </joint>
+  <joint name="l_finger_middle_joint_3" type="fixed">
+    <parent link="l_finger_middle_link_2"/>
+    <child link="l_finger_middle_link_3"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 0" xyz="0.039 0 0"/>
+    <limit effort="100" lower="0" upper="3.1416" velocity="100"/>
+  </joint>
+  <link name="l_palm">
+    <visual>
+      <geometry>
+        <mesh filename="package://robotiq_s_model_visualization/meshes/s-model_articulated/visual/robotiq_palm.dae"/>
+      </geometry>
+      <material name="green">
+        <color rgba="0 1 0 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 -0.033 0"/>
+      <geometry>
+        <box size="0.13 0.17 0.13"/>
+      </geometry>
+      <material name="yellow">
+        <color rgba="0 1 1 1"/>
+      </material>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="-0.085 -0.02 0"/>
+      <geometry>
+        <box size="0.08 0.08 0.07"/>
+      </geometry>
+      <material name="yellow">
+        <color rgba="0 1 1 1"/>
+      </material>
+    </collision>
+  </link>
+  <link name="l_wrist">
+    <visual>
+      <geometry>
+        <cylinder length="0.02614" radius="0.04"/>
+      </geometry>
+      <material name="green">
+        <color rgba="0 1 0 1"/>
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <cylinder length="0.02614" radius="0.04"/>
+      </geometry>
+      <material name="yellow">
+        <color rgba="0 1 1 1"/>
+      </material>
+    </collision>
+  </link>
+  <joint name="l_palm_finger_1_joint" type="fixed">
+    <parent link="l_palm"/>
+    <child link="l_finger_1_link_0"/>
+    <axis xyz="0 1 0"/>
+    <origin rpy="0 3.1416 -1.57" xyz="-0.0455 0.0214 0.036"/>
+    <limit effort="100" lower="-0.16" upper="0.25" velocity="100"/>
+  </joint>
+  <joint name="l_wrist_joint" type="fixed">
+    <parent link="l_palm"/>
+    <child link="l_wrist"/>
+    <axis xyz="0 1 0"/>
+    <origin rpy="1.57 0 0" xyz="0 -0.0575 0"/>
+  </joint>
+  <joint name="l_palm_finger_2_joint" type="fixed">
+    <parent link="l_palm"/>
+    <child link="l_finger_2_link_0"/>
+    <axis xyz="0 1 0"/>
+    <origin rpy="0 3.1416 -1.57" xyz="-0.0455 0.0214 -0.036"/>
+    <limit effort="100" lower="-0.25" upper="0.16" velocity="100"/>
+  </joint>
+  <joint name="l_palm_finger_middle_joint" type="fixed">
+    <parent link="l_palm"/>
+    <child link="l_finger_middle_link_0"/>
+    <axis xyz="0 0 1"/>
+    <origin rpy="0 0 1.57" xyz="0.0455 0.0214 0"/>
+  </joint>
+  <joint name="l_hand_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0.212725 0.013"/>
+    <parent link="l_hand"/>
+    <child link="l_palm"/>
+  </joint>
+</robot>
+