<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from bigman.urdf.xacro              | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="bigman" xmlns:xacro="http://ros.org/wiki/xacro">
  <!-- 
  ROS urdf definition of the BIGMAN humanoid robot by Istituto Italiano di Tecnologia
  
  DO NOT INCLUDE THIS FILE DIRECTLY! USE bigman_robot.urdf.xacro INSTEAD!

  Author: Enrico Mingo & Alessio Rocchi

  Inertial matrix has to be in CoM frame (URDF specs)

  Velocity and effort limits are fake!
-->
  <!-- ************ MODEL CONFIGURATION ************ -->
  <!-- Head -->
  <!-- This links are defined in order to be compliant with http://www.ros.org/reps/rep-0120.html#coordinate-frames
and to define the end effectors of the robot.
The coordinate frame called gaze defines the head position and orientation. The orientation indicates in which direction the head appears to be looking regardless of any sensors (camera for instance). -->
  <link name="gaze"/>
  <joint name="gaze_joint" type="fixed">
    <parent link="torso"/>
    <child link="gaze"/>
    <origin rpy="0 0 0" xyz="0 0 0.1"/>
  </joint>
  <!-- ************ BIGMAN BASE ************** -->
  <link name="base_link"/>
  <link name="torso"/>
  <link name="Waist">
    <inertial>
      <origin rpy="0.0 0.0 0.0" xyz="-0.032862932 -0.00050930727 -0.0012899617"/>
      <mass value="6.4080904"/>
      <inertia ixx="0.038826078" ixy="0.00029047605" ixz="0.00068975598" iyy="0.025050374" iyz="-1.3370623e-05" izz="0.048055286"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="Waist_visual">
        <mesh filename="package://bigman_urdf/meshes/Waist.STL" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="Waist_collision">
        <mesh filename="package://bigman_urdf/meshes/simple/Waist.STL" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <link name="DWL">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.04610869 0.02232230 -0.02249514"/>
      <mass value="1.44965471"/>
      <inertia ixx="0.01096760" ixy="0.00111556" ixz="0.00030030" iyy="0.00409572" iyz="0.00054456" izz="0.01143795"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <geometry name="DWL_visual">
        <mesh filename="package://bigman_urdf/meshes/TorsoRoll.STL" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <geometry name="DWL_collision">
        <mesh filename="package://bigman_urdf/meshes/simple/TorsoRoll.STL" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <link name="DWS">
    <inertial>
      <origin rpy="0 0 0" xyz="0.00010203 0.00928164 0.08113610"/>
      <mass value="5.28825567"/>
      <inertia ixx="0.01628069" ixy="-0.00000323" ixz="0.00001424" iyy="0.01424286" iyz="0.00075458" izz="0.00994169"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <geometry name="DWS_visual">
        <mesh filename="package://bigman_urdf/meshes/TorsoYawPitch.STL" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <geometry name="DWS_collision">
        <mesh filename="package://bigman_urdf/meshes/simple/TorsoYawPitch.STL" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <link name="DWYTorso">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.10981228 -0.00551229 -0.06272896"/>
      <mass value="22.38913469"/>
      <inertia ixx="0.15989993" ixy="0.00087514" ixz="0.01598045" iyy="0.11274359" iyz="0.00019540" izz="0.21318148"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <geometry name="DWYTorso_visual">
        <mesh filename="package://bigman_urdf/meshes/Chest.STL" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
      <geometry name="DWYTorso_collision">
        <mesh filename="package://bigman_urdf/meshes/simple/Chest.STL" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="base_joint" type="fixed">
    <parent link="base_link"/>
    <child link="Waist"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <joint name="torso_joint" type="fixed">
    <parent link="DWYTorso"/>
    <child link="torso"/>
    <origin rpy="0 0 0" xyz="0.0 0 0.0"/>
  </joint>
  <joint name="WaistLat" type="revolute">
    <parent link="Waist"/>
    <child link="DWL"/>
    <origin rpy="0 0 0" xyz="0.0715192 0 0.195982"/>
    <axis xyz="0.89879991307 0 0.43835911792"/>
    <limit effort="120" lower="-0.610" upper="0.610" velocity="6.283"/>
    <!-- dynamics friction="0" damping="0.3"/ -->
    <dynamics damping="3.0" friction="0"/>
  </joint>
  <joint name="WaistSag" type="revolute">
    <parent link="DWL"/>
    <child link="DWS"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit effort="220" lower="-0.349" upper="1.396" velocity="6.283"/>
    <dynamics damping="3.0" friction="0"/>
  </joint>
  <joint name="WaistYaw" type="revolute">
    <parent link="DWS"/>
    <child link="DWYTorso"/>
    <origin rpy="0 0 0" xyz="0 0 0.22275"/>
    <axis xyz="0 0 1"/>
    <limit effort="120" lower="-2.844" upper="2.844" velocity="6.283"/>
    <dynamics damping="3.0" friction="0"/>
  </joint>
  <link name="l_ankle"/>
  <link name="r_ankle"/>
  <link name="l_sole"/>
  <link name="r_sole"/>
  <link name="l_toe"/>
  <link name="r_toe"/>
  <link name="RHipMot">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.10348188 -0.10919813 -0.064924056"/>
      <mass value="3.7958197"/>
      <inertia ixx="0.015639017" ixy="0.0018504661" ixz="0.0023907793" iyy="0.023519749" iyz="-0.0023303162" izz="0.020953643"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="RHipMot_visual">
        <mesh filename="package://bigman_urdf/meshes/HipRoll.STL" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="RHipMot_collision">
        <mesh filename="package://bigman_urdf/meshes/simple/HipRoll.STL" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <link name="RThighUpLeg">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.0052929709 -0.022561833 0.0056701669"/>
      <mass value="3.2402068"/>
      <inertia ixx="0.010102097" ixy="0.00010858487" ixz="0.00025291395" iyy="0.0067147" iyz="8.3447072e-05" izz="0.01048513"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="RThighUpLeg_visual">
        <mesh filename="package://bigman_urdf/meshes/HipYaw.STL" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="RThighUpLeg_collision">
        <mesh filename="package://bigman_urdf/meshes/simple/HipYaw.STL" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <link name="RThighLowLeg">
    <inertial>
      <origin rpy="0 0 0" xyz="0.037746286 -0.028539939 -0.19310133"/>
      <mass value="5.0950736"/>
      <inertia ixx="0.051607858" ixy="0.0022253472" ixz="0.002452979" iyy="0.044349153" iyz="-0.0041006942" izz="0.023374453"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="RThighLowLeg_visual">
        <mesh filename="package://bigman_urdf/meshes/HipPitch.STL" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="RThighLowLeg_collision">
        <mesh filename="package://bigman_urdf/meshes/simple/HipPitch.STL" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <link name="RLowLeg">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.0050828668 0.0037732093 -0.072605017"/>
      <mass value="5.6495309"/>
      <inertia ixx="0.10535542" ixy="-0.00058199221" ixz="0.0016743598" iyy="0.097857796" iyz="-0.0033770181" izz="0.020854086"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="RLowLeg_visual">
        <mesh filename="package://bigman_urdf/meshes/KneePitch.STL" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="RLowLeg_collision">
        <mesh filename="package://bigman_urdf/meshes/simple/KneePitch.STL" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <link name="RFootmot">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.022024173 0.0025677238 0.00094588455"/>
      <mass value="3.2844159"/>
      <inertia ixx="0.0075550679" ixy="0.00067488292" ixz="5.8332889e-05" iyy="0.011241163" iyz="-0.00014552081" izz="0.013391865"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="RFootmot_visual">
        <mesh filename="package://bigman_urdf/meshes/AnklePitch.STL" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="RFootmot_collision">
        <mesh filename="package://bigman_urdf/meshes/simple/AnklePitch.STL" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <link name="RFoot">
    <inertial>
      <origin rpy="0 0 0" xyz="0.01282904 -0.00463694 -0.10109499"/>
      <mass value="3.33109981"/>
      <inertia ixx="0.01006627" ixy="-0.00051955" ixz="-0.00374069" iyy="0.02882577" iyz="0.00044505" izz="0.02819025"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="RFoot_visual">
        <mesh filename="package://bigman_urdf/meshes/Foot.STL" scale="0.001 -0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="RFoot_collision">
        <mesh filename="package://bigman_urdf/meshes/simple/Foot.STL" scale="0.001 -0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <link name="LHipMot">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.10348188 0.10919813 -0.064924056"/>
      <mass value="3.7958197"/>
      <inertia ixx="0.015639017" ixy="-0.0018504661" ixz="0.0023907793" iyy="0.023519749" iyz="0.0023303162" izz="0.020953643"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="LHipMot_visual">
        <mesh filename="package://bigman_urdf/meshes/HipRoll.STL" scale="0.001 -0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="LHipMot_collision">
        <mesh filename="package://bigman_urdf/meshes/simple/HipRoll.STL" scale="0.001 -0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <link name="LThighUpLeg">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.0052929709 0.022561833 0.0056701669"/>
      <mass value="3.2402068"/>
      <inertia ixx="0.010102097" ixy="-0.00010858487" ixz="0.00025291395" iyy="0.0067147" iyz="-8.3447072e-05" izz="0.01048513"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="LThighUpLeg_visual">
        <mesh filename="package://bigman_urdf/meshes/HipYaw.STL" scale="0.001 -0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="LThighUpLeg_collision">
        <mesh filename="package://bigman_urdf/meshes/simple/HipYaw.STL" scale="0.001 -0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <link name="LThighLowLeg">
    <inertial>
      <origin rpy="0 0 0" xyz="0.037746286 0.028539939 -0.19310133"/>
      <mass value="5.0950736"/>
      <inertia ixx="0.051607858" ixy="-0.0022253472" ixz="0.002452979" iyy="0.044349153" iyz="0.0041006942" izz="0.023374453"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="LThighLowLeg_visual">
        <mesh filename="package://bigman_urdf/meshes/HipPitch.STL" scale="0.001 -0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="LThighLowLeg_collision">
        <mesh filename="package://bigman_urdf/meshes/simple/HipPitch.STL" scale="0.001 -0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <link name="LLowLeg">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.0050828668 -0.0037732093 -0.072605017"/>
      <mass value="5.6495309"/>
      <inertia ixx="0.10535542" ixy="0.00058199221" ixz="0.0016743598" iyy="0.097857796" iyz="0.0033770181" izz="0.020854086"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="LLowLeg_visual">
        <mesh filename="package://bigman_urdf/meshes/KneePitch.STL" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="LLowLeg_collision">
        <mesh filename="package://bigman_urdf/meshes/simple/KneePitch.STL" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <link name="LFootmot">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.022024173 -0.0025677238 0.00094588455"/>
      <mass value="3.2844159"/>
      <inertia ixx="0.0075550679" ixy="-0.00067488292" ixz="5.8332889e-05" iyy="0.011241163" iyz="0.00014552081" izz="0.013391865"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="LFootmot_visual">
        <mesh filename="package://bigman_urdf/meshes/AnklePitch.STL" scale="0.001 -0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="LFootmot_collision">
        <mesh filename="package://bigman_urdf/meshes/simple/AnklePitch.STL" scale="0.001 -0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <link name="LFoot">
    <inertial>
      <origin rpy="0 0 0" xyz="0.01282904 -0.00463694 -0.10109499"/>
      <mass value="3.33109981"/>
      <inertia ixx="0.01006627" ixy="0.00051955" ixz="-0.00374069" iyy="0.02882577" iyz="-0.00044505" izz="0.02819025"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="LFoot_visual">
        <mesh filename="package://bigman_urdf/meshes/Foot.STL" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="LFoot_collision">
        <mesh filename="package://bigman_urdf/meshes/simple/Foot.STL" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="RHipLat" type="revolute">
    <parent link="Waist"/>
    <child link="RHipMot"/>
    <origin rpy="0 0 0" xyz="0.1059 -0.06 0.0"/>
    <axis xyz="1 0 0"/>
    <limit effort="220" lower="-0.87222" upper="0.697778" velocity="6.283"/>
    <dynamics damping="3.0" friction="0"/>
  </joint>
  <joint name="RHipYaw" type="revolute">
    <parent link="RHipMot"/>
    <child link="RThighUpLeg"/>
    <origin rpy="0 0 0" xyz="-0.0889 -0.121032 -0.217872"/>
    <axis xyz="0 0 1"/>
    <limit effort="120" lower="-1.57" upper="0.872222" velocity="6.283"/>
    <dynamics damping="3.0" friction="0"/>
  </joint>
  <joint name="RHipSag" type="revolute">
    <parent link="RThighUpLeg"/>
    <child link="RThighLowLeg"/>
    <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
    <axis xyz="0 1 0"/>
    <limit effort="220" lower="-2.09333" upper="1.046667" velocity="6.283"/>
    <dynamics damping="3.0" friction="0"/>
  </joint>
  <joint name="RKneeSag" type="revolute">
    <parent link="RThighLowLeg"/>
    <child link="RLowLeg"/>
    <origin rpy="0 0 0" xyz="0.0 0.0 -0.356"/>
    <axis xyz="0 1 0"/>
    <limit effort="220" lower="0.0" upper="2.442222" velocity="6.283"/>
    <dynamics damping="3.0" friction="0"/>
  </joint>
  <joint name="RAnkSag" type="revolute">
    <parent link="RLowLeg"/>
    <child link="RFootmot"/>
    <origin rpy="0 0 0" xyz="0.0 0.0 -0.4"/>
    <axis xyz="0 1 0"/>
    <limit effort="220" lower="-1.39556" upper="0.697778" velocity="6.283"/>
    <dynamics damping="3.0" friction="0"/>
  </joint>
  <joint name="RAnkLat" type="revolute">
    <parent link="RFootmot"/>
    <child link="RFoot"/>
    <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
    <axis xyz="1 0 0"/>
    <limit effort="120" lower="-0.785" upper="0.785" velocity="6.283"/>
    <dynamics damping="3.0" friction="0"/>
  </joint>
  <joint name="LHipLat" type="revolute">
    <parent link="Waist"/>
    <child link="LHipMot"/>
    <origin rpy="0 0 0" xyz="0.1059 0.06 0.0"/>
    <axis xyz="1 0 0"/>
    <limit effort="220" lower="-0.697778" upper="0.87222" velocity="6.283"/>
    <dynamics damping="3.0" friction="0"/>
  </joint>
  <joint name="LHipYaw" type="revolute">
    <parent link="LHipMot"/>
    <child link="LThighUpLeg"/>
    <origin rpy="0 0 0" xyz="-0.0889 0.121032 -0.217872"/>
    <axis xyz="0 0 1"/>
    <limit effort="120" lower="-0.872222" upper="1.57" velocity="6.283"/>
    <dynamics damping="3.0" friction="0"/>
  </joint>
  <joint name="LHipSag" type="revolute">
    <parent link="LThighUpLeg"/>
    <child link="LThighLowLeg"/>
    <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
    <axis xyz="0 1 0"/>
    <limit effort="220" lower="-2.09333" upper="1.046667" velocity="6.283"/>
    <dynamics damping="3.0" friction="0"/>
  </joint>
  <joint name="LKneeSag" type="revolute">
    <parent link="LThighLowLeg"/>
    <child link="LLowLeg"/>
    <origin rpy="0 0 0" xyz="0.0 0.0 -0.356"/>
    <axis xyz="0 1 0"/>
    <limit effort="220" lower="0.0" upper="2.442222" velocity="6.283"/>
    <dynamics damping="3.0" friction="0"/>
  </joint>
  <joint name="LAnkSag" type="revolute">
    <parent link="LLowLeg"/>
    <child link="LFootmot"/>
    <origin rpy="0 0 0" xyz="0.0 0.0 -0.4"/>
    <axis xyz="0 1 0"/>
    <limit effort="220" lower="-1.39556" upper="0.697778" velocity="6.283"/>
    <dynamics damping="3.0" friction="0"/>
  </joint>
  <joint name="LAnkLat" type="revolute">
    <parent link="LFootmot"/>
    <child link="LFoot"/>
    <origin rpy="0 0 0" xyz="0.0 0.0 0.0"/>
    <axis xyz="1 0 0"/>
    <limit effort="120" lower="-0.785" upper="0.785" velocity="6.283"/>
    <dynamics damping="3.0" friction="0"/>
  </joint>
  <joint name="l_ankle_joint" type="fixed">
    <parent link="LFoot"/>
    <child link="l_ankle"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <joint name="r_ankle_joint" type="fixed">
    <parent link="RFoot"/>
    <child link="r_ankle"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <joint name="l_sole_joint" type="fixed">
    <parent link="LFoot"/>
    <child link="l_sole"/>
    <origin rpy="0 0 0" xyz="0 0 -0.1435"/>
  </joint>
  <joint name="r_sole_joint" type="fixed">
    <parent link="RFoot"/>
    <child link="r_sole"/>
    <origin rpy="0 0 0" xyz="0 0 -0.1435"/>
  </joint>
  <joint name="l_toe_joint" type="fixed">
    <parent link="l_sole"/>
    <child link="l_toe"/>
    <origin rpy="0 0 0" xyz="0.195 0 0"/>
  </joint>
  <joint name="r_toe_joint" type="fixed">
    <parent link="r_sole"/>
    <child link="r_toe"/>
    <origin rpy="0 0 0" xyz="0.195 0 0"/>
  </joint>
  <link name="l_foot_upper_right_link"/>
  <joint name="l_foot_upper_right_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.195 -0.09 0"/>
    <parent link="l_sole"/>
    <child link="l_foot_upper_right_link"/>
  </joint>
  <link name="l_foot_upper_left_link"/>
  <joint name="l_foot_upper_left_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.195 0.07 0"/>
    <parent link="l_sole"/>
    <child link="l_foot_upper_left_link"/>
  </joint>
  <link name="l_foot_lower_right_link"/>
  <joint name="l_foot_lower_right_joint" type="fixed">
    <origin rpy="0 0 0" xyz="-0.111 -0.09 0"/>
    <parent link="l_sole"/>
    <child link="l_foot_lower_right_link"/>
  </joint>
  <link name="l_foot_lower_left_link"/>
  <joint name="l_foot_lower_left_joint" type="fixed">
    <origin rpy="0 0 0" xyz="-0.111 0.07 0"/>
    <parent link="l_sole"/>
    <child link="l_foot_lower_left_link"/>
  </joint>
  <link name="r_foot_upper_right_link"/>
  <joint name="r_foot_upper_right_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.195 0.09 0"/>
    <parent link="r_sole"/>
    <child link="r_foot_upper_right_link"/>
  </joint>
  <link name="r_foot_upper_left_link"/>
  <joint name="r_foot_upper_left_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.195 -0.07 0"/>
    <parent link="r_sole"/>
    <child link="r_foot_upper_left_link"/>
  </joint>
  <link name="r_foot_lower_right_link"/>
  <joint name="r_foot_lower_right_joint" type="fixed">
    <origin rpy="0 0 0" xyz="-0.111 0.09 0"/>
    <parent link="r_sole"/>
    <child link="r_foot_lower_right_link"/>
  </joint>
  <link name="r_foot_lower_left_link"/>
  <joint name="r_foot_lower_left_joint" type="fixed">
    <origin rpy="0 0 0" xyz="-0.111 -0.07 0"/>
    <parent link="r_sole"/>
    <child link="r_foot_lower_left_link"/>
  </joint>
  <link name="RShp">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.00543349 -0.04480023 0.01581062"/>
      <mass value="1.81745527"/>
      <inertia ixx="0.00571053" ixy="0.00041083" ixz="-0.00005130" iyy="0.01126908" iyz="-0.00003461" izz="0.01371197"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="RShp_visual">
        <mesh filename="package://bigman_urdf/meshes/ShoulderPitch.STL" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="RShp_collision">
        <mesh filename="package://bigman_urdf/meshes/simple/ShoulderPitch.STL" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <link name="RShr">
    <inertial>
      <origin rpy="0 0 0" xyz="0.00729315 0.00002698 -0.07529696"/>
      <mass value="5.00486460"/>
      <inertia ixx="0.01644345" ixy="-0.00001187" ixz="-0.00010546" iyy="0.01804804" iyz="-0.00000565" izz="0.00802420"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="RShr_visual">
        <mesh filename="package://bigman_urdf/meshes/TopUpperArm.STL" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="RShr_collision">
        <mesh filename="package://bigman_urdf/meshes/simple/TopUpperArm.STL" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <link name="RShy">
    <inertial>
      <origin rpy="0 0 0" xyz="0.02765373 -0.03258647 -0.12698781"/>
      <mass value="3.20289133"/>
      <inertia ixx="0.01129046" ixy="0.00013518" ixz="-0.00134760" iyy="0.00984951" iyz="-0.00026836" izz="0.00728549"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="RShy_visual">
        <mesh filename="package://bigman_urdf/meshes/BottomUpperArm.STL" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="RShy_collision">
        <mesh filename="package://bigman_urdf/meshes/simple/BottomUpperArm.STL" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <link name="RElb">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.055754915 -0.007582604 -0.043820396"/>
      <mass value="8.6765385e-01"/>
      <inertia ixx="0.0040395238" ixy="0.00034973136" ixz="-0.00068400465" iyy="0.0022460561" iyz="0.00028844444" izz="0.0044895027"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="RElb_visual">
        <mesh filename="package://bigman_urdf/meshes/Elbow.STL" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="RElb_collision">
        <mesh filename="package://bigman_urdf/meshes/simple/Elbow.STL" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <link name="LShp">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.00543349 0.04480023 0.01581062"/>
      <mass value="1.81745527"/>
      <inertia ixx="0.00571053" ixy="-0.00041083" ixz="-0.00005130" iyy="0.01126908" iyz="0.00003461" izz="0.01371197"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="LShp_visual">
        <mesh filename="package://bigman_urdf/meshes/ShoulderPitch.STL" scale="0.001 -0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="LShp_collision">
        <mesh filename="package://bigman_urdf/meshes/simple/ShoulderPitch.STL" scale="0.001 -0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <link name="LShr">
    <inertial>
      <origin rpy="0 0 0" xyz="0.00729315 -0.00002698 -0.07529696"/>
      <mass value="5.00486460"/>
      <inertia ixx="0.01644345" ixy="0.00001187" ixz="-0.00010546" iyy="0.01804804" iyz="0.00000565" izz="0.00802420"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="LShr_visual">
        <mesh filename="package://bigman_urdf/meshes/TopUpperArm.STL" scale="0.001 -0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="LShr_collision">
        <mesh filename="package://bigman_urdf/meshes/simple/TopUpperArm.STL" scale="0.001 -0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <link name="LShy">
    <inertial>
      <origin rpy="0 0 0" xyz="0.02765373 0.03258647 -0.12698781"/>
      <mass value="3.20289133"/>
      <inertia ixx="0.01129046" ixy="-0.00013518" ixz="-0.00134760" iyy="0.00984951" iyz="0.00026836" izz="0.00728549"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="LShy_visual">
        <mesh filename="package://bigman_urdf/meshes/BottomUpperArm.STL" scale="0.001 -0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="LShy_collision">
        <mesh filename="package://bigman_urdf/meshes/simple/BottomUpperArm.STL" scale="0.001 -0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <link name="LElb">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.055754915 0.007582604 -0.043820396"/>
      <mass value="8.6765385e-01"/>
      <inertia ixx="0.0040395238" ixy="-0.00034973136" ixz="-0.00068400465" iyy="0.0022460561" iyz="-0.00028844444" izz="0.0044895027"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="LElb_visual">
        <mesh filename="package://bigman_urdf/meshes/Elbow.STL" scale="0.001 -0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="LElb_collision">
        <mesh filename="package://bigman_urdf/meshes/simple/Elbow.STL" scale="0.001 -0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="RShSag" type="revolute">
    <parent link="DWYTorso"/>
    <child link="RShp"/>
    <origin rpy="0 0 0" xyz="0.0 -0.2 0.0"/>
    <axis xyz="0 0.743144825 -0.669130606"/>
    <limit effort="120" lower="-2.844" upper="2.844" velocity="6.283"/>
    <dynamics damping="3.0" friction="0"/>
  </joint>
  <joint name="RShLat" type="revolute">
    <parent link="RShp"/>
    <child link="RShr"/>
    <origin rpy="0 0 0" xyz="0.0 -0.116568264 0.03364"/>
    <axis xyz="1 0 0"/>
    <limit effort="120" lower="-3.699" upper="-0.052" velocity="6.283"/>
    <dynamics damping="3.0" friction="0"/>
  </joint>
  <joint name="RShYaw" type="revolute">
    <parent link="RShr"/>
    <child link="RShy"/>
    <origin rpy="0 0 0" xyz="0.0 0.0 -0.222"/>
    <axis xyz="0 0 1"/>
    <limit effort="120" lower="-2.844" upper="2.844" velocity="6.283"/>
    <dynamics damping="3.0" friction="0"/>
  </joint>
  <joint name="RElbj" type="revolute">
    <parent link="RShy"/>
    <child link="RElb"/>
    <origin rpy="0 0 0" xyz="0.036 0.0 -0.15"/>
    <axis xyz="0 1 0"/>
    <limit effort="60" lower="-2.966" upper="0.523" velocity="6.283"/>
    <dynamics damping="3.0" friction="0"/>
  </joint>
  <joint name="LShSag" type="revolute">
    <parent link="DWYTorso"/>
    <child link="LShp"/>
    <origin rpy="0 0 0" xyz="0.0 0.2 0.0"/>
    <axis xyz="0 0.743144825 0.669130606"/>
    <limit effort="120" lower="-2.844" upper="2.844" velocity="6.283"/>
    <dynamics damping="3.0" friction="0"/>
  </joint>
  <joint name="LShLat" type="revolute">
    <parent link="LShp"/>
    <child link="LShr"/>
    <origin rpy="0 0 0" xyz="0.0 0.116568264 0.03364"/>
    <axis xyz="1 0 0"/>
    <limit effort="120" lower="0.052" upper="3.699" velocity="6.283"/>
    <dynamics damping="3.0" friction="0"/>
  </joint>
  <joint name="LShYaw" type="revolute">
    <parent link="LShr"/>
    <child link="LShy"/>
    <origin rpy="0 0 0" xyz="0.0 0.0 -0.222"/>
    <axis xyz="0 0 1"/>
    <limit effort="120" lower="-2.844" upper="2.844" velocity="6.283"/>
    <dynamics damping="3.0" friction="0"/>
  </joint>
  <joint name="LElbj" type="revolute">
    <parent link="LShy"/>
    <child link="LElb"/>
    <origin rpy="0 0 0" xyz="0.036 0.0 -0.15"/>
    <axis xyz="0 1 0"/>
    <limit effort="60" lower="-2.966" upper="0.523" velocity="6.283"/>
    <dynamics damping="3.0" friction="0"/>
  </joint>
  <!-- ***** LEGS F/T SENSOR REF FRAMES *******-->
  <link name="l_leg_ft"/>
  <link name="r_leg_ft"/>
  <joint name="l_leg_ft_joint" type="fixed">
    <parent link="LFootmot"/>
    <child link="l_leg_ft"/>
    <origin rpy="0 0 0" xyz="0.005 0 -0.107"/>
  </joint>
  <joint name="r_leg_ft_joint" type="fixed">
    <parent link="RFootmot"/>
    <child link="r_leg_ft"/>
    <origin rpy="0 0 0" xyz="0.005 0 -0.107"/>
  </joint>
  <!-- ************ FOREARMS ************ -->
  <!-- This links are defined in order to be compliant with http://www.ros.org/reps/rep-0120.html#coordinate-frames
and to define the end effectors of the robot -->
  <!-- position and orientation of the arm regardless of the tool (grasping device for instance) attached to it -->
  <link name="l_wrist"/>
  <link name="r_wrist"/>
  <link name="LForearm">
    <inertial>
      <origin rpy="0 0 0" xyz="1.2625599e-05 0.0042975381 0.044717986"/>
      <mass value="2.2127686"/>
      <inertia ixx="0.0065428368" ixy="-4.1196967e-05" ixz="3.272727e-06" iyy="0.005587829" iyz="0.00042088668" izz="0.0024783023"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="LForearm_visual">
        <mesh filename="package://bigman_urdf/meshes/Forearm.STL" scale="0.001 -0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="LForearm_collision">
        <mesh filename="package://bigman_urdf/meshes/simple/Forearm.STL" scale="0.001 -0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <link name="LWrMot2">
    <inertial>
      <origin rpy="0 0 0" xyz="0.009819377 0.0013797184 -0.080380065"/>
      <mass value="1.2838072"/>
      <inertia ixx="0.001994124" ixy="2.569706e-05" ixz="0.00017443538" iyy="0.0021755329" iyz="-0.00012100987" izz="0.001830376"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="LWrMot2_visual">
        <mesh filename="package://bigman_urdf/meshes/ForearmPitch.STL" scale="0.001 -0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="LWrMot2_collision">
        <mesh filename="package://bigman_urdf/meshes/simple/ForearmPitch.STL" scale="0.001 -0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <link name="LWrMot3">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0070380706 -1.0811188e-05 -0.039669274"/>
      <mass value="4.2801447e-01"/>
      <inertia ixx="0.00028620156" ixy="5.2570457e-08" ixz="-3.9174213e-05" iyy="0.00071261855" iyz="-9.5020266e-08" izz="0.00061444258"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="LWrMot3_visual">
        <mesh filename="package://bigman_urdf/meshes/ForearmRoll.STL" scale="0.001 -0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="LWrMot3_collision">
        <mesh filename="package://bigman_urdf/meshes/simple/ForearmRoll.STL" scale="0.001 -0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <link name="RForearm">
    <inertial>
      <origin rpy="0 0 0" xyz="1.2625599e-05 -0.0042975381 0.044717986"/>
      <mass value="2.2127686"/>
      <inertia ixx="0.0065428368" ixy="4.1196967e-05" ixz="3.272727e-06" iyy="0.005587829" iyz="-0.00042088668" izz="0.0024783023"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="RForearm_visual">
        <mesh filename="package://bigman_urdf/meshes/Forearm.STL" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="RForearm_collision">
        <mesh filename="package://bigman_urdf/meshes/simple/Forearm.STL" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <link name="RWrMot2">
    <inertial>
      <origin rpy="0 0 0" xyz="0.009819377 -0.0013797184 -0.080380065"/>
      <mass value="1.2838072"/>
      <inertia ixx="0.001994124" ixy="-2.569706e-05" ixz="0.00017443538" iyy="0.0021755329" iyz="0.00012100987" izz="0.001830376"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="RWrMot2_visual">
        <mesh filename="package://bigman_urdf/meshes/ForearmPitch.STL" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="RWrMot2_collision">
        <mesh filename="package://bigman_urdf/meshes/simple/ForearmPitch.STL" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <link name="RWrMot3">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0070380706 1.0811188e-05 -0.039669274"/>
      <mass value="4.2801447e-01"/>
      <inertia ixx="0.00028620156" ixy="-5.2570457e-08" ixz="-3.9174213e-05" iyy="0.00071261855" iyz="9.5020266e-08" izz="0.00061444258"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="RWrMot3_visual">
        <mesh filename="package://bigman_urdf/meshes/ForearmRoll.STL" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="RWrMot3_collision">
        <mesh filename="package://bigman_urdf/meshes/simple/ForearmRoll.STL" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="LForearmPlate" type="revolute">
    <parent link="LElb"/>
    <child link="LForearm"/>
    <origin rpy="0 0 0" xyz="-0.075 0.0 -0.1955"/>
    <axis xyz="0 0 1"/>
    <limit effort="60" lower="-2.529" upper="2.529" velocity="6.283"/>
    <dynamics damping="0.03" friction="0"/>
  </joint>
  <joint name="LWrj1" type="revolute">
    <parent link="LForearm"/>
    <child link="LWrMot2"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit effort="60" lower="-1.48" upper="1.48" velocity="6.283"/>
    <dynamics damping="0.03" friction="0"/>
  </joint>
  <joint name="LWrj2" type="revolute">
    <parent link="LWrMot2"/>
    <child link="LWrMot3"/>
    <origin rpy="0 0 0" xyz="0 0 -0.092"/>
    <axis xyz="1 0 0"/>
    <limit effort="60" lower="-1.48" upper="1.48" velocity="6.283"/>
    <dynamics damping="0.03" friction="0"/>
  </joint>
  <joint name="l_wrist_joint" type="fixed">
    <parent link="LWrMot3"/>
    <child link="l_wrist"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <joint name="RForearmPlate" type="revolute">
    <parent link="RElb"/>
    <child link="RForearm"/>
    <origin rpy="0 0 0" xyz="-0.075 0.0 -0.1955"/>
    <axis xyz="0 0 1"/>
    <limit effort="60" lower="-2.529" upper="2.529" velocity="6.283"/>
    <dynamics damping="0.03" friction="0"/>
  </joint>
  <joint name="RWrj1" type="revolute">
    <parent link="RForearm"/>
    <child link="RWrMot2"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit effort="60" lower="-1.48" upper="1.48" velocity="6.283"/>
    <dynamics damping="0.03" friction="0"/>
  </joint>
  <joint name="RWrj2" type="revolute">
    <parent link="RWrMot2"/>
    <child link="RWrMot3"/>
    <origin rpy="0 0 0" xyz="0 0 -0.092"/>
    <axis xyz="1 0 0"/>
    <limit effort="60" lower="-1.48" upper="1.48" velocity="6.283"/>
    <dynamics damping="0.03" friction="0"/>
  </joint>
  <joint name="r_wrist_joint" type="fixed">
    <parent link="RWrMot3"/>
    <child link="r_wrist"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <!-- ************ HANDS ************ -->
  <link name="RSoftHand">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.3"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
    </inertial>
    <visual>
      <origin rpy="-1.5707963268 0 3.14159265359" xyz="0.007 0.01 -0.04"/>
      <geometry name="RSoftHand_visual">
        <mesh filename="package://bigman_urdf/meshes/collada/forearms/R_soft_hand_open_lowpoly.dae" scale="0.1 0.1 0.1"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 3.14159265359" xyz="0 0.01 -0.04"/>
      <geometry name="RSoftHand_collision">
        <mesh filename="package://bigman_urdf/meshes/collada/simple/forearms/R_soft_hand_open.stl" scale="1E-3 1E-3 1E-3"/>
      </geometry>
    </collision>
  </link>
  <link name="LSoftHand">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0.3"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
    </inertial>
    <visual>
      <origin rpy="-1.5707963268 0 1.5707963268" xyz="0.007 -0.012 -0.04"/>
      <geometry name="LSoftHand_visual">
        <mesh filename="package://bigman_urdf/meshes/collada/forearms/L_soft_hand_open_lowpoly.dae" scale="0.1 0.1 0.1"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 1.5707963268" xyz="0.007 -0.012 -0.04"/>
      <geometry name="LSoftHand_collision">
        <mesh filename="package://bigman_urdf/meshes/collada/simple/forearms/L_soft_hand_open.stl" scale="1E-3 1E-3 1E-3"/>
      </geometry>
    </collision>
  </link>
  <joint name="r_handj" type="fixed">
    <parent link="RWrMot3"/>
    <child link="RSoftHand"/>
    <origin rpy="0 0 0" xyz="0 0 -0.11"/>
  </joint>
  <joint name="l_handj" type="fixed">
    <parent link="LWrMot3"/>
    <child link="LSoftHand"/>
    <origin rpy="0 0 0" xyz="0 0 -0.11"/>
  </joint>
  <!-- Reference frames in hands -->
  <link name="l_hand_upper_right_link"/>
  <joint name="l_hand_upper_right_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.04 0 -0.03"/>
    <parent link="LSoftHand"/>
    <child link="l_hand_upper_right_link"/>
  </joint>
  <link name="l_hand_upper_left_link"/>
  <joint name="l_hand_upper_left_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.04 0 0.03"/>
    <parent link="LSoftHand"/>
    <child link="l_hand_upper_left_link"/>
  </joint>
  <link name="l_hand_lower_right_link"/>
  <joint name="l_hand_lower_right_joint" type="fixed">
    <origin rpy="0 0 0" xyz="-0.04 0 -0.03"/>
    <parent link="LSoftHand"/>
    <child link="l_hand_lower_right_link"/>
  </joint>
  <link name="l_hand_lower_left_link"/>
  <joint name="l_hand_lower_left_joint" type="fixed">
    <origin rpy="0 0 0" xyz="-0.04 0 0.03"/>
    <parent link="LSoftHand"/>
    <child link="l_hand_lower_left_link"/>
  </joint>
  <link name="r_hand_upper_right_link"/>
  <joint name="r_hand_upper_right_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.04 0 -0.03"/>
    <parent link="RSoftHand"/>
    <child link="r_hand_upper_right_link"/>
  </joint>
  <link name="r_hand_upper_left_link"/>
  <joint name="r_hand_upper_left_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.04 0 0.03"/>
    <parent link="RSoftHand"/>
    <child link="r_hand_upper_left_link"/>
  </joint>
  <link name="r_hand_lower_right_link"/>
  <joint name="r_hand_lower_right_joint" type="fixed">
    <origin rpy="0 0 0" xyz="-0.04 0 -0.03"/>
    <parent link="RSoftHand"/>
    <child link="r_hand_lower_right_link"/>
  </joint>
  <link name="r_hand_lower_left_link"/>
  <joint name="r_hand_lower_left_joint" type="fixed">
    <origin rpy="0 0 0" xyz="-0.04 0 0.03"/>
    <parent link="RSoftHand"/>
    <child link="r_hand_lower_left_link"/>
  </joint>
  <!-- IMU -->
  <link name="imu_link">
    <inertial>
      <mass value="0.01"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="1.0E-6" ixy="0" ixz="0" iyy="1.0E-6" iyz="0" izz="1.0E-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="imu_joint" type="fixed">
    <parent link="Waist"/>
    <child link="imu_link"/>
    <origin rpy="0 3.14159265359 0" xyz="-0.072 -0.0068 0.044"/>
  </joint>
  <!-- multisense_sl_v4 with fixed joints (NOT FOR SIMULATION) -->
  <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 1.570795" xyz="-0.0446 0 0.0880"/>
      <geometry>
        <mesh filename="package://bigman_urdf/meshes/multisense.STL"/>
      </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 rpy="1.5707963267949 0 1.5707963267949" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://bigman_urdf/meshes/hokuyo_link.STL"/>
      </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>
  <joint name="hokuyo_joint" type="fixed">
    <origin rpy="0 0 0" xyz="-0.0446 0 0.0880"/>
    <parent link="head"/>
    <child link="hokuyo_link"/>
  </joint>
  <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>
  <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"/>
  <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 rpy="0 -0.15 0.53" 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 rpy="0 -0.15 -0.53" 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>
  <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>
  <joint name="neck_joint" type="fixed">
    <parent link="DWYTorso"/>
    <child link="head"/>
    <origin rpy="0 0 0" xyz="0.05 0 0.05"/>
  </joint>
</robot>

