<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from nao_robot_v4.xacro             | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="nao" xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#slider" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" xmlns:xacro="http://ros.org/wiki/xacro">
  <!-- Macro for insert a link visualization -->
  <!-- Macro for insert a link collision -->
  <!-- dummy first link, directly connects with torso -->
  <link name="base_link">
    <inertial>
      <mass value="0.000001"/>
      <inertia ixx="0.000001" ixy="0" ixz="0" iyy="0.000001" iyz="0" izz="0.000001"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </inertial>
  </link>
  <!-- MACRO FOR DEFINE LINK-->
  <link name="Torso">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.00413 9e-05 0.04342"/>
      <mass value="1.04956"/>
      <inertia ixx="0.00704108" ixy="1.39215e-05" ixz="-3.30211e-05" iyy="0.00687677" iyz="-2.29779e-05" izz="0.00162821"/>
      <!-- <inertia ixx="1"  ixy="0"  ixz="0" iyy="1" iyz="0" izz="1" /> -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="Torso_visual_geom">
        <mesh filename="package://nao_model/mesh/dae/Torso.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.008 0 0.035"/>
      <geometry name="Torso_collision_geom">
        <box size="0.111 0.192 0.23"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="Torso">
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <link name="Neck">
    <inertial>
      <origin rpy="0 0 0" xyz="-1e-05 0.00014 -0.02742"/>
      <mass value="0.06442"/>
      <inertia ixx="0.000123429" ixy="1.47981e-09" ixz="-6.76036e-10" iyy="0.000124435" iyz="-3.00245e-07" izz="5.535e-06"/>
      <!-- <inertia ixx="1"  ixy="0"  ixz="0" iyy="1" iyz="0" izz="1" /> -->
    </inertial>
  </link>
  <gazebo reference="Neck">
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <link name="Head">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.00112 3e-05 0.05258"/>
      <mass value="0.60533"/>
      <inertia ixx="0.00430483" ixy="8.7678e-06" ixz="5.33702e-06" iyy="0.00416541" iyz="-2.90031e-05" izz="0.000986496"/>
      <!-- <inertia ixx="1"  ixy="0"  ixz="0" iyy="1" iyz="0" izz="1" /> -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="Head_visual_geom">
        <mesh filename="package://nao_model/mesh/dae/Head.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.002 0 0.054"/>
      <geometry name="Head_collision_geom">
        <box size="0.119 0.133 0.116"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="Head">
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <link name="RShoulder">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.00165 0.02663 0.00014"/>
      <mass value="0.07504"/>
      <inertia ixx="0.000137501" ixy="-1.2692e-06" ixz="6.04576e-09" iyy="1.43614e-05" iyz="2.99484e-07" izz="0.000139839"/>
      <!-- <inertia ixx="1"  ixy="0"  ixz="0" iyy="1" iyz="0" izz="1" /> -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="RShoulder_visual_geom">
        <mesh filename="package://nao_model/mesh/dae/RShoulder.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="-0.018 -0.007 0"/>
      <geometry name="RShoulder_collision_geom">
        <box size="0.061 0.057 0.082"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="RShoulder">
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <link name="LShoulder">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.00165 -0.02663 0.00014"/>
      <mass value="0.07504"/>
      <inertia ixx="0.000137501" ixy="5.32524e-06" ixz="6.04576e-09" iyy="1.43614e-05" iyz="-2.60044e-07" izz="0.000139839"/>
      <!-- <inertia ixx="1"  ixy="0"  ixz="0" iyy="1" iyz="0" izz="1" /> -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="LShoulder_visual_geom">
        <mesh filename="package://nao_model/mesh/dae/LShoulder.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="-0.018 0.007 0"/>
      <geometry name="LShoulder_collision_geom">
        <box size="0.061 0.057 0.082"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="LShoulder">
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <link name="RBicep">
    <inertial>
      <origin rpy="0 0 0" xyz="0.02429 -0.00952 0.0032"/>
      <mass value="0.15794"/>
      <inertia ixx="0.000126052" ixy="4.01691e-05" ixz="-1.37697e-05" iyy="0.000462379" iyz="7.2868e-06" izz="0.000462117"/>
      <!-- <inertia ixx="1"  ixy="0"  ixz="0" iyy="1" iyz="0" izz="1" /> -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="RBicep_visual_geom">
        <mesh filename="package://nao_model/mesh/dae/RBicep.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.052 -0.008 0.001"/>
      <geometry name="RBicep_collision_geom">
        <box size="0.143 0.05 0.055"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="RBicep">
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <link name="LBicep">
    <inertial>
      <origin rpy="0 0 0" xyz="0.02455 0.00563 0.0033"/>
      <mass value="0.15777"/>
      <inertia ixx="0.000100619" ixy="-2.53381e-05" ixz="-1.4213e-05" iyy="0.000468325" iyz="4.71439e-07" izz="0.00044199"/>
      <!-- <inertia ixx="1"  ixy="0"  ixz="0" iyy="1" iyz="0" izz="1" /> -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="LBicep_visual_geom">
        <mesh filename="package://nao_model/mesh/dae/LBicep.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.052 0.008 0.001"/>
      <geometry name="LBicep_collision_geom">
        <box size="0.143 0.05 0.055"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="LBicep">
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <link name="RElbow">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.02744 0 -0.00014"/>
      <mass value="0.06483"/>
      <inertia ixx="5.59842e-06" ixy="4.21e-09" ixz="2.92241e-07" iyy="0.000124248" iyz="-1.84e-09" izz="0.000125257"/>
      <!-- <inertia ixx="1"  ixy="0"  ixz="0" iyy="1" iyz="0" izz="1" /> -->
    </inertial>
  </link>
  <gazebo reference="RElbow">
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <link name="LElbow">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.02744 0 -0.00014"/>
      <mass value="0.06483"/>
      <inertia ixx="5.59842e-06" ixy="4.21e-09" ixz="2.92241e-07" iyy="0.000124248" iyz="-1.84e-09" izz="0.000125257"/>
      <!-- <inertia ixx="1"  ixy="0"  ixz="0" iyy="1" iyz="0" izz="1" /> -->
    </inertial>
  </link>
  <gazebo reference="LElbow">
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <link name="RForeArm">
    <inertial>
      <origin rpy="0 0 0" xyz="0.02552 -0.00281 0.0009"/>
      <mass value="0.07778"/>
      <inertia ixx="2.60679e-05" ixy="-3.24527e-06" ixz="1.18528e-06" iyy="0.000139939" iyz="-1.69766e-07" izz="0.000138518"/>
      <!-- <inertia ixx="1"  ixy="0"  ixz="0" iyy="1" iyz="0" izz="1" /> -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="RForeArm_visual_geom">
        <mesh filename="package://nao_model/mesh/dae/RForeArm.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.023 -0.003 0.003"/>
      <geometry name="RForeArm_collision_geom">
        <box size="0.072 0.051 0.052"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="RForeArm">
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <link name="LForeArm">
    <inertial>
      <origin rpy="0 0 0" xyz="0.02556 0.00281 0.00076"/>
      <mass value="0.07761"/>
      <inertia ixx="2.59898e-05" ixy="3.23152e-06" ixz="1.58221e-06" iyy="0.000139881" iyz="1.39194e-07" izz="0.000138604"/>
      <!-- <inertia ixx="1"  ixy="0"  ixz="0" iyy="1" iyz="0" izz="1" /> -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="LForeArm_visual_geom">
        <mesh filename="package://nao_model/mesh/dae/LForeArm.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.023 0.003 0.003"/>
      <geometry name="LForeArm_collision_geom">
        <box size="0.072 0.051 0.052"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="LForeArm">
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <link name="RHand">
    <inertial>
      <origin rpy="0 0 0" xyz="0.03434 -0.00088 0.00308"/>
      <mass value="0.18533"/>
      <inertia ixx="7.2451e-05" ixy="1.15466e-07" ixz="-2.87253e-06" iyy="0.000576368" iyz="2.67539e-06" izz="0.000570611"/>
      <!-- <inertia ixx="1"  ixy="0"  ixz="0" iyy="1" iyz="0" izz="1" /> -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="RHand_visual_geom">
        <mesh filename="package://nao_model/mesh/dae/RHand.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.047 0 0.001"/>
      <geometry name="RHand_collision_geom">
        <box size="0.084 0.068 0.063"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="RHand">
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <link name="LHand">
    <inertial>
      <origin rpy="0 0 0" xyz="0.03434 -0.00088 0.00308"/>
      <mass value="0.18533"/>
      <inertia ixx="7.2451e-05" ixy="1.15466e-07" ixz="-2.87253e-06" iyy="0.000576368" iyz="2.67539e-06" izz="0.000570611"/>
      <!-- <inertia ixx="1"  ixy="0"  ixz="0" iyy="1" iyz="0" izz="1" /> -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="LHand_visual_geom">
        <mesh filename="package://nao_model/mesh/dae/LHand.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.047 0 0.001"/>
      <geometry name="LHand_collision_geom">
        <box size="0.084 0.068 0.063"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="LHand">
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <link name="RFinger">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0"/>
      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
      <!-- <inertia ixx="1"  ixy="0"  ixz="0" iyy="1" iyz="0" izz="1" /> -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="RFinger_visual_geom">
        <mesh filename="package://nao_model/mesh/dae/RFinger.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.06 0 -0.019"/>
      <geometry name="RFinger_collision_geom">
        <box size="0.04 0.04 0.04"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="RFinger">
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <link name="LFinger">
    <inertial>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="0"/>
      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
      <!-- <inertia ixx="1"  ixy="0"  ixz="0" iyy="1" iyz="0" izz="1" /> -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="LFinger_visual_geom">
        <mesh filename="package://nao_model/mesh/dae/LFinger.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.06 0 -0.019"/>
      <geometry name="LFinger_collision_geom">
        <box size="0.04 0.04 0.04"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="LFinger">
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <link name="RPelvis">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.00766 0.012 0.02716"/>
      <mass value="0.07118"/>
      <inertia ixx="0.000152729" ixy="-1.54068e-06" ixz="-2.07344e-06" iyy="0.00016221" iyz="-4.50181e-06" izz="8.13137e-05"/>
      <!-- <inertia ixx="1"  ixy="0"  ixz="0" iyy="1" iyz="0" izz="1" /> -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="RPelvis_visual_geom">
        <mesh filename="package://nao_model/mesh/dae/RPelvis.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="-0.011 -0.008 0.018"/>
      <geometry name="RPelvis_collision_geom">
        <box size="0.08 0.05 0.056"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="RPelvis">
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <link name="LPelvis">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.00781 -0.01114 0.02661"/>
      <mass value="0.06981"/>
      <inertia ixx="0.000139598" ixy="1.07922e-06" ixz="-1.76003e-06" iyy="0.000155016" iyz="2.76058e-06" izz="7.55452e-05"/>
      <!-- <inertia ixx="1"  ixy="0"  ixz="0" iyy="1" iyz="0" izz="1" /> -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="LPelvis_visual_geom">
        <mesh filename="package://nao_model/mesh/dae/LPelvis.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="-0.011 0.008 0.018"/>
      <geometry name="LPelvis_collision_geom">
        <box size="0.08 0.05 0.056"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="LPelvis">
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <link name="RHip">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.01549 -0.00029 -0.00516"/>
      <mass value="0.13053"/>
      <inertia ixx="3.1073e-05" ixy="5.67164e-07" ixz="6.32483e-06" iyy="0.000133065" iyz="1.97835e-07" izz="0.000119434"/>
      <!-- <inertia ixx="1"  ixy="0"  ixz="0" iyy="1" iyz="0" izz="1" /> -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="RHip_visual_geom">
        <mesh filename="package://nao_model/mesh/dae/RHip.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="-0.013 -0 -0.002"/>
      <geometry name="RHip_collision_geom">
        <box size="0.076 0.028 0.049"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="RHip">
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <link name="LHip">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.01549 0.00029 -0.00515"/>
      <mass value="0.13053"/>
      <inertia ixx="3.10565e-05" ixy="-6.08684e-07" ixz="6.33119e-06" iyy="0.000133052" iyz="-1.99137e-07" izz="0.00011943"/>
      <!-- <inertia ixx="1"  ixy="0"  ixz="0" iyy="1" iyz="0" izz="1" /> -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="LHip_visual_geom">
        <mesh filename="package://nao_model/mesh/dae/LHip.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="-0.013 -0 -0.002"/>
      <geometry name="LHip_collision_geom">
        <box size="0.076 0.028 0.049"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="LHip">
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <link name="RThigh">
    <inertial>
      <origin rpy="0 0 0" xyz="0.00139 -0.00225 -0.05374"/>
      <mass value="0.38976"/>
      <inertia ixx="0.00276508" ixy="-2.05851e-06" ixz="5.67685e-05" iyy="0.00271859" iyz="7.95157e-06" izz="0.000306704"/>
      <!-- <inertia ixx="1"  ixy="0"  ixz="0" iyy="1" iyz="0" izz="1" /> -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="RThigh_visual_geom">
        <mesh filename="package://nao_model/mesh/dae/RThigh.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.006 -0.004 -0.057"/>
      <geometry name="RThigh_collision_geom">
        <box size="0.095 0.089 0.143"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="RThigh">
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <link name="LThigh">
    <inertial>
      <origin rpy="0 0 0" xyz="0.00138 0.00221 -0.05373"/>
      <mass value="0.38968"/>
      <inertia ixx="0.0027636" ixy="2.11296e-06" ixz="5.64129e-05" iyy="0.00271679" iyz="-7.91029e-06" izz="0.000306389"/>
      <!-- <inertia ixx="1"  ixy="0"  ixz="0" iyy="1" iyz="0" izz="1" /> -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="LThigh_visual_geom">
        <mesh filename="package://nao_model/mesh/dae/LThigh.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.006 0.004 -0.057"/>
      <geometry name="LThigh_collision_geom">
        <box size="0.095 0.089 0.143"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="LThigh">
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <link name="RTibia">
    <inertial>
      <origin rpy="0 0 0" xyz="0.00394 -0.00221 -0.04938"/>
      <mass value="0.29163"/>
      <inertia ixx="0.00189536" ixy="-3.43584e-06" ixz="-2.87418e-05" iyy="0.00184391" iyz="-6.65052e-06" izz="0.000197404"/>
      <!-- <inertia ixx="1"  ixy="0"  ixz="0" iyy="1" iyz="0" izz="1" /> -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="RTibia_visual_geom">
        <mesh filename="package://nao_model/mesh/dae/RTibia.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 -0.01 -0.062"/>
      <geometry name="RTibia_collision_geom">
        <box size="0.07 0.093 0.1"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="RTibia">
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <link name="LTibia">
    <inertial>
      <origin rpy="0 0 0" xyz="0.00453 0.00225 -0.04936"/>
      <mass value="0.29142"/>
      <inertia ixx="0.00189357" ixy="3.60392e-06" ixz="-2.86648e-05" iyy="0.00184465" iyz="7.13012e-06" izz="0.000200683"/>
      <!-- <inertia ixx="1"  ixy="0"  ixz="0" iyy="1" iyz="0" izz="1" /> -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="LTibia_visual_geom">
        <mesh filename="package://nao_model/mesh/dae/LTibia.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0.01 -0.062"/>
      <geometry name="LTibia_collision_geom">
        <box size="0.07 0.093 0.1"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="LTibia">
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <link name="RAnkle">
    <inertial>
      <origin rpy="0 0 0" xyz="0.00045 -0.0003 0.00684"/>
      <mass value="0.13415"/>
      <inertia ixx="4.47965e-05" ixy="4.62298e-08" ixz="4.28757e-06" iyy="8.06143e-05" iyz="-2.79856e-07" izz="5.49524e-05"/>
      <!-- <inertia ixx="1"  ixy="0"  ixz="0" iyy="1" iyz="0" izz="1" /> -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="RAnkle_visual_geom">
        <mesh filename="package://nao_model/mesh/dae/RAnkle.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.001 0 0.015"/>
      <geometry name="RAnkle_collision_geom">
        <box size="0.078 0.028 0.058"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="RAnkle">
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <link name="LAnkle">
    <inertial>
      <origin rpy="0 0 0" xyz="0.00045 0.00029 0.00685"/>
      <mass value="0.13416"/>
      <inertia ixx="4.48162e-05" ixy="-8.83212e-09" ixz="4.27549e-06" iyy="8.05876e-05" iyz="2.84849e-07" izz="5.49039e-05"/>
      <!-- <inertia ixx="1"  ixy="0"  ixz="0" iyy="1" iyz="0" izz="1" /> -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="LAnkle_visual_geom">
        <mesh filename="package://nao_model/mesh/dae/LAnkle.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.001 0 0.015"/>
      <geometry name="LAnkle_collision_geom">
        <box size="0.078 0.028 0.058"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="LAnkle">
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <link name="RFeet">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0254 -0.00332 -0.03239"/>
      <mass value="0.16171"/>
      <inertia ixx="0.000440736" ixy="-7.76163e-06" ixz="6.09349e-06" iyy="0.000917455" iyz="-1.45972e-06" izz="0.000631146"/>
      <!-- <inertia ixx="1"  ixy="0"  ixz="0" iyy="1" iyz="0" izz="1" /> -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="RFeet_visual_geom">
        <mesh filename="package://nao_model/mesh/dae/RFeet.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.021 -0.005 -0.013"/>
      <geometry name="RFeet_collision_geom">
        <box size="0.161 0.088 0.064"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="RFeet">
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <link name="LFeet">
    <inertial>
      <origin rpy="0 0 0" xyz="0.02542 0.0033 -0.03239"/>
      <mass value="0.16184"/>
      <inertia ixx="0.000440993" ixy="7.88039e-06" ixz="6.1279e-06" iyy="0.000918708" iyz="1.44233e-06" izz="0.000632096"/>
      <!-- <inertia ixx="1"  ixy="0"  ixz="0" iyy="1" iyz="0" izz="1" /> -->
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry name="LFeet_visual_geom">
        <mesh filename="package://nao_model/mesh/dae/LFeet.dae"/>
      </geometry>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0.021 0.005 -0.013"/>
      <geometry name="LFeet_collision_geom">
        <box size="0.161 0.088 0.064"/>
      </geometry>
    </collision>
  </link>
  <gazebo reference="LFeet">
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <gazebo reference="RFeet">
    <mu1 value="0.8"/>
    <mu2 value="0.8"/>
  </gazebo>
  <gazebo reference="LFeet">
    <mu1 value="0.8"/>
    <mu2 value="0.8"/>
  </gazebo>
  <link name="gaze"/>
  <link name="CameraTop_frame">
    <xacro:insert_visualization_cameraTop/>
  </link>
  <link name="CameraBottom_frame">
    <xacro:insert_visualization_cameraBottom/>
  </link>
  <link name="l_sole"/>
  <link name="r_sole"/>
  <!-- JOINT DEFINITION -->
  <!-- MACRO FOR DEFINE JOINT-->
  <!-- BASE -->
  <joint name="base_joint" type="fixed">
    <child link="Torso"/>
    <parent link="base_link"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <!-- HeadYaw -->
  <joint name="HeadYaw" type="revolute">
    <limit effort="100.0" lower="-2.0857" upper="2.0857" velocity="5"/>
    <parent link="Torso"/>
    <child link="Neck"/>
    <dynamics damping="0" friction="25"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 0" xyz="0 0 0.1265"/>
  </joint>
  <!-- HeadPitch -->
  <joint name="HeadPitch" type="revolute">
    <limit effort="100.0" lower="-0.672" upper="0.5149" velocity="5"/>
    <parent link="Neck"/>
    <child link="Head"/>
    <dynamics damping="0" friction="25"/>
    <axis xyz="0 1 0"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <!-- TODO CAMERA -->
  <joint name="gaze_joint" type="fixed">
    <parent link="Head"/>
    <child link="gaze"/>
    <origin xyz="0.0539 0 0.05"/>
  </joint>
  <joint name="CameraTop" type="fixed">
    <parent link="Head"/>
    <child link="CameraTop_frame"/>
    <!-- rotated by 1.2 degress -->
    <origin rpy="-1.59174 0 -1.57079632679" xyz="0.05871 0 0.06364"/>
  </joint>
  <joint name="CameraBottom" type="fixed">
    <parent link="Head"/>
    <child link="CameraBottom_frame"/>
    <!-- rotated by 39.7 degrees -->
    <origin rpy="-2.263692 0 -1.57079632679" xyz="0.05071 0 0.01774"/>
  </joint>
  <!-- END TODO CAMERA -->
  <!-- ARMS JOINT -->
  <!-- arm_joints -->
  <!-- LShoulderRoll -->
  <joint name="LShoulderRoll" type="revolute">
    <limit effort="100.0" lower="-0.3142" upper="1.3265" velocity="5"/>
    <parent link="LShoulder"/>
    <child link="LBicep"/>
    <dynamics damping="0" friction="25"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <!-- RShoulderRoll -->
  <joint name="RShoulderRoll" type="revolute">
    <limit effort="100.0" lower="-1.3265" upper="0.3142" velocity="5"/>
    <parent link="RShoulder"/>
    <child link="RBicep"/>
    <dynamics damping="0" friction="25"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <!-- LElbowRoll -->
  <joint name="LElbowRoll" type="revolute">
    <limit effort="100.0" lower="-1.5446" upper="-0.0349" velocity="5"/>
    <parent link="LElbow"/>
    <child link="LForeArm"/>
    <dynamics damping="0" friction="25"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <!-- RElbowRoll -->
  <joint name="RElbowRoll" type="revolute">
    <limit effort="100.0" lower="0.0349" upper="1.5446" velocity="5"/>
    <parent link="RElbow"/>
    <child link="RForeArm"/>
    <dynamics damping="0" friction="25"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <!-- INSERT ARM JOINTS -->
  <joint name="LShoulderPitch" type="revolute">
    <limit effort="100.0" lower="-2.0857" upper="2.0857" velocity="5"/>
    <parent link="Torso"/>
    <child link="LShoulder"/>
    <dynamics damping="0" friction="25"/>
    <axis xyz="0 1 0"/>
    <origin rpy="0 0 0" xyz="0 0.098 0.1"/>
  </joint>
  <joint name="LElbowYaw" type="revolute">
    <limit effort="100.0" lower="-2.0857" upper="2.0857" velocity="5"/>
    <parent link="LBicep"/>
    <child link="LElbow"/>
    <dynamics damping="0" friction="25"/>
    <axis xyz="1 0 0"/>
    <origin rpy="0 0 0" xyz="0.105 0.015 0"/>
  </joint>
  <joint name="LWristYaw" type="revolute">
    <limit effort="100.0" lower="-1.8238" upper="1.8238" velocity="5"/>
    <parent link="LForeArm"/>
    <child link="LHand"/>
    <dynamics damping="0" friction="25"/>
    <axis xyz="1 0 0"/>
    <origin rpy="0 0 0" xyz="0.05595 0 0"/>
  </joint>
  <joint name="LGripper" type="fixed">
    <parent link="LHand"/>
    <child link="LFinger"/>
    <origin xyz="0 0 0"/>
  </joint>
  <joint name="RShoulderPitch" type="revolute">
    <limit effort="100.0" lower="-2.0857" upper="2.0857" velocity="5"/>
    <parent link="Torso"/>
    <child link="RShoulder"/>
    <dynamics damping="0" friction="25"/>
    <axis xyz="0 1 0"/>
    <origin rpy="0 0 0" xyz="0 -0.098 0.1"/>
  </joint>
  <joint name="RElbowYaw" type="revolute">
    <limit effort="100.0" lower="-2.0857" upper="2.0857" velocity="5"/>
    <parent link="RBicep"/>
    <child link="RElbow"/>
    <dynamics damping="0" friction="25"/>
    <axis xyz="1 0 0"/>
    <origin rpy="0 0 0" xyz="0.105 -0.015 0"/>
  </joint>
  <joint name="RWristYaw" type="revolute">
    <limit effort="100.0" lower="-1.8238" upper="1.8238" velocity="5"/>
    <parent link="RForeArm"/>
    <child link="RHand"/>
    <dynamics damping="0" friction="25"/>
    <axis xyz="1 0 0"/>
    <origin rpy="0 0 0" xyz="0.05595 0 0"/>
  </joint>
  <joint name="RGripper" type="fixed">
    <parent link="RHand"/>
    <child link="RFinger"/>
    <origin xyz="0 0 0"/>
  </joint>
  <!-- LEG JOINTS -->
  <!-- LHipYawPitch -->
  <joint name="LHipYawPitch" type="revolute">
    <limit effort="100.0" lower="-1.145303" upper="0.74081" velocity="5"/>
    <parent link="Torso"/>
    <child link="LPelvis"/>
    <dynamics damping="0" friction="25"/>
    <axis xyz="0 0.707106 -0.707106"/>
    <origin rpy="0 0 0" xyz="0 0.05 -0.085"/>
  </joint>
  <!-- RHipYawPitch -->
  <joint name="RHipYawPitch" type="revolute">
    <limit effort="100.0" lower="-1.145303" upper="0.74081" velocity="5"/>
    <parent link="Torso"/>
    <child link="RPelvis"/>
    <dynamics damping="0" friction="25"/>
    <axis xyz="0 0.707106 0.707106"/>
    <origin rpy="0 0 0" xyz="0 -0.05 -0.085"/>
  </joint>
  <!-- LHipRoll -->
  <joint name="LHipRoll" type="revolute">
    <limit effort="100.0" lower="-0.379472" upper="0.790477" velocity="5"/>
    <parent link="LPelvis"/>
    <child link="LHip"/>
    <dynamics damping="0" friction="25"/>
    <axis xyz="1 0 0"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <!-- RHipRoll -->
  <joint name="RHipRoll" type="revolute">
    <limit effort="100.0" lower="-0.738321" upper="0.414754" velocity="5"/>
    <parent link="RPelvis"/>
    <child link="RHip"/>
    <dynamics damping="0" friction="25"/>
    <axis xyz="1 0 0"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <!-- LHipPitch -->
  <joint name="LHipPitch" type="revolute">
    <limit effort="100.0" lower="-1.773912" upper="0.48409" velocity="5"/>
    <parent link="LHip"/>
    <child link="LThigh"/>
    <dynamics damping="0" friction="25"/>
    <axis xyz="0 1 0"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <!-- RHipPitch -->
  <joint name="RHipPitch" type="revolute">
    <limit effort="100.0" lower="-1.772308" upper="0.485624" velocity="5"/>
    <parent link="RHip"/>
    <child link="RThigh"/>
    <dynamics damping="0" friction="25"/>
    <axis xyz="0 1 0"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <!-- LKneePitch -->
  <joint name="LKneePitch" type="revolute">
    <limit effort="100.0" lower="-0.092346" upper="2.112528" velocity="5"/>
    <parent link="LThigh"/>
    <child link="LTibia"/>
    <dynamics damping="0" friction="25"/>
    <axis xyz="0 1 0"/>
    <origin rpy="0 0 0" xyz="0 0 -0.1"/>
  </joint>
  <!-- RKneePitch -->
  <joint name="RKneePitch" type="revolute">
    <limit effort="100.0" lower="-0.103083" upper="2.120198" velocity="5"/>
    <parent link="RThigh"/>
    <child link="RTibia"/>
    <dynamics damping="0" friction="25"/>
    <axis xyz="0 1 0"/>
    <origin rpy="0 0 0" xyz="0 0 -0.1"/>
  </joint>
  <!-- LAnklePitch -->
  <joint name="LAnklePitch" type="revolute">
    <limit effort="100.0" lower="-1.189516" upper="0.922747" velocity="5"/>
    <parent link="LTibia"/>
    <child link="LAnkle"/>
    <dynamics damping="0" friction="25"/>
    <axis xyz="0 1 0"/>
    <origin rpy="0 0 0" xyz="0 0 -0.10290"/>
  </joint>
  <!-- RAnklePitch -->
  <joint name="RAnklePitch" type="revolute">
    <limit effort="100.0" lower="-1.186448" upper="0.932056" velocity="5"/>
    <parent link="RTibia"/>
    <child link="RAnkle"/>
    <dynamics damping="0" friction="25"/>
    <axis xyz="0 1 0"/>
    <origin rpy="0 0 0" xyz="0 0 -0.10290"/>
  </joint>
  <!-- LAnkleRoll -->
  <joint name="LAnkleRoll" type="revolute">
    <limit effort="100.0" lower="-0.39788" upper="0.769001" velocity="5"/>
    <parent link="LAnkle"/>
    <child link="LFeet"/>
    <dynamics damping="0" friction="25"/>
    <axis xyz="1 0 0"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <!-- RAnkleRoll -->
  <joint name="RAnkleRoll" type="revolute">
    <limit effort="100.0" lower="-0.785875" upper="0.388676" velocity="5"/>
    <parent link="RAnkle"/>
    <child link="RFeet"/>
    <dynamics damping="0" friction="25"/>
    <axis xyz="1 0 0"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <!-- l_sole_joint -->
  <joint name="l_sole_joint" type="fixed">
    <child link="l_sole"/>
    <parent link="LFeet"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <!-- r_sole_joint -->
  <joint name="r_sole_joint" type="fixed">
    <child link="r_sole"/>
    <parent link="RFeet"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
  </joint>
  <gazebo>
    <controller:nao_gazebo_plugin name="nao_gazebo_plugin" plugin="libnaoGazeboPlugin.so">
      <alwaysOn>true</alwaysOn>
      <updateRate>-1000.0</updateRate>
      <timeout>5</timeout>
    </controller:nao_gazebo_plugin>
  </gazebo>
</robot>

