<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from ModelGenerator/ihmc_robotics/robots/atlas_v5_robotiq_hands.urdf.xacro | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="atlas" xmlns:xacro="http://www.ros.org/wiki/xacro">
  <link name="l_clav">
    <inertial>
      <mass value="4.466"/>
      <origin rpy="0 0 0" xyz="0 0 -0.084"/>
      <inertia ixx="0.011" ixy="0" ixz="0" iyy="0.009" iyz="-0.004" izz="0.004"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://atlas_description/meshes_unplugged/l_clav.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 1.5707963267949 0" xyz="0 0.11 -0.248"/>
      <geometry>
        <cylinder length="0.1525" radius="0.0555"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="0.005 0.015 -0.08"/>
      <geometry>
        <cylinder length="0.26" radius="0.067"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="-1.05 0 0" xyz="0.004 0.064 -0.247"/>
      <geometry>
        <box size="0.1 0.07 0.072"/>
      </geometry>
    </collision>
  </link>
  <link name="l_foot">
    <inertial>
      <mass value="2.410"/>
      <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>
      <geometry>
        <mesh filename="package://atlas_description/meshes_unplugged/l_foot.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.023 0 -0.049119"/>
      <geometry>
        <box size="0.227 0.133887 0.05"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="0.1225 0 -0.0641"/>
      <geometry>
        <box size="0.1 0.08 0.02"/>
      </geometry>
    </collision>
  </link>
  <link name="l_hand">
    <inertial>
      <mass value="1.7839"/>
      <origin rpy="0 0 0" xyz="0.00016 0.08159 0.00002"/>
      <inertia ixx="0.000388" ixy="0" ixz="0" iyy="0.000477" iyz="0" izz="0.000379"/>
    </inertial>
  </link>
  <link name="l_larm">
    <inertial>
      <mass value="3.248"/>
      <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>
      <geometry>
        <mesh filename="package://atlas_description/meshes_unplugged/l_larm.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="1.5707963267949 0 0" xyz="0.00 0.075 -0.01"/>
      <geometry>
        <cylinder length="0.04" radius="0.051"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 1.5707963267949 0" xyz="-0.012 0 0.0025"/>
      <geometry>
        <cylinder length="0.147" radius="0.045"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="-0.07 0.077 -0.015"/>
      <geometry>
        <box size="0.025 0.155 0.07"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="0.05 0.067 -0.009"/>
      <geometry>
        <box size="0.025 0.06 0.06"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="-0.032 0.071 -0.06"/>
      <geometry>
        <box size="0.04 0.115 0.05"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="0.03 0.01 -0.05"/>
      <geometry>
        <box size="0.045 0.055 0.023"/>
      </geometry>
    </collision>
  </link>
  <link name="l_lfarm">
    <inertial>
      <mass value="1.6"/>
      <origin rpy="0 0 0" xyz="0.00017 0.02515 0.00163"/>
      <inertia ixx="0.000764" ixy="0" ixz="0" iyy="0.000429" iyz="0" izz="0.000825"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://atlas_description/meshes_unplugged/l_hand.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="-1.5707963267949 0 0" xyz="0.00 0.104 0.00"/>
      <geometry>
        <cylinder length="0.02" radius="0.031"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="-1.5707963267949 0 0" xyz="0.00 0.087 0.00"/>
      <geometry>
        <cylinder length="0.015" radius="0.045"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="-1.5707963267949 0 0" xyz="0.00 0.075 0.00"/>
      <geometry>
        <cylinder length="0.015" radius="0.052"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="-1.5707963267949 0 0" xyz="0.00 0.052 0.00"/>
      <geometry>
        <cylinder length="0.027" radius="0.043"/>
      </geometry>
    </collision>
  </link>
  <link name="l_lglut">
    <inertial>
      <mass value="0.898"/>
      <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>
      <geometry>
        <mesh filename="package://atlas_description/meshes_unplugged/l_lglut.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.015 0.026 -0.028"/>
      <geometry>
        <box size="0.125 0.05 0.08"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="1.5707963267949 0 0" xyz="0.05 0.027488 -0.067"/>
      <geometry>
        <cylinder length="0.060306" radius="0.02009687"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 1.5707963267949 0" xyz="0.0115 0 0"/>
      <geometry>
        <cylinder length="0.144" radius="0.02"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="0.01 0.02 0.01"/>
      <geometry>
        <box size="0.0975 0.069 0.103"/>
      </geometry>
    </collision>
  </link>
  <link name="l_lleg">
    <inertial>
      <mass value="4.515"/>
      <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>
      <geometry>
        <mesh filename="package://atlas_description/meshes_unplugged/l_lleg.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0.10 0" xyz="0.02 0.00295 -0.182"/>
      <geometry>
        <cylinder length="0.40" radius="0.07"/>
      </geometry>
    </collision>
  </link>
  <link name="l_scap">
    <inertial>
      <mass value="3.899"/>
      <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>
      <geometry>
        <mesh filename="package://atlas_description/meshes_unplugged/l_scap.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="-1.5707963267949 0 0" xyz="-0.005 0.115 -0.014"/>
      <geometry>
        <cylinder length="0.105" radius="0.057"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="-0.005 0.075 -0.015"/>
      <geometry>
        <box size="0.16 0.17 0.08"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="-2.25 0 0" xyz="0.076 0.0615 0.0128"/>
      <geometry>
        <box size="0.02 0.029 0.018"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="-1 0 0" xyz="-0.075 -0.015 0.019"/>
      <geometry>
        <box size="0.028 0.090 0.05"/>
      </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>
      <geometry>
        <mesh filename="package://atlas_description/meshes_unplugged/l_talus.dae"/>
      </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="4.386"/>
      <origin rpy="0 0 0" xyz="0.0 -0.065 0.0"/>
      <inertia ixx="0.00656" ixy="0" ixz="0" iyy="0.00358" iyz="0" izz="0.00656"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://atlas_description/meshes_unplugged/l_uarm.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="-1.5708 0 0" xyz="0.0 0.035 -0.005"/>
      <geometry>
        <cylinder length="0.11" radius="0.065"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 1.5707963267949 0" xyz="-0.01 0.043 0.002"/>
      <geometry>
        <box size="0.05 0.07 0.15"/>
      </geometry>
    </collision>
  </link>
  <link name="l_ufarm">
    <inertial>
      <mass value="2.4798"/>
      <origin rpy="0 0 0" xyz="0.00015 -0.08296 0.00037"/>
      <inertia ixx="0.012731" ixy="0" ixz="0" iyy="0.002857" iyz="0" izz="0.011948"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://atlas_description/meshes_unplugged/l_farm.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="1.57 0 0" xyz="0 -0.15 0"/>
      <geometry>
        <cylinder length="0.1" radius="0.053"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 1.5707963267949 0" xyz="-0.001 -0.07 0.0"/>
      <geometry>
        <box size="0.08 0.09 0.11"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 1.57 0" xyz="0 -0.005 0"/>
      <geometry>
        <cylinder length="0.125" radius="0.040"/>
      </geometry>
    </collision>
  </link>
  <link name="l_uglut">
    <inertial>
      <mass value="1.959"/>
      <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>
      <geometry>
        <mesh filename="package://atlas_description/meshes_unplugged/l_uglut.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0.52539 0 0" xyz="0.01 0.005 0"/>
      <geometry>
        <box size="0.05 0.1 0.1"/>
      </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="8.204"/>
      <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>
      <geometry>
        <mesh filename="package://atlas_description/meshes_unplugged/l_uleg.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="-0.0199 0.00585 -0.122"/>
      <geometry>
        <cylinder length="0.15" radius="0.09"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 -0.3 0" xyz="-0.032 -0.005 -0.224"/>
      <geometry>
        <cylinder length="0.15" radius="0.07"/>
      </geometry>
    </collision>
    <!-- to cover the logo shield -->
    <collision>
      <origin rpy="-0.15 0.13 0" xyz="0.003 0.015 -0.254"/>
      <geometry>
        <cylinder length="0.15" radius="0.05"/>
      </geometry>
    </collision>
    <!-- inside strut -->
    <collision>
      <origin rpy="0.0 0.0 0" xyz="-0.007 -0.053 -0.052"/>
      <geometry>
        <box size="0.07 0.038 0.15"/>
      </geometry>
    </collision>
    <!-- outside strut -->
    <collision>
      <origin rpy="0.0 0.0 0" xyz="-0.017 0.0595 -0.049"/>
      <geometry>
        <box size="0.07 0.038 0.14"/>
      </geometry>
    </collision>
  </link>
  <link name="ltorso">
    <inertial>
      <mass value="2.270"/>
      <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>
      <geometry>
        <mesh filename="package://atlas_description/meshes_unplugged/ltorso.dae"/>
      </geometry>
    </visual>
  </link>
  <link name="mtorso">
    <inertial>
      <mass value="0.799"/>
      <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>
      <geometry>
        <mesh filename="package://atlas_description/meshes_unplugged/mtorso.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.0 0 -0.02"/>
      <geometry>
        <cylinder length="0.15" radius="0.02"/>
      </geometry>
    </collision>
  </link>
  <link name="pelvis">
    <inertial>
      <mass value="9.509"/>
      <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>
      <geometry>
        <mesh filename="package://atlas_description/meshes_unplugged/pelvis.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="1.5707963267949 0 0" xyz="0.046 0.0 0.01"/>
      <geometry>
        <cylinder length="0.06" radius="0.11"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="1.5707963267949 0 0" xyz="-0.03 0.0 0.01"/>
      <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.02"/>
      <geometry>
        <box size="0.1 0.15 0.2"/>
      </geometry>
    </collision>
  </link>
  <link name="r_clav">
    <inertial>
      <mass value="4.466"/>
      <origin rpy="0 0 0" xyz="0 0 -0.084"/>
      <inertia ixx="0.011" ixy="0" ixz="0" iyy="0.009" iyz="0.004" izz="0.004"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://atlas_description/meshes_unplugged/r_clav.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 1.5707963267949 0" xyz="0 -0.11 -0.248"/>
      <geometry>
        <cylinder length="0.1525" radius="0.0555"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="0.005 -0.015 -0.08"/>
      <geometry>
        <cylinder length="0.26" radius="0.067"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="1.05 0 0" xyz="0.004 -0.064 -0.247"/>
      <geometry>
        <box size="0.1 0.07 0.072"/>
      </geometry>
    </collision>
  </link>
  <link name="r_foot">
    <inertial>
      <mass value="2.41"/>
      <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>
      <geometry>
        <mesh filename="package://atlas_description/meshes_unplugged/r_foot.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.023 0 -0.049119"/>
      <geometry>
        <box size="0.227 0.133887 0.05"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="0.1225 0 -0.0641"/>
      <geometry>
        <box size="0.1 0.08 0.02"/>
      </geometry>
    </collision>
  </link>
  <link name="r_hand">
    <inertial>
      <mass value="1.7839"/>
      <origin rpy="0 0 0" xyz="0.00016 -0.08159 0.00002"/>
      <inertia ixx="0.000388" ixy="0" ixz="0" iyy="0.000477" iyz="0" izz="0.000379"/>
    </inertial>
  </link>
  <link name="r_larm">
    <inertial>
      <mass value="3.248"/>
      <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>
      <geometry>
        <mesh filename="package://atlas_description/meshes_unplugged/r_larm.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="-1.5707963267949 0 0" xyz="0.00 -0.075 -0.01"/>
      <geometry>
        <cylinder length="0.04" radius="0.051"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 1.5707963267949 0" xyz="-0.012 0 0.0025"/>
      <geometry>
        <cylinder length="0.147" radius="0.045"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="-0.07 -0.077 -0.015"/>
      <geometry>
        <box size="0.025 0.155 0.07"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="0.05 -0.067 -0.009"/>
      <geometry>
        <box size="0.025 0.06 0.06"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="-0.032 -0.071 -0.06"/>
      <geometry>
        <box size="0.04 0.115 0.05"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="0.03 0.01 -0.05"/>
      <geometry>
        <box size="0.045 0.055 0.023"/>
      </geometry>
    </collision>
  </link>
  <link name="r_lfarm">
    <inertial>
      <mass value="1.6"/>
      <origin rpy="0 0 0" xyz="0.00017 -0.02515 0.00163"/>
      <inertia ixx="0.000764" ixy="0" ixz="0" iyy="0.000429" iyz="0" izz="0.000825"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://atlas_description/meshes_unplugged/r_hand.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="1.5707963267949 0 0" xyz="0.00 -0.104 0.00"/>
      <geometry>
        <cylinder length="0.02" radius="0.031"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="1.5707963267949 0 0" xyz="0.00 -0.087 0.00"/>
      <geometry>
        <cylinder length="0.015" radius="0.045"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="1.5707963267949 0 0" xyz="0.00 -0.075 0.00"/>
      <geometry>
        <cylinder length="0.015" radius="0.052"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="1.5707963267949 0 0" xyz="0.00 -0.052 0.00"/>
      <geometry>
        <cylinder length="0.027" radius="0.043"/>
      </geometry>
    </collision>
  </link>
  <link name="r_lglut">
    <inertial>
      <mass value="0.898"/>
      <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>
      <geometry>
        <mesh filename="package://atlas_description/meshes_unplugged/r_lglut.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.015 -0.026 -0.028"/>
      <geometry>
        <box size="0.125 0.05 0.08"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="1.5707963267949 0 0" xyz="0.05 -0.027488 -0.067"/>
      <geometry>
        <cylinder length="0.060306" radius="0.02009687"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 1.5707963267949 0" xyz="0.0105 0 0"/>
      <geometry>
        <cylinder length="0.142" radius="0.02"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="0.01 -0.02 0.01"/>
      <geometry>
        <box size="0.0975 0.069 0.103"/>
      </geometry>
    </collision>
  </link>
  <link name="r_lleg">
    <inertial>
      <mass value="4.515"/>
      <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>
      <geometry>
        <mesh filename="package://atlas_description/meshes_unplugged/r_lleg.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0.10 0" xyz="0.02 -0.00295 -0.182"/>
      <geometry>
        <cylinder length="0.40" radius="0.07"/>
      </geometry>
    </collision>
  </link>
  <link name="r_scap">
    <inertial>
      <mass value="3.899"/>
      <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>
      <geometry>
        <mesh filename="package://atlas_description/meshes_unplugged/r_scap.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="1.5707963267949 0 0" xyz="-0.005 -0.115 -0.014"/>
      <geometry>
        <cylinder length="0.105" radius="0.057"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="-0.005 -0.075 -0.015"/>
      <geometry>
        <box size="0.16 0.17 0.08"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="2.25 0 0" xyz="0.076 -0.0615 0.0128"/>
      <geometry>
        <box size="0.02 0.029 0.018"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="1 0 0" xyz="-0.075 0.015 0.019"/>
      <geometry>
        <box size="0.028 0.090 0.05"/>
      </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>
      <geometry>
        <mesh filename="package://atlas_description/meshes_unplugged/r_talus.dae"/>
      </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="4.386"/>
      <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>
      <geometry>
        <mesh filename="package://atlas_description/meshes_unplugged/r_uarm.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="1.5708 0 0" xyz="0.0 -0.035 -0.005"/>
      <geometry>
        <cylinder length="0.11" radius="0.065"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 1.5707963267949 0" xyz="-0.01 -0.043 0.002"/>
      <geometry>
        <box size="0.05 0.07 0.15"/>
      </geometry>
    </collision>
  </link>
  <link name="r_ufarm">
    <inertial>
      <mass value="2.4798"/>
      <origin rpy="0 0 0" xyz="0.00015 0.08296 0.00037"/>
      <inertia ixx="0.012731" ixy="0" ixz="0" iyy="0.002857" iyz="0" izz="0.011948"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://atlas_description/meshes_unplugged/r_farm.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="1.57 0 0" xyz="0 0.15 0"/>
      <geometry>
        <cylinder length="0.1" radius="0.053"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 1.5707963267949 0" xyz="-0.001 0.07 0.0"/>
      <geometry>
        <box size="0.08 0.09 0.11"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 1.57 0" xyz="0 0.005 0"/>
      <geometry>
        <cylinder length="0.125" radius="0.040"/>
      </geometry>
    </collision>
  </link>
  <link name="r_uglut">
    <inertial>
      <mass value="1.959"/>
      <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>
      <geometry>
        <mesh filename="package://atlas_description/meshes_unplugged/r_uglut.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="-0.52539 0 0" xyz="0.01 -0.005 0"/>
      <geometry>
        <box size="0.05 0.1 0.1"/>
      </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="8.204"/>
      <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>
      <geometry>
        <mesh filename="package://atlas_description/meshes_unplugged/r_uleg.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="-0.0199 -0.00585 -0.122"/>
      <geometry>
        <cylinder length="0.15" radius="0.09"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 -0.3 0" xyz="-0.032 0.005 -0.224"/>
      <geometry>
        <cylinder length="0.15" radius="0.07"/>
      </geometry>
    </collision>
    <!-- to cover the logo shield -->
    <collision>
      <origin rpy="0.15 0.13 0" xyz="0.003 -0.015 -0.254"/>
      <geometry>
        <cylinder length="0.15" radius="0.05"/>
      </geometry>
    </collision>
    <!-- inside strut -->
    <collision>
      <origin rpy="0.0 0.0 0" xyz="-0.007 0.053 -0.052"/>
      <geometry>
        <box size="0.07 0.038 0.15"/>
      </geometry>
    </collision>
    <!-- outside strut -->
    <collision>
      <origin rpy="0.0 0.0 0" xyz="-0.017 -0.0595 -0.049"/>
      <geometry>
        <box size="0.07 0.038 0.14"/>
      </geometry>
    </collision>
  </link>
  <link name="utorso">
    <inertial>
      <mass value="84.409"/>
      <origin rpy="0 0 0" xyz="-0.035 0.0023 0.3157"/>
      <inertia ixx="1.577" ixy="-0.032" ixz="0.102" iyy="1.602" iyz="0.047" izz="0.565"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="package://atlas_description/meshes_unplugged/utorso.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="-0.102 0 0.25"/>
      <geometry>
        <box size="0.4 0.35 0.5"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 1.5707 0" xyz="0.2 0 0.46"/>
      <geometry>
        <box size="0.13 0.50 0.10"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 1.7 0" xyz="0.2 0 0.285"/>
      <geometry>
        <box size="0.2 0.23 0.10"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 2.0 0" xyz="0.15 0 0.10"/>
      <geometry>
        <box size="0.2 0.18 0.10"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="-0.035 0 0.6265"/>
      <geometry>
        <box size="0.506306 0.341719 0.12"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="1.5707 0 0" xyz="0.208 0 0.8215"/>
      <geometry>
        <cylinder length="0.13" radius="0.008"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="-0.38535 0 0" xyz="0.208 -0.125 0.745"/>
      <geometry>
        <cylinder length="0.15" radius="0.008"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0.38535 0 0" xyz="0.208 0.125 0.745"/>
      <geometry>
        <cylinder length="0.15" radius="0.008"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="-0.38535 0 0" xyz="0.063 -0.125 0.745"/>
      <geometry>
        <cylinder length="0.15" radius="0.008"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0.38535 0 0" xyz="0.063 0.125 0.745"/>
      <geometry>
        <cylinder length="0.15" radius="0.008"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="1.5707 0 1.5707" xyz="0.14 0.06 0.8215"/>
      <geometry>
        <cylinder length="0.15" radius="0.008"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="1.5707 0 1.5707" xyz="0.14 -0.06 0.8215"/>
      <geometry>
        <cylinder length="0.15" radius="0.008"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 -0.65 0" xyz="-0.029 0 0.731"/>
      <geometry>
        <box size="0.2 0.25 0.01"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="1.5707 0 0" xyz="0.063 0 0.8215"/>
      <geometry>
        <cylinder length="0.13" radius="0.008"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="1.5707 0 0" xyz="0.261 0.156 0.6215"/>
      <geometry>
        <cylinder length="0.016" radius="0.055"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="1.5707 0 0" xyz="0.261 -0.156 0.6215"/>
      <geometry>
        <cylinder length="0.016" radius="0.055"/>
      </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="300" lower="-0.523599" upper="0.523599" velocity="12"/>
    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.5236" soft_upper_limit="10.5236"/>
  </joint>
  <joint name="back_bky" type="revolute">
    <origin rpy="0 0 0" xyz="0 0 0.162"/>
    <axis xyz="0 1 0"/>
    <parent link="ltorso"/>
    <child link="mtorso"/>
    <dynamics damping="0.1" friction="0"/>
    <limit effort="445" lower="-0.219388" upper="0.538783" velocity="9"/>
    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.2194" soft_upper_limit="10.5388"/>
  </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="106" lower="-0.663225" upper="0.663225" 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.0092"/>
    <axis xyz="1 0 0"/>
    <parent link="l_uarm"/>
    <child link="l_larm"/>
    <dynamics damping="0.1" friction="0"/>
    <limit effort="112" lower="0" upper="2.35619" 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="63" lower="0" upper="3.14159" 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.11 -0.245"/>
    <axis xyz="1 0 0"/>
    <parent link="l_clav"/>
    <child link="l_scap"/>
    <dynamics damping="0.1" friction="0"/>
    <limit effort="99" lower="-1.5708" upper="1.5708" 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_shz" type="revolute">
    <origin rpy="0 0 0" xyz="0.1406 0.2256 0.4776"/>
    <axis xyz="0 0 1"/>
    <parent link="utorso"/>
    <child link="l_clav"/>
    <dynamics damping="0.1" friction="0"/>
    <limit effort="87" lower="-1.5708" upper="0.785398" 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 0"/>
    <axis xyz="1 0 0"/>
    <parent link="l_ufarm"/>
    <child link="l_lfarm"/>
    <dynamics damping="0.1" friction="0"/>
    <limit effort="25" lower="-1.7628" upper="1.7628" velocity="10"/>
    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11.7628" soft_upper_limit="11.7628"/>
  </joint>
  <joint name="l_arm_wry" type="revolute">
    <origin rpy="0 0 0" xyz="0 0.29955 -0.00921"/>
    <axis xyz="0 1 0"/>
    <parent link="l_larm"/>
    <child link="l_ufarm"/>
    <dynamics damping="0.1" friction="0"/>
    <limit effort="25" lower="-3.011" upper="3.011" velocity="10"/>
    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-13.011" soft_upper_limit="13.011"/>
  </joint>
  <joint name="l_arm_wry2" type="revolute">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <axis xyz="0 1 0"/>
    <parent link="l_lfarm"/>
    <child link="l_hand"/>
    <dynamics damping="0.1" friction="0"/>
    <limit effort="25" lower="-2.9671" upper="2.9671" velocity="10"/>
    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-12.9671" soft_upper_limit="12.9671"/>
  </joint>
  <joint name="l_leg_akx" type="revolute">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <axis xyz="1 0 0"/>
    <parent link="l_talus"/>
    <child link="l_foot"/>
    <dynamics damping="0.1" friction="0"/>
    <limit effort="360" lower="-0.8" upper="0.8" 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.422"/>
    <axis xyz="0 1 0"/>
    <parent link="l_lleg"/>
    <child link="l_talus"/>
    <dynamics damping="0.1" friction="0"/>
    <limit effort="740" lower="-1" upper="0.7" 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 0 0"/>
    <axis xyz="1 0 0"/>
    <parent link="l_uglut"/>
    <child link="l_lglut"/>
    <dynamics damping="0.1" friction="0"/>
    <limit effort="530" lower="-0.523599" upper="0.523599" 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.0225 -0.066"/>
    <axis xyz="0 1 0"/>
    <parent link="l_lglut"/>
    <child link="l_uleg"/>
    <dynamics damping="0.1" friction="0"/>
    <limit effort="840" lower="-1.61234" upper="0.65764" velocity="12"/>
    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11.6123" soft_upper_limit="10.6576"/>
  </joint>
  <joint name="l_leg_hpz" type="revolute">
    <origin rpy="0 0 0" xyz="0 0.089 0"/>
    <axis xyz="0 0 1"/>
    <parent link="pelvis"/>
    <child link="l_uglut"/>
    <dynamics damping="0.1" friction="0"/>
    <limit effort="275" lower="-0.174358" upper="0.786794" velocity="12"/>
    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.1744" soft_upper_limit="10.7868"/>
  </joint>
  <joint name="l_leg_kny" type="revolute">
    <origin rpy="0 0 0" xyz="-0.05 0 -0.374"/>
    <axis xyz="0 1 0"/>
    <parent link="l_uleg"/>
    <child link="l_lleg"/>
    <dynamics damping="0.1" friction="0"/>
    <limit effort="890" lower="0" upper="2.35637" velocity="12"/>
    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10" soft_upper_limit="12.3564"/>
  </joint>
  <joint name="neck_ry" type="revolute">
    <origin rpy="0 0 0" xyz="0.2546 0 0.6215"/>
    <axis xyz="0 1 0"/>
    <parent link="utorso"/>
    <child link="head"/>
    <dynamics damping="0.1" friction="0"/>
    <limit effort="25" lower="-0.602139" upper="1.14319" velocity="6.28"/>
    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.6021" soft_upper_limit="11.1432"/>
  </joint>
  <joint name="r_arm_elx" type="revolute">
    <origin rpy="0 0 0" xyz="0 -0.119 0.0092"/>
    <axis xyz="1 0 0"/>
    <parent link="r_uarm"/>
    <child link="r_larm"/>
    <dynamics damping="0.1" friction="0"/>
    <limit effort="112" lower="-2.35619" 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="63" lower="0" upper="3.14159" 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.11 -0.245"/>
    <axis xyz="1 0 0"/>
    <parent link="r_clav"/>
    <child link="r_scap"/>
    <dynamics damping="0.1" friction="0"/>
    <limit effort="99" lower="-1.5708" upper="1.5708" 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_shz" type="revolute">
    <origin rpy="0 0 0" xyz="0.1406 -0.2256 0.4776"/>
    <axis xyz="0 0 1"/>
    <parent link="utorso"/>
    <child link="r_clav"/>
    <dynamics damping="0.1" friction="0"/>
    <limit effort="87" lower="-0.785398" upper="1.5708" velocity="12"/>
    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.7854" soft_upper_limit="11.5708"/>
  </joint>
  <joint name="r_arm_wrx" type="revolute">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <axis xyz="1 0 0"/>
    <parent link="r_ufarm"/>
    <child link="r_lfarm"/>
    <dynamics damping="0.1" friction="0"/>
    <limit effort="25" lower="-1.7628" upper="1.7628" velocity="10"/>
    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11.7628" soft_upper_limit="11.7628"/>
  </joint>
  <joint name="r_arm_wry" type="revolute">
    <origin rpy="0 0 0" xyz="0 -0.29955 -0.00921"/>
    <axis xyz="0 1 0"/>
    <parent link="r_larm"/>
    <child link="r_ufarm"/>
    <dynamics damping="0.1" friction="0"/>
    <limit effort="25" lower="-3.011" upper="3.011" velocity="10"/>
    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-13.011" soft_upper_limit="13.011"/>
  </joint>
  <joint name="r_arm_wry2" type="revolute">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <axis xyz="0 1 0"/>
    <parent link="r_lfarm"/>
    <child link="r_hand"/>
    <dynamics damping="0.1" friction="0"/>
    <limit effort="25" lower="-2.9671" upper="2.9671" velocity="10"/>
    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-12.9671" soft_upper_limit="12.9671"/>
  </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="360" lower="-0.8" upper="0.8" 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="740" lower="-1" upper="0.7" 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="530" lower="-0.523599" upper="0.523599" 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.0225 -0.066"/>
    <axis xyz="0 1 0"/>
    <parent link="r_lglut"/>
    <child link="r_uleg"/>
    <dynamics damping="0.1" friction="0"/>
    <limit effort="840" lower="-1.61234" upper="0.65764" velocity="12"/>
    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-11.6123" soft_upper_limit="10.6576"/>
  </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="275" lower="-0.786794" upper="0.174358" velocity="12"/>
    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10.7868" soft_upper_limit="10.1744"/>
  </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="890" lower="0" upper="2.35637" velocity="12"/>
    <safety_controller k_position="100" k_velocity="100" soft_lower_limit="-10" soft_upper_limit="12.3564"/>
  </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"/>
    <!-- SA camera mounted above the multisense head -->
    <origin rpy="0 0 -1.309" xyz="0.155 -0.121 0.785"/>
  </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"/>
    <!-- SA camera mounted above the multisense head -->
    <origin rpy="0 0 1.309" xyz="0.155 0.121 0.785"/>
  </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"/>
  <!-- <xacro:include filename="$(find robotiq_hand_description)/urdf/robotiq_hand.urdf.xacro" /> -->
  <!--
  <xacro:robotiq_hand prefix="l_" parent="l_hand" reflect="-1">
    <origin rpy="0 -1.57079 0" xyz="-0.00179 0.168 0.0"/>
  </xacro:robotiq_hand>
  <xacro:robotiq_hand prefix="r_" parent="r_hand" reflect="1">
	<origin rpy="3.141592 -1.57079 0" xyz="-0.00179 -0.168 0.0" />
  </xacro:robotiq_hand>
  -->
  <gazebo>
    <!-- robot model offset -->
    <pose>0 0 0.93 0 0 0</pose>
  </gazebo>
  <gazebo>
    <plugin name="ihmc_gazebo" filename="libDRCSimIHMCPlugin.so"/>
    <plugin filename="libgazebo_ros_joint_pose_trajectory.so" name="joint_trajectory_plugin">
      <topicName>joint_trajectory</topicName>
      <updateRate>1000.0</updateRate>
    </plugin>
    <!--
    <plugin filename="libgazebo_ros_controller_manager.so" name="gazebo_ros_controller_manager">
      <alwaysOn>true</alwaysOn>
      <updateRate>1000.0</updateRate>
    </plugin>
    -->
  </gazebo>
  <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 0 0" xyz="0 0 0"/>
      <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 0 0"/>
  </joint>
  <gazebo reference="imu_link">
    <!-- this is expected to be reparented to pelvis with appropriate offset
         when imu_link is reduced by fixed joint reduction -->
    <!-- todo: this is working with gazebo 1.4, need to write a unit test -->
    <sensor name="imu_sensor" type="imu">
      <always_on>1</always_on>
      <update_rate>1000.0</update_rate>
      <imu>
        <noise>
          <type>gaussian</type>
          <!-- Noise parameters from Boston Dynamics
               (http://gazebosim.org/wiki/Sensor_noise):
                 rates (rad/s): mean=0, stddev=2e-4
                 accels (m/s/s): mean=0, stddev=1.7e-2
                 rate bias (rad/s): 5e-6 - 1e-5
                 accel bias (m/s/s): 1e-1
               Experimentally, simulation provide rates with noise of
               about 1e-3 rad/s and accels with noise of about 1e-1 m/s/s.
               So we don't expect to see the noise unless number of inner iterations
               are increased.

               We will add bias.  In this model, bias is sampled once for rates
               and once for accels at startup; the sign (negative or positive)
               of each bias is then switched with equal probability.  Thereafter,
               the biases are fixed additive offsets.  We choose
               bias means and stddevs to produce biases close to the provided
               data. -->
          <rate>
            <mean>0.0</mean>
            <stddev>2e-8</stddev>
            <bias_mean>0.00000075</bias_mean>
            <bias_stddev>0.00000008</bias_stddev>
          </rate>
          <accel>
            <mean>0.0</mean>
            <stddev>1.7e-8</stddev>
            <bias_mean>0.01</bias_mean>
            <bias_stddev>0.0001</bias_stddev>
          </accel>
        </noise>
      </imu>
    </sensor>
  </gazebo>
  <gazebo reference="l_arm_wrx">
    <provideFeedback>1</provideFeedback>
  </gazebo>
  <gazebo reference="r_arm_wrx">
    <provideFeedback>1</provideFeedback>
  </gazebo>
  <gazebo reference="l_leg_akx">
    <provideFeedback>1</provideFeedback>
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="r_leg_akx">
    <provideFeedback>1</provideFeedback>
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="r_foot">
    <!-- kp and kd for rubber -->
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>1.5</mu1>
    <mu2>1.5</mu2>
    <fdir1>1 0 0</fdir1>
    <maxVel>1.0</maxVel>
    <minDepth>0.00</minDepth>
  </gazebo>
  <gazebo reference="l_foot">
    <kp>1000000.0</kp>
    <kd>100.0</kd>
    <mu1>1.5</mu1>
    <mu2>1.5</mu2>
    <fdir1>1 0 0</fdir1>
    <maxVel>1.0</maxVel>
    <minDepth>0.00</minDepth>
  </gazebo>
  <gazebo reference="l_clav">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="l_lfarm">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="l_ufarm">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="l_hand">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="l_larm">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="l_lglut">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="l_lleg">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="l_scap">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="l_talus">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="l_uarm">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="l_uglut">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="l_uleg">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="ltorso">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="mtorso">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="pelvis">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="r_clav">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="r_lfarm">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="r_ufarm">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="r_hand">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="r_larm">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="r_lglut">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="r_lleg">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="r_scap">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="r_talus">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="r_uarm">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="r_uglut">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="r_uleg">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="utorso">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo>
    <plugin filename="libgazebo_ros_p3d.so" name="gazebo_ros_p3d">
      <bodyName>pelvis</bodyName>
      <topicName>/ground_truth_odom</topicName>
      <alwaysOn>true</alwaysOn>
      <updateRate>100.0</updateRate>
    </plugin>
  </gazebo>
  <gazebo reference="r_foot">
    <!-- contact sensor -->
    <sensor name="r_foot_contact_sensor" type="contact">
      <update_rate>1000.0</update_rate>
      <always_on>1</always_on>
      <contact>
        <collision>r_foot_collision</collision>
        <topic>/r_foot_contact</topic>
      </contact>
    </sensor>
  </gazebo>
  <gazebo reference="l_foot">
    <!-- contact sensor -->
    <sensor name="l_foot_contact_sensor" type="contact">
      <update_rate>1000.0</update_rate>
      <always_on>1</always_on>
      <contact>
        <collision>l_foot_collision</collision>
        <topic>/l_foot_contact</topic>
      </contact>
    </sensor>
  </gazebo>
  <gazebo>
    <plugin filename="libgazebo_ros_force.so" name="gazebo_ros_force">
      <bodyName>pelvis</bodyName>
      <topicName>/atlas/apply_pelvis_force</topicName>
    </plugin>
  </gazebo>
  <gazebo reference="back_bkz">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="back_bky">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="back_bkx">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="l_arm_elx">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="l_arm_ely">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="l_arm_wrx">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="l_arm_shx">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="l_arm_shz">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="l_arm_wry">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="l_arm_wry2">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="l_leg_kny">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="l_leg_hpy">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="l_leg_hpx">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="l_leg_aky">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="l_leg_hpz">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="neck_ry">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="r_arm_elx">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="r_arm_ely">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="r_arm_wrx">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="r_arm_shx">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="r_arm_shz">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="r_arm_wry">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="r_arm_wry2">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="r_leg_kny">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="r_leg_hpy">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="r_leg_hpx">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="r_leg_aky">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="r_leg_hpz">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="r_situational_awareness_camera_link">
    <sensor name="r_situational_awareness_camera_sensor" type="camera">
      <update_rate>60.0</update_rate>
      <camera name="r_situational_awareness_camera">
        <pose>0 0 0 0 0 0</pose>
        <!-- Spec sheet says 185deg HFOV @ 1280x1024pi @ 60 hz.
             Don't think most GPU's can achieve this at real-time right now.
        <horizontal_fov>3.22885912</horizontal_fov>
        -->
        <horizontal_fov>2.0</horizontal_fov>
        <image>
          <width>1280</width>
          <height>1024</height>
          <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.02</near>
          <far>300</far>
        </clip>
        <noise>
          <type>gaussian</type>
          <!-- Noise is sampled independently per pixel on each frame.  
               That pixel's noise value is added to each of its color
               channels, which at that point lie in the range [0,1].
               The stddev value of 0.007 is based on experimental data 
               from a camera in a Sandia hand pointed at a static scene
               in a couple of different lighting conditions.  -->
          <mean>0.0</mean>
          <stddev>0.007</stddev>
        </noise>
      </camera>
      <plugin filename="libgazebo_ros_camera.so" name="r_situational_awareness_camera_controller">
        <alwaysOn>true</alwaysOn>
        <updateRate>0.0</updateRate>
        <cameraName>r_situational_awareness_camera</cameraName>
        <imageTopicName>image_raw</imageTopicName>
        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
        <frameName>r_situational_awareness_camera_optical_frame</frameName>
        <hackBaseline>0.0</hackBaseline>
        <distortionK1>0.0</distortionK1>
        <distortionK2>0.0</distortionK2>
        <distortionK3>0.0</distortionK3>
        <distortionT1>0.0</distortionT1>
        <distortionT2>0.0</distortionT2>
      </plugin>
    </sensor>
  </gazebo>
  <gazebo reference="l_situational_awareness_camera_link">
    <sensor name="l_situational_awareness_camera_sensor" type="camera">
      <update_rate>60.0</update_rate>
      <camera name="l_situational_awareness_camera">
        <pose>0 0 0 0 0 0</pose>
        <!-- Spec sheet says 185deg HFOV @ 1280x1024pi @ 60 hz.
             Don't think most GPU's can achieve this at real-time right now.
        <horizontal_fov>3.22885912</horizontal_fov>
        -->
        <horizontal_fov>2.0</horizontal_fov>
        <image>
          <width>1280</width>
          <height>1024</height>
          <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.02</near>
          <far>300</far>
        </clip>
        <noise>
          <type>gaussian</type>
          <!-- Noise is sampled independently per pixel on each frame.  
               That pixel's noise value is added to each of its color
               channels, which at that point lie in the range [0,1].
               The stddev value of 0.007 is based on experimental data 
               from a camera in a Sandia hand pointed at a static scene
               in a couple of different lighting conditions.  -->
          <mean>0.0</mean>
          <stddev>0.007</stddev>
        </noise>
      </camera>
      <plugin filename="libgazebo_ros_camera.so" name="l_situational_awareness_camera_controller">
        <alwaysOn>true</alwaysOn>
        <updateRate>0.0</updateRate>
        <cameraName>l_situational_awareness_camera</cameraName>
        <imageTopicName>image_raw</imageTopicName>
        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
        <frameName>l_situational_awareness_camera_optical_frame</frameName>
        <hackBaseline>0.0</hackBaseline>
        <distortionK1>0.0</distortionK1>
        <distortionK2>0.0</distortionK2>
        <distortionK3>0.0</distortionK3>
        <distortionT1>0.0</distortionT1>
        <distortionT2>0.0</distortionT2>
      </plugin>
    </sensor>
  </gazebo>
  <gazebo reference="rear_situational_awareness_camera_link">
    <!-- Rear situational awareness camera is a D-Link DCS-2310L camera.
         Specs taken from website:
         http://www.dlink.com/us/en/home-solutions/view/network-cameras/dcs-2310l-outdoor-hd-poe-day-night-cloud-camera
         FOV: (H)57.8 deg, (V)37.8 deg, (D)66.0 deg
         Avalilable Resolutions:
           16:9 - 1280 x 800, 1280 x 720, 800 x 450, 640 x 360, 480 x 270, 320 x 176, 176 x 144 up to 30 fps recording1
           4:3 - 1024 x 768, 800 x 600, 640 x 480, 480 x 360, 320 x 240, 176 x 144 up to 30 fps recording1 
        -->
    <sensor name="rear_situational_awareness_camera_sensor" type="camera">
      <update_rate>30.0</update_rate>
      <camera name="rear_situational_awareness_camera">
        <pose>0 0 0 0 0 0</pose>
        <horizontal_fov>1.0</horizontal_fov>
        <image>
          <width>1280</width>
          <height>800</height>
          <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.02</near>
          <far>300</far>
        </clip>
        <noise>
          <type>gaussian</type>
          <!-- Noise is sampled independently per pixel on each frame.  
               That pixel's noise value is added to each of its color
               channels, which at that point lie in the range [0,1].
               The stddev value of 0.007 is based on experimental data 
               from a camera in a Sandia hand pointed at a static scene
               in a couple of different lighting conditions.  -->
          <mean>0.0</mean>
          <stddev>0.007</stddev>
        </noise>
      </camera>
      <plugin filename="libgazebo_ros_camera.so" name="rear_situational_awareness_camera_controller">
        <alwaysOn>true</alwaysOn>
        <updateRate>0.0</updateRate>
        <cameraName>rear_situational_awareness_camera</cameraName>
        <imageTopicName>image_raw</imageTopicName>
        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
        <frameName>rear_situational_awareness_camera_optical_frame</frameName>
        <hackBaseline>0.0</hackBaseline>
        <distortionK1>0.0</distortionK1>
        <distortionK2>0.0</distortionK2>
        <distortionK3>0.0</distortionK3>
        <distortionT1>0.0</distortionT1>
        <distortionT2>0.0</distortionT2>
      </plugin>
    </sensor>
  </gazebo>
  <transmission name="head_hokuyo_trans" type="pr2_mechanism_model/SimpleTransmission">
    <joint name="hokuyo_joint"/>
    <actuator name="hokuyo_motor"/>
    <mechanicalReduction>1</mechanicalReduction>
  </transmission>
  <transmission name="back_bkz_trans" type="pr2_mechanism_model/SimpleTransmission">
    <joint name="back_bkz"/>
    <actuator name="back_bkz_motor"/>
    <mechanicalReduction>1</mechanicalReduction>
  </transmission>
  <transmission name="back_bky_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="back_bky_motor"/>
    <joint name="back_bky"/>
    <mechanicalReduction>1</mechanicalReduction>
  </transmission>
  <transmission name="back_bkx_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="back_bkx_motor"/>
    <joint name="back_bkx"/>
    <mechanicalReduction>1</mechanicalReduction>
  </transmission>
  <transmission name="l_arm_elx_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="l_arm_elx_motor"/>
    <joint name="l_arm_elx"/>
    <mechanicalReduction>1</mechanicalReduction>
  </transmission>
  <transmission name="l_arm_ely_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="l_arm_ely_motor"/>
    <joint name="l_arm_ely"/>
    <mechanicalReduction>1</mechanicalReduction>
  </transmission>
  <transmission name="l_arm_wrx_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="l_arm_wrx_motor"/>
    <joint name="l_arm_wrx"/>
    <mechanicalReduction>1</mechanicalReduction>
  </transmission>
  <transmission name="l_arm_shx_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="l_arm_shx_motor"/>
    <joint name="l_arm_shx"/>
    <mechanicalReduction>1</mechanicalReduction>
  </transmission>
  <transmission name="l_arm_shz_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="l_arm_shz_motor"/>
    <joint name="l_arm_shz"/>
    <mechanicalReduction>1</mechanicalReduction>
  </transmission>
  <transmission name="l_arm_wry_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="l_arm_wry_motor"/>
    <joint name="l_arm_wry"/>
    <mechanicalReduction>1</mechanicalReduction>
  </transmission>
  <transmission name="l_arm_wry2_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="l_arm_wry2_motor"/>
    <joint name="l_arm_wry2"/>
    <mechanicalReduction>1</mechanicalReduction>
  </transmission>
  <transmission name="l_leg_kny_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="l_leg_kny_motor"/>
    <joint name="l_leg_kny"/>
    <mechanicalReduction>1</mechanicalReduction>
  </transmission>
  <transmission name="l_leg_akx_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="l_leg_akx_motor"/>
    <joint name="l_leg_akx"/>
    <mechanicalReduction>1</mechanicalReduction>
  </transmission>
  <transmission name="l_leg_hpy_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="l_leg_hpy_motor"/>
    <joint name="l_leg_hpy"/>
    <mechanicalReduction>1</mechanicalReduction>
  </transmission>
  <transmission name="l_leg_hpx_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="l_leg_hpx_motor"/>
    <joint name="l_leg_hpx"/>
    <mechanicalReduction>1</mechanicalReduction>
  </transmission>
  <transmission name="l_leg_aky_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="l_leg_aky_motor"/>
    <joint name="l_leg_aky"/>
    <mechanicalReduction>1</mechanicalReduction>
  </transmission>
  <transmission name="l_leg_hpz_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="l_leg_hpz_motor"/>
    <joint name="l_leg_hpz"/>
    <mechanicalReduction>1</mechanicalReduction>
  </transmission>
  <transmission name="neck_ry_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="neck_ry_motor"/>
    <joint name="neck_ry"/>
    <mechanicalReduction>1</mechanicalReduction>
  </transmission>
  <transmission name="r_arm_elx_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="r_arm_elx_motor"/>
    <joint name="r_arm_elx"/>
    <mechanicalReduction>1</mechanicalReduction>
  </transmission>
  <transmission name="r_arm_ely_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="r_arm_ely_motor"/>
    <joint name="r_arm_ely"/>
    <mechanicalReduction>1</mechanicalReduction>
  </transmission>
  <transmission name="r_arm_wrx_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="r_arm_wrx_motor"/>
    <joint name="r_arm_wrx"/>
    <mechanicalReduction>1</mechanicalReduction>
  </transmission>
  <transmission name="r_arm_shx_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="r_arm_shx_motor"/>
    <joint name="r_arm_shx"/>
    <mechanicalReduction>1</mechanicalReduction>
  </transmission>
  <transmission name="r_arm_shz_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="r_arm_shz_motor"/>
    <joint name="r_arm_shz"/>
    <mechanicalReduction>1</mechanicalReduction>
  </transmission>
  <transmission name="r_arm_wry_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="r_arm_wry_motor"/>
    <joint name="r_arm_wry"/>
    <mechanicalReduction>1</mechanicalReduction>
  </transmission>
  <transmission name="r_arm_wry2_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="r_arm_wry2_motor"/>
    <joint name="r_arm_wry2"/>
    <mechanicalReduction>1</mechanicalReduction>
  </transmission>
  <transmission name="r_leg_kny_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="r_leg_kny_motor"/>
    <joint name="r_leg_kny"/>
    <mechanicalReduction>1</mechanicalReduction>
  </transmission>
  <transmission name="r_leg_akx_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="r_leg_akx_motor"/>
    <joint name="r_leg_akx"/>
    <mechanicalReduction>1</mechanicalReduction>
  </transmission>
  <transmission name="r_leg_hpy_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="r_leg_hpy_motor"/>
    <joint name="r_leg_hpy"/>
    <mechanicalReduction>1</mechanicalReduction>
  </transmission>
  <transmission name="r_leg_hpx_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="r_leg_hpx_motor"/>
    <joint name="r_leg_hpx"/>
    <mechanicalReduction>1</mechanicalReduction>
  </transmission>
  <transmission name="r_leg_aky_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="r_leg_aky_motor"/>
    <joint name="r_leg_aky"/>
    <mechanicalReduction>1</mechanicalReduction>
  </transmission>
  <transmission name="r_leg_hpz_trans" type="pr2_mechanism_model/SimpleTransmission">
    <actuator name="r_leg_hpz_motor"/>
    <joint name="r_leg_hpz"/>
    <mechanicalReduction>1</mechanicalReduction>
  </transmission>
  <link name="head">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.075493 3.3383E-05 0.02774"/>
      <mass value="1.41984"/>
      <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_sl_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>
    <collision>
      <origin rpy="0 0 0" xyz="-0.093 0 0.0868"/>
      <geometry>
        <box size="0.0468 0.12 0.1184"/>
      </geometry>
    </collision>
  </link>
  <link name="hokuyo_link">
    <inertial>
      <origin rpy="0 0 0" xyz="0.032428 0.0004084 -0.0041835"/>
      <mass value="0.057654"/>
      <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 xyz="0.065774 -0.001163 -0.08809" rpy="0 0 0" /-->
      <!--origin xyz="0.045 -0.001163 -0.08809" rpy="-0.314 0 0" /-->
      <origin rpy="-0.314 0 0" xyz="0.045 -0.0261018277 -0.08342369"/>
      <geometry>
        <mesh filename="package://multisense_sl_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.01885 0 -0.02119"/>
      <geometry>
        <box size="0.08 0.06 0.04238"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="0 0 0" xyz="0.03 0 0.0235"/>
      <geometry>
        <cylinder length="0.047" radius="0.024425"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="head">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <gazebo reference="hokuyo_link">
    <mu1>0.9</mu1>
    <mu2>0.9</mu2>
  </gazebo>
  <joint name="hokuyo_joint" type="continuous">
    <origin rpy="0 0 0" xyz="-0.0446 0 0.0880"/>
    <parent link="head"/>
    <child link="hokuyo_link"/>
    <axis xyz="1 0 0"/>
    <dynamics damping="0.01" friction="0"/>
  </joint>
  <gazebo reference="hokuyo_joint">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <joint name="head_hokuyo_joint" type="fixed">
    <axis xyz="0 1 0"/>
    <origin rpy="0 0 0" xyz="0.03 0 0.015"/>
    <parent link="hokuyo_link"/>
    <child link="head_hokuyo_frame"/>
  </joint>
  <link name="head_hokuyo_frame">
    <inertial>
      <mass value="1e-5"/>
      <!-- collocate with parent link and remove mass from it -->
      <origin rpy="0 0 0" xyz="0.042428 0.0004084 0.0108165"/>
      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
    </inertial>
  </link>
  <gazebo reference="head_hokuyo_frame">
    <sensor name="head_hokuyo_sensor" type="gpu_ray">
      <pose>0 0 0 0 0 0</pose>
      <visualize>false</visualize>
      <update_rate>40</update_rate>
      <ray>
        <scan>
          <horizontal>
            <samples>1081</samples>
            <resolution>1</resolution>
            <min_angle>-2.356194</min_angle>
            <max_angle>2.356194</max_angle>
          </horizontal>
        </scan>
        <range>
          <min>0.10</min>
          <max>30.0</max>
          <resolution>0.01</resolution>
        </range>
        <noise>
          <type>gaussian</type>
          <!-- Noise parameters based on published spec for Hokuyo laser
               achieving "+-30mm" accuracy at range < 10m.  A mean of 0.0m and
               stddev of 0.01m will put 99.7% of samples within 0.03m of the true
               reading. -->
          <mean>0.0</mean>
          <stddev>0.01</stddev>
        </noise>
      </ray>
      <plugin filename="libgazebo_ros_gpu_laser.so" name="gazebo_ros_head_hokuyo_controller">
        <topicName>/multisense/lidar_scan</topicName>
        <frameName>head_hokuyo_frame</frameName>
      </plugin>
    </sensor>
  </gazebo>
  <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="1e-5"/>
      <!-- collocate with parent link and remove mass from it -->
      <origin xyz="-0.075493 0.035033383 0.02574"/>
      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
    </inertial>
  </link>
  <joint name="left_camera_optical_frame_joint" type="fixed">
    <origin rpy="-1.5708 0.0 -1.5708" xyz="0 0 0"/>
    <parent link="left_camera_frame"/>
    <child link="left_camera_optical_frame"/>
  </joint>
  <link name="left_camera_optical_frame"/>
  <gazebo reference="left_camera_frame">
    <sensor name="stereo_camera" type="multicamera">
      <!-- see MultiSenseSLPlugin.h for available modes -->
      <update_rate>10.0</update_rate>
      <camera name="left">
        <!-- Spec sheet says 80deg X 45deg @ 1024x544pix.  Based on feedback
             from teams, we're instead doing 80deg X 80deg @ 800x800pix. -->
        <horizontal_fov>1.3962634</horizontal_fov>
        <image>
          <width>1024</width>
          <height>544</height>
          <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.02</near>
          <far>300</far>
        </clip>
        <noise>
          <type>gaussian</type>
          <!-- Noise is sampled independently per pixel on each frame.  
               That pixel's noise value is added to each of its color
               channels, which at that point lie in the range [0,1].
               The stddev value of 0.007 is based on experimental data 
               from a camera in a Sandia hand pointed at a static scene
               in a couple of different lighting conditions.  -->
          <mean>0.0</mean>
          <stddev>0.007</stddev>
        </noise>
      </camera>
      <camera name="right">
        <pose>0 -0.07 0 0 0 0</pose>
        <!-- Spec sheet says 80deg X 45deg @ 1024x544pix. -->
        <horizontal_fov>1.3962634</horizontal_fov>
        <image>
          <width>1024</width>
          <height>544</height>
          <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.02</near>
          <far>300</far>
        </clip>
        <noise>
          <type>gaussian</type>
          <!-- Noise is sampled independently per pixel on each frame.  
               That pixel's noise value is added to each of its color
               channels, which at that point lie in the range [0,1].
               The stddev value of 0.007 is based on experimental data 
               from a camera in a Sandia hand pointed at a static scene
               in a couple of different lighting conditions.  -->
          <mean>0.0</mean>
          <stddev>0.007</stddev>
        </noise>
      </camera>
      <plugin filename="libgazebo_ros_multicamera.so" name="stereo_camera_controller">
        <alwaysOn>true</alwaysOn>
        <updateRate>0.0</updateRate>
        <cameraName>multisense/camera</cameraName>
        <imageTopicName>image_raw</imageTopicName>
        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
        <frameName>left_camera_optical_frame</frameName>
        <!--<rightFrameName>right_camera_optical_frame</rightFrameName>-->
        <hackBaseline>0.07</hackBaseline>
        <distortionK1>0.0</distortionK1>
        <distortionK2>0.0</distortionK2>
        <distortionK3>0.0</distortionK3>
        <distortionT1>0.0</distortionT1>
        <distortionT2>0.0</distortionT2>
      </plugin>
    </sensor>
  </gazebo>
  <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="1e-5"/>
      <!-- collocate with parent link and remove mass from it -->
      <origin xyz="-0.075493 -0.034966617 0.02574"/>
      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
    </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="1e-5"/>
      <!-- collocate with parent link and remove mass from it -->
      <origin rpy="0 0 0" xyz="-0.064243 0.000033383 0.03824"/>
      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
    </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="1e-5"/>
      <!-- collocate with parent link and remove mass from it -->
      <origin rpy="0 0 0" xyz="-0.064243 0.000033383 0.01224"/>
      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
    </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="1e-5"/>
      <!-- collocate with parent link and remove mass from it -->
      <origin xyz="-0.089923 0.074553383 0.078086"/>
      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
    </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="1e-5"/>
      <!-- collocate with parent link and remove mass from it -->
      <origin xyz="-0.089923 -0.074486617 0.07908"/>
      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
    </inertial>
  </link>
  <gazebo>
    <plugin filename="libMultiSenseSLPlugin.so" name="multisense_plugin"/>
  </gazebo>
  <link name="head_imu_link">
    <inertial>
      <mass value="1e-5"/>
      <!-- collocate with parent link and remove mass from it -->
      <origin rpy="0 0 0" xyz="-0.122993 0.035033383 0.02774"/>
      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
    </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="head_imu_joint" type="fixed">
    <parent link="head"/>
    <child link="head_imu_link"/>
    <!-- putting it at the same z-level as the stereo -->
    <origin rpy="0 0 0" xyz="-0.0475 0.035 0.0"/>
  </joint>
  <gazebo reference="head_imu_link">
    <!-- this is expected to be reparented to head with appropriate offset
         when head_imu_link is reduced by fixed joint reduction -->
    <!-- todo: this is working with gazebo 1.4, need to write a unit test -->
    <sensor name="head_imu_sensor" type="imu">
      <always_on>1</always_on>
      <update_rate>1000.0</update_rate>
      <imu>
        <noise>
          <type>gaussian</type>
          <!-- Noise parameters from Boston Dynamics
               (http://gazebosim.org/wiki/Sensor_noise):
                 rates (rad/s): mean=0, stddev=2e-4
                 accels (m/s/s): mean=0, stddev=1.7e-2
                 rate bias (rad/s): 5e-6 - 1e-5
                 accel bias (m/s/s): 1e-1
               Experimentally, simulation provide rates with noise of
               about 1e-3 rad/s and accels with noise of about 1e-1 m/s/s.
               So we don't expect to see the noise unless number of inner iterations
               are increased.

               We will add bias.  In this model, bias is sampled once for rates
               and once for accels at startup; the sign (negative or positive)
               of each bias is then switched with equal probability.  Thereafter,
               the biases are fixed additive offsets.  We choose
               bias means and stddevs to produce biases close to the provided
               data. -->
          <rate>
            <mean>0.0</mean>
            <stddev>2e-4</stddev>
            <bias_mean>0.0000075</bias_mean>
            <bias_stddev>0.0000008</bias_stddev>
          </rate>
          <accel>
            <mean>0.0</mean>
            <stddev>1.7e-2</stddev>
            <bias_mean>0.1</bias_mean>
            <bias_stddev>0.001</bias_stddev>
          </accel>
        </noise>
      </imu>
    </sensor>
  </gazebo>
</robot>