nao.urdf

<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from nao_robot.xacro                | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<!--******************************************************************
 ****** File automatically generated by generate_urdf.py script ******
 *********************************************************************-->
<robot name="NaoH25V50" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- Import Rviz   <material name="Grey"/>
</visual>s -->
<material name="Grey">
  <color rgba="0.1 0.1 0.1 .4"/>
</material>
<material name="Orange">
  <color rgba="1.0 0.423529411765 0.0392156862745 1.0"/>
</material>
<material name="Blue">
<color rgba="0.5 0.7 1.0 1.0"/>
</material>

<material name="c1">
<color rgba="0.5 1.0 .5 1.0"/>
</material>

<material name="c2">
<color rgba="1 .3 .5 1.0"/>
</material>

<material name="c3">
<color rgba=".8 .4 .9 1.0"/>
</material>

<material name="c4">
<color rgba=".3 .1 .6 1.0"/>
</material>

<material name="c5">
<color rgba=".8 .2 1.0 1.0"/>
</material>


<material name="c6">
<color rgba=".1 .4 .6 1.0"/>
</material>

<material name="c7">
<color rgba=".3 .1 .5 1.0"/>
</material>

<material name="c8">
<color rgba=".8 .2 .8 1.0"/>
</material>

<material name="c9">
<color rgba=".2 .5 .5 1.0"/>
</material>
  <joint name="HeadYaw" type="revolute">
    <parent link="torso"/>
    <child link="Neck"/>
    <origin rpy="0 0 0" xyz="0 0 0.1265"/>
    <axis xyz="0 0 1.0"/>
    <limit effort="1.547" lower="-2.08567" upper="2.08567" velocity="8.26797"/>
  </joint>
  <link name="Neck">
    <inertial>
      <mass value="0.07842"/>
      <inertia ixx="1.60324e-05" ixy="1.57e-09" ixz="3.16276e-09" iyy="1.70394e-05" iyz="-5.295e-08" izz="5.53372e-06"/>
      <origin rpy="0 0 0" xyz="-1e-05 0 -0.02742"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/HeadYaw_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="c1"/>
</visual>
    <collision>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/HeadYaw_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="HeadPitch" type="revolute">
    <parent link="Neck"/>
    <child link="Head"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <axis xyz="0 1.0 0"/>
    <limit effort="1.532" lower="-0.671952" upper="0.514872" velocity="7.19407"/>
  </joint>
  <link name="Head">
    <inertial>
      <mass value="0.60533"/>
      <inertia ixx="0.000957766" ixy="8.78814e-06" ixz="5.33702e-06" iyy="0.000816836" iyz="-2.99579e-05" izz="0.000984976"/>
      <origin rpy="0 0 0" xyz="-0.00112 0 0.05258"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/HeadPitch_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="c2"/>
</visual>
    <collision>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/HeadPitch_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="gaze_joint" type="fixed">
    <parent link="Head"/>
    <child link="gaze"/>
    <origin rpy="0 0 0" xyz="0.05871 0 0.06364"/>
  </joint>
  <link name="gaze"/>
  <joint name="LHipYawPitch" type="revolute">
    <parent link="torso"/>
    <child link="LPelvis"/>
    <origin rpy="0 0 0" xyz="0 0.05 -0.085"/>
    <axis xyz="0 0.707106 -0.707106"/>
    <limit effort="3.348" lower="-1.14529" upper="0.740718" velocity="4.16174"/>
  </joint>
  <link name="LPelvis">
    <inertial>
      <mass value="0.06981"/>
      <inertia ixx="2.3407e-05" ixy="1.07922e-06" ixz="-1.76003e-06" iyy="4.76355e-05" iyz="2.76058e-06" izz="4.97021e-05"/>
      <origin rpy="0 0 0" xyz="-0.00781 -0.01114 0.02661"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/LHipYawPitch_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="Orange"/>
</visual>
    <collision>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/LHipYawPitch_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="LHipRoll" type="revolute">
    <parent link="LPelvis"/>
    <child link="LHip"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <axis xyz="1.0 0 0"/>
    <limit effort="3.348" lower="-0.379435" upper="0.79046" velocity="4.16174"/>
  </joint>
  <link name="LHip">
    <inertial>
      <mass value="0.14053"/>
      <inertia ixx="2.38445e-05" ixy="-6.53605e-07" ixz="7.12893e-06" iyy="6.08246e-05" iyz="-2.14072e-07" izz="5.43691e-05"/>
      <origin rpy="0 0 0" xyz="-0.01549 0.00029 -0.00515"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/LHipRoll_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="Orange"/>
</visual>
    <collision>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/LHipRoll_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="LHipPitch" type="revolute">
    <parent link="LHip"/>
    <child link="LThigh"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <axis xyz="0 1.0 0"/>
    <limit effort="3.023" lower="-1.53589" upper="0.48398" velocity="6.40239"/>
  </joint>
  <link name="LThigh">
    <inertial>
      <mass value="0.38968"/>
      <inertia ixx="0.000509844" ixy="2.11296e-06" ixz="5.64129e-05" iyy="0.000465358" iyz="-7.91029e-06" izz="0.000301098"/>
      <origin rpy="0 0 0" xyz="0.00138 0.00221 -0.05373"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/LHipPitch_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="c2"/>
</visual>
    <collision>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/LHipPitch_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="LKneePitch" type="revolute">
    <parent link="LThigh"/>
    <child link="LTibia"/>
    <origin rpy="0 0 0" xyz="0 0 -0.1"/>
    <axis xyz="0 1.0 0"/>
    <limit effort="3.023" lower="-0.0923279" upper="2.11255" velocity="6.40239"/>
  </joint>
  <link name="LTibia">
    <inertial>
      <mass value="0.30142"/>
      <inertia ixx="0.000446171" ixy="3.70584e-06" ixz="-3.09008e-05" iyy="0.000388084" iyz="6.01953e-06" izz="0.000185516"/>
      <origin rpy="0 0 0" xyz="0.00453 0.00225 -0.04936"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/LKneePitch_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="c3"/>
</visual>
    <collision>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/LKneePitch_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="LAnklePitch" type="revolute">
    <parent link="LTibia"/>
    <child link="LAnklePitch"/>
    <origin rpy="0 0 0" xyz="0 0 -0.1029"/>
    <axis xyz="0 1.0 0"/>
    <limit effort="3.023" lower="-1.18944" upper="0.922581" velocity="6.40239"/>
  </joint>
  <link name="LAnklePitch">
    <inertial>
      <mass value="0.13416"/>
      <inertia ixx="3.22034e-05" ixy="-8.83212e-09" ixz="4.27549e-06" iyy="6.7943e-05" iyz="2.84849e-07" izz="5.48269e-05"/>
      <origin rpy="0 0 0" xyz="0.00045 0.00029 0.00685"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/LAnklePitch_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="Blue"/>
</visual>
    <collision>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/LAnklePitch_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="LAnkleRoll" type="revolute">
    <parent link="LAnklePitch"/>
    <child link="l_ankle"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <axis xyz="1.0 0 0"/>
    <limit effort="3.348" lower="-0.397761" upper="0.768992" velocity="4.16174"/>
  </joint>
  <link name="l_ankle">
    <inertial>
      <mass value="0.17184"/>
      <inertia ixx="8.72911e-05" ixy="8.71925e-06" ixz="-2.10563e-06" iyy="0.000353024" iyz="3.73459e-07" izz="0.000412846"/>
      <origin rpy="0 0 0" xyz="0.02542 0.0033 -0.03239"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/LAnkleRoll_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="Blue"/>
</visual>
    <collision>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/LAnkleRoll_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="LLeg_effector_fixedjoint" type="fixed">
    <parent link="l_ankle"/>
    <child link="l_sole"/>
    <origin rpy="0 0 0" xyz="0 0 -0.04511"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="l_sole"/>
  <joint name="RHipYawPitch" type="revolute">
    <parent link="torso"/>
    <child link="RPelvis"/>
    <origin rpy="0 0 0" xyz="0 -0.05 -0.085"/>
    <axis xyz="0 0.707106 0.707106"/>
    <limit effort="3.348" lower="-1.14529" upper="0.740718" velocity="4.16174"/>
    <mimic joint="LHipYawPitch" multiplier="1.0" offset="0"/>
  </joint>
  <link name="RPelvis">
    <inertial>
      <mass value="0.06981"/>
      <inertia ixx="3.18766e-05" ixy="-1.07152e-06" ixz="-1.77295e-06" iyy="5.18361e-05" iyz="-7.00664e-06" izz="5.39657e-05"/>
      <origin rpy="0 0 0" xyz="-0.00781 0.01114 0.02661"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/RHipYawPitch_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="c4"/>
</visual>
    <collision>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/RHipYawPitch_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="RHipRoll" type="revolute">
    <parent link="RPelvis"/>
    <child link="RHip"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <axis xyz="1.0 0 0"/>
    <limit effort="3.348" lower="-0.79046" upper="0.379435" velocity="4.16174"/>
  </joint>
  <link name="RHip">
    <inertial>
      <mass value="0.14053"/>
      <inertia ixx="2.38475e-05" ixy="6.12085e-07" ixz="7.10235e-06" iyy="6.0824e-05" iyz="2.12392e-07" izz="5.43727e-05"/>
      <origin rpy="0 0 0" xyz="-0.01549 -0.00029 -0.00515"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/RHipRoll_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="c4"/>
</visual>
    <collision>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/RHipRoll_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="RHipPitch" type="revolute">
    <parent link="RHip"/>
    <child link="RThigh"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <axis xyz="0 1.0 0"/>
    <limit effort="3.0226" lower="-1.53589" upper="0.48398" velocity="6.40239"/>
  </joint>
  <link name="RThigh">
    <inertial>
      <mass value="0.38968"/>
      <inertia ixx="0.000510607" ixy="-2.02799e-06" ixz="5.69893e-05" iyy="0.0004665" iyz="7.09563e-06" izz="0.000301333"/>
      <origin rpy="0 0 0" xyz="0.00138 -0.00221 -0.05373"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/RHipPitch_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="c5"/>
</visual>
    <collision>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/RHipPitch_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="RKneePitch" type="revolute">
    <parent link="RThigh"/>
    <child link="RTibia"/>
    <origin rpy="0 0 0" xyz="0 0 -0.1"/>
    <axis xyz="0 1.0 0"/>
    <limit effort="3.0226" lower="-0.0923279" upper="2.11255" velocity="6.40239"/>
  </joint>
  <link name="RTibia">
    <inertial>
      <mass value="0.30142"/>
      <inertia ixx="0.000446921" ixy="-3.96872e-06" ixz="-3.94009e-05" iyy="0.000387711" iyz="-5.00034e-06" izz="0.000183741"/>
      <origin rpy="0 0 0" xyz="0.00453 -0.00225 -0.04936"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/RKneePitch_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="c5"/>
</visual>
    <collision>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/RKneePitch_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="RAnklePitch" type="revolute">
    <parent link="RTibia"/>
    <child link="RAnklePitch"/>
    <origin rpy="0 0 0" xyz="0 0 -0.1029"/>
    <axis xyz="0 1.0 0"/>
    <limit effort="3.0226" lower="-1.1863" upper="0.932006" velocity="6.40239"/>
  </joint>
  <link name="RAnklePitch">
    <inertial>
      <mass value="0.13416"/>
      <inertia ixx="3.22017e-05" ixy="4.68321e-08" ixz="4.28821e-06" iyy="6.79885e-05" iyz="-2.71089e-07" izz="5.48747e-05"/>
      <origin rpy="0 0 0" xyz="0.00045 -0.00029 0.00685"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/RAnklePitch_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="c6"/>
</visual>
    <collision>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/RAnklePitch_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="RAnkleRoll" type="revolute">
    <parent link="RAnklePitch"/>
    <child link="r_ankle"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <axis xyz="1.0 0 0"/>
    <limit effort="3.348" lower="-0.768992" upper="0.397761" velocity="4.16174"/>
  </joint>
  <link name="r_ankle">
    <inertial>
      <mass value="0.17184"/>
      <inertia ixx="8.71513e-05" ixy="-8.53992e-06" ixz="-2.35184e-06" iyy="0.000352156" iyz="-4.81708e-07" izz="0.000412125"/>
      <origin rpy="0 0 0" xyz="0.02542 -0.0033 -0.03239"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/RAnkleRoll_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="c6"/>
</visual>
    <collision>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/RAnkleRoll_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="RLeg_effector_fixedjoint" type="fixed">
    <parent link="r_ankle"/>
    <child link="r_sole"/>
    <origin rpy="0 0 0" xyz="0 0 -0.04511"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="r_sole"/>
  <link name="base_link"/>
  <joint name="base_link_fixedjoint" type="fixed">
    <parent link="base_link"/>
    <child link="torso"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="torso">
    <inertial>
      <mass value="1.04956"/>
      <inertia ixx="0.00308361" ixy="1.43116e-05" ixz="-3.30211e-05" iyy="0.0028835" iyz="-2.70793e-05" izz="0.0015924"/>
      <origin rpy="0 0 0" xyz="-0.00413 0 0.04342"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/Torso_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="c7"/>
</visual>
    <collision>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/Torso_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="LShoulderPitch" type="revolute">
    <parent link="torso"/>
    <child link="LShoulder"/>
    <origin rpy="0 0 0" xyz="0 0.098 0.1"/>
    <axis xyz="0 1.0 0"/>
    <limit effort="1.329" lower="-2.08567" upper="2.08567" velocity="8.26797"/>
  </joint>
  <link name="LShoulder">
    <inertial>
      <mass value="0.09304"/>
      <inertia ixx="1.83025e-05" ixy="2.06011e-06" ixz="1.88776e-09" iyy="1.39005e-05" iyz="-3.66592e-07" izz="2.01862e-05"/>
      <origin rpy="0 0 0" xyz="-0.00165 -0.02663 0.00014"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/LShoulderPitch_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="c8"/>
</visual>
    <collision>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/LShoulderPitch_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="LShoulderRoll" type="revolute">
    <parent link="LShoulder"/>
    <child link="LBicep"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <axis xyz="0 0 1.0"/>
    <limit effort="1.7835" lower="-0.314159" upper="1.32645" velocity="7.19407"/>
  </joint>
  <link name="LBicep">
    <inertial>
      <mass value="0.15777"/>
      <inertia ixx="8.7181e-05" ixy="-2.53381e-05" ixz="-1.4213e-05" iyy="0.000274712" iyz="4.71439e-07" izz="0.000241812"/>
      <origin rpy="0 0 0" xyz="0.02455 0.00563 0.0033"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/LShoulderRoll_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="c9"/>
</visual>
    <collision>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/LShoulderRoll_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="LElbowYaw" type="revolute">
    <parent link="LBicep"/>
    <child link="LElbow"/>
    <origin rpy="0 0 0" xyz="0.105 0.015 0"/>
    <axis xyz="1.0 0 0"/>
    <limit effort="1.547" lower="-2.08567" upper="2.08567" velocity="8.26797"/>
  </joint>
  <link name="LElbow">
    <inertial>
      <mass value="0.06483"/>
      <inertia ixx="5.59588e-06" ixy="4.21e-09" ixz="2.92241e-07" iyy="2.66179e-05" iyz="-1.84e-09" izz="2.76294e-05"/>
      <origin rpy="0 0 0" xyz="-0.02744 0 -0.00014"/>
    </inertial>
  </link>
  <joint name="LElbowRoll" type="revolute">
    <parent link="LElbow"/>
    <child link="LForeArm"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <axis xyz="0 0 1.0"/>
    <limit effort="1.532" lower="-1.54462" upper="-0.0349066" velocity="7.19407"/>
  </joint>
  <link name="LForeArm">
    <inertial>
      <mass value="0.07761"/>
      <inertia ixx="2.46746e-05" ixy="3.23152e-06" ixz="1.58221e-06" iyy="3.83837e-05" iyz="1.39194e-07" izz="3.59708e-05"/>
      <origin rpy="0 0 0" xyz="0.02556 0.00281 0.00076"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/LElbowRoll_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="c1"/>
</visual>
    <collision>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/LElbowRoll_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="LWristYaw" type="revolute">
    <parent link="LForeArm"/>
    <child link="l_wrist"/>
    <origin rpy="0 0 0" xyz="0.05595 0 0"/>
    <axis xyz="1.0 0 0"/>
    <limit effort="0.4075" lower="-1.82387" upper="1.82387" velocity="24.6229"/>
  </joint>
  <link name="l_wrist">
    <inertial>
      <mass value="0.18533"/>
      <inertia ixx="6.86477e-05" ixy="1.15465e-07" ixz="-2.87254e-06" iyy="0.000135756" iyz="2.67539e-06" izz="0.000133228"/>
      <origin rpy="0 0 0" xyz="0.03434 -0.00088 0.00308"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/LWristYaw_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="c2"/>
</visual>
    <collision>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/LWristYaw_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="LHand" type="revolute">
    <parent link="l_wrist"/>
    <child link="l_gripper"/>
    <origin rpy="0 0 0" xyz="0.05775 0 -0.01231"/>
    <axis xyz="1.0 0 0"/>
    <limit effort="0.292" lower="0" upper="1.0" velocity="8.33"/>
  </joint>
  <link name="l_gripper">
    <inertial>
      <mass value="2e-06"/>
      <inertia ixx="1.1e-09" ixy="0" ixz="0" iyy="1.1e-09" iyz="0" izz="1.1e-09"/>
    </inertial>
  </link>
  <joint name="RShoulderPitch" type="revolute">
    <parent link="torso"/>
    <child link="RShoulder"/>
    <origin rpy="0 0 0" xyz="0 -0.098 0.1"/>
    <axis xyz="0 1.0 0"/>
    <limit effort="1.329" lower="-2.08567" upper="2.08567" velocity="8.26797"/>
  </joint>
  <link name="RShoulder">
    <inertial>
      <mass value="0.09304"/>
      <inertia ixx="1.83025e-05" ixy="-2.06011e-06" ixz="1.88776e-09" iyy="1.39005e-05" iyz="3.66592e-07" izz="2.01862e-05"/>
      <origin rpy="0 0 0" xyz="-0.00165 0.02663 0.00014"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/RShoulderPitch_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="c3"/>
</visual>
    <collision>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/RShoulderPitch_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="RShoulderRoll" type="revolute">
    <parent link="RShoulder"/>
    <child link="RBicep"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <axis xyz="0 0 1.0"/>
    <limit effort="1.783" lower="-1.32645" upper="0.314159" velocity="7.19407"/>
  </joint>
  <link name="RBicep">
    <inertial>
      <mass value="0.15777"/>
      <inertia ixx="0.000103401" ixy="5.48849e-05" ixz="-1.32643e-05" iyy="0.00027077" iyz="9.16707e-06" izz="0.000254529"/>
      <origin rpy="0 0 0" xyz="0.02455 -0.00563 0.0033"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/RShoulderRoll_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="c4"/>
</visual>
    <collision>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/RShoulderRoll_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="RElbowYaw" type="revolute">
    <parent link="RBicep"/>
    <child link="RElbow"/>
    <origin rpy="0 0 0" xyz="0.105 -0.015 0"/>
    <axis xyz="1.0 0 0"/>
    <limit effort="1.547" lower="-2.08567" upper="2.08567" velocity="8.26797"/>
  </joint>
  <link name="RElbow">
    <inertial>
      <mass value="0.06483"/>
      <inertia ixx="5.59588e-06" ixy="4.21e-09" ixz="2.92241e-07" iyy="2.66179e-05" iyz="-1.84e-09" izz="2.76294e-05"/>
      <origin rpy="0 0 0" xyz="-0.02744 0 -0.00014"/>
    </inertial>
  </link>
  <joint name="RElbowRoll" type="revolute">
    <parent link="RElbow"/>
    <child link="RForeArm"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <axis xyz="0 0 1.0"/>
    <limit effort="1.532" lower="0.0349066" upper="1.54462" velocity="7.19407"/>
  </joint>
  <link name="RForeArm">
    <inertial>
      <mass value="0.07761"/>
      <inertia ixx="2.47331e-05" ixy="-3.2418e-06" ixz="9.06451e-07" iyy="3.84719e-05" iyz="-1.38804e-07" izz="3.59319e-05"/>
      <origin rpy="0 0 0" xyz="0.02556 -0.00281 0.00076"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/RElbowRoll_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="c5"/>
</visual>
    <collision>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/RElbowRoll_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="RWristYaw" type="revolute">
    <parent link="RForeArm"/>
    <child link="r_wrist"/>
    <origin rpy="0 0 0" xyz="0.05595 0 0"/>
    <axis xyz="1.0 0 0"/>
    <limit effort="0.4075" lower="-1.82387" upper="1.82387" velocity="24.6229"/>
  </joint>
  <link name="r_wrist">
    <inertial>
      <mass value="0.18533"/>
      <inertia ixx="6.86477e-05" ixy="1.13165e-05" ixz="-2.87254e-06" iyy="0.000135756" iyz="3.68003e-06" izz="0.000133228"/>
      <origin rpy="0 0 0" xyz="0.03434 0.00088 0.00308"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/RWristYaw_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="c6"/>
</visual>
    <collision>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/RWristYaw_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="RHand" type="revolute">
    <parent link="r_wrist"/>
    <child link="r_gripper"/>
    <origin rpy="0 0 0" xyz="0.05775 0 -0.01213"/>
    <axis xyz="1.0 0 0"/>
    <limit effort="0.292" lower="0" upper="1.0" velocity="8.33"/>
  </joint>
  <link name="r_gripper">
    <inertial>
      <mass value="2e-06"/>
      <inertia ixx="1.1e-09" ixy="0" ixz="0" iyy="1.1e-09" iyz="0" izz="1.1e-09"/>
    </inertial>
  </link>
  <link name="LFootBumperRight_frame"/>
  <joint name="LFoot/Bumper/Right_sensor_fixedjoint" type="fixed">
    <parent link="l_ankle"/>
    <child link="LFootBumperRight_frame"/>
    <origin rpy="0 0 0" xyz="0.0876 -0.014 -0.0353"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="RFsrRL_frame">
    <collision>
      <geometry>
        <box size="0.01 0.01 0.005"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
    <inertial>
      <mass value="2e-06"/>
      <inertia ixx="1.1e-09" ixy="0" ixz="0" iyy="1.1e-09" iyz="0" izz="1.1e-09"/>
    </inertial>
  </link>
  <joint name="RFoot/FSR/RearLeft_sensor_fixedjoint" type="fixed">
    <parent link="r_ankle"/>
    <child link="RFsrRL_frame"/>
    <origin rpy="0 0 0" xyz="-0.03025 0.0191 -0.04511"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="CameraBottom_optical_frame"/>
  <joint name="CameraBottom_optical_frame_fixedjoint" type="fixed">
    <parent link="CameraBottom_frame"/>
    <child link="CameraBottom_optical_frame"/>
    <origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 0 0"/>
  </joint>
  <link name="RFsrRR_frame">
    <collision>
      <geometry>
        <box size="0.01 0.01 0.005"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
    <inertial>
      <mass value="2e-06"/>
      <inertia ixx="1.1e-09" ixy="0" ixz="0" iyy="1.1e-09" iyz="0" izz="1.1e-09"/>
    </inertial>
  </link>
  <joint name="RFoot/FSR/RearRight_sensor_fixedjoint" type="fixed">
    <parent link="r_ankle"/>
    <child link="RFsrRR_frame"/>
    <origin rpy="0 0 0" xyz="-0.02965 -0.0299 -0.04511"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="LFsrFR_frame">
    <collision>
      <geometry>
        <box size="0.01 0.01 0.005"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
    <inertial>
      <mass value="2e-06"/>
      <inertia ixx="1.1e-09" ixy="0" ixz="0" iyy="1.1e-09" iyz="0" izz="1.1e-09"/>
    </inertial>
  </link>
  <joint name="LFoot/FSR/FrontRight_sensor_fixedjoint" type="fixed">
    <parent link="l_ankle"/>
    <child link="LFsrFR_frame"/>
    <origin rpy="0 0 0" xyz="0.07025 -0.0231 -0.04511"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="LHandTouchLeft_frame"/>
  <joint name="LHand/Touch/Left_sensor_fixedjoint" type="fixed">
    <parent link="l_wrist"/>
    <child link="LHandTouchLeft_frame"/>
    <origin rpy="0 0 1.5708" xyz="0.032 0.025 0.003"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="RFootBumperRight_frame"/>
  <joint name="RFoot/Bumper/Right_sensor_fixedjoint" type="fixed">
    <parent link="r_ankle"/>
    <child link="RFootBumperRight_frame"/>
    <origin rpy="0 0 0" xyz="0.0876 -0.019 -0.0353"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="RFsrFR_frame">
    <collision>
      <geometry>
        <box size="0.01 0.01 0.005"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
    <inertial>
      <mass value="2e-06"/>
      <inertia ixx="1.1e-09" ixy="0" ixz="0" iyy="1.1e-09" iyz="0" izz="1.1e-09"/>
    </inertial>
  </link>
  <joint name="RFoot/FSR/FrontRight_sensor_fixedjoint" type="fixed">
    <parent link="r_ankle"/>
    <child link="RFsrFR_frame"/>
    <origin rpy="0 0 0" xyz="0.07025 -0.0299 -0.04511"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="LFsrRR_frame">
    <collision>
      <geometry>
        <box size="0.01 0.01 0.005"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
    <inertial>
      <mass value="2e-06"/>
      <inertia ixx="1.1e-09" ixy="0" ixz="0" iyy="1.1e-09" iyz="0" izz="1.1e-09"/>
    </inertial>
  </link>
  <joint name="LFoot/FSR/RearRight_sensor_fixedjoint" type="fixed">
    <parent link="l_ankle"/>
    <child link="LFsrRR_frame"/>
    <origin rpy="0 0 0" xyz="-0.02965 -0.0191 -0.04511"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="ImuTorsoAccelerometer_frame"/>
  <joint name="Accelerometer_sensor_fixedjoint" type="fixed">
    <parent link="torso"/>
    <child link="ImuTorsoAccelerometer_frame"/>
    <origin rpy="0 0 0" xyz="-0.008 0.00606 0.027"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="HeadTouchFront_frame"/>
  <joint name="Head/Touch/Front_sensor_fixedjoint" type="fixed">
    <parent link="Head"/>
    <child link="HeadTouchFront_frame"/>
    <origin rpy="0 -1.1861 0" xyz="0.0312 0 0.1014"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="RFootBumperLeft_frame"/>
  <joint name="RFoot/Bumper/Left_sensor_fixedjoint" type="fixed">
    <parent link="r_ankle"/>
    <child link="RFootBumperLeft_frame"/>
    <origin rpy="0 0 0" xyz="0.0876 0.014 -0.0353"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="RSonar_frame"/>
  <joint name="Sonar/Right_sensor_fixedjoint" type="fixed">
    <parent link="torso"/>
    <child link="RSonar_frame"/>
    <origin rpy="0.00478508 0.0436497 -0.392441" xyz="0.00558253 -0.0192068 0.0623853"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="RHandTouchRight_frame"/>
  <joint name="RHand/Touch/Right_sensor_fixedjoint" type="fixed">
    <parent link="r_wrist"/>
    <child link="RHandTouchRight_frame"/>
    <origin rpy="0 0 -1.5708" xyz="0.035 -0.025 0.0035"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="RHandTouchBack_frame"/>
  <joint name="RHand/Touch/Back_sensor_fixedjoint" type="fixed">
    <parent link="r_wrist"/>
    <child link="RHandTouchBack_frame"/>
    <origin rpy="3.14159 -1.5708 3.14159" xyz="0.038 0 0.025"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="RFsrFL_frame">
    <collision>
      <geometry>
        <box size="0.01 0.01 0.005"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
    <inertial>
      <mass value="2e-06"/>
      <inertia ixx="1.1e-09" ixy="0" ixz="0" iyy="1.1e-09" iyz="0" izz="1.1e-09"/>
    </inertial>
  </link>
  <joint name="RFoot/FSR/FrontLeft_sensor_fixedjoint" type="fixed">
    <parent link="r_ankle"/>
    <child link="RFsrFL_frame"/>
    <origin rpy="0 0 0" xyz="0.07025 0.0231 -0.04511"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="LSonar_frame"/>
  <joint name="Sonar/Left_sensor_fixedjoint" type="fixed">
    <parent link="torso"/>
    <child link="LSonar_frame"/>
    <origin rpy="-0.00478508 0.0436497 0.392441" xyz="0.00558253 0.0192068 0.0623853"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="LFsrFL_frame">
    <collision>
      <geometry>
        <box size="0.01 0.01 0.005"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
    <inertial>
      <mass value="2e-06"/>
      <inertia ixx="1.1e-09" ixy="0" ixz="0" iyy="1.1e-09" iyz="0" izz="1.1e-09"/>
    </inertial>
  </link>
  <joint name="LFoot/FSR/FrontLeft_sensor_fixedjoint" type="fixed">
    <parent link="l_ankle"/>
    <child link="LFsrFL_frame"/>
    <origin rpy="0 0 0" xyz="0.07025 0.0299 -0.04511"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="LFsrRL_frame">
    <collision>
      <geometry>
        <box size="0.01 0.01 0.005"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
    <inertial>
      <mass value="2e-06"/>
      <inertia ixx="1.1e-09" ixy="0" ixz="0" iyy="1.1e-09" iyz="0" izz="1.1e-09"/>
    </inertial>
  </link>
  <joint name="LFoot/FSR/RearLeft_sensor_fixedjoint" type="fixed">
    <parent link="l_ankle"/>
    <child link="LFsrRL_frame"/>
    <origin rpy="0 0 0" xyz="-0.03025 0.0299 -0.04511"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="RHandTouchLeft_frame"/>
  <joint name="RHand/Touch/Left_sensor_fixedjoint" type="fixed">
    <parent link="r_wrist"/>
    <child link="RHandTouchLeft_frame"/>
    <origin rpy="0 0 1.5708" xyz="0.032 0.025 0.003"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="CameraBottom_frame"/>
  <joint name="CameraBottom_sensor_fixedjoint" type="fixed">
    <parent link="Head"/>
    <child link="CameraBottom_frame"/>
    <origin rpy="0 0.692896 0" xyz="0.05071 0 0.01774"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="CameraTop_frame"/>
  <joint name="CameraTop_sensor_fixedjoint" type="fixed">
    <parent link="Head"/>
    <child link="CameraTop_frame"/>
    <origin rpy="0 0.0209435 0" xyz="0.05871 0 0.06364"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="CameraTop_optical_frame"/>
  <joint name="CameraTop_optical_frame_fixedjoint" type="fixed">
    <parent link="CameraTop_frame"/>
    <child link="CameraTop_optical_frame"/>
    <origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 0 0"/>
  </joint>
  <link name="LHandTouchBack_frame"/>
  <joint name="LHand/Touch/Back_sensor_fixedjoint" type="fixed">
    <parent link="l_wrist"/>
    <child link="LHandTouchBack_frame"/>
    <origin rpy="3.14159 -1.5708 3.14159" xyz="0.038 0 0.025"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="LInfraRed_frame"/>
  <joint name="InfraredL_sensor_fixedjoint" type="fixed">
    <parent link="Head"/>
    <child link="LInfraRed_frame"/>
    <origin rpy="0 0 0" xyz="0.0441 0.0257 0.0466"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="ImuTorsoGyrometer_frame"/>
  <joint name="Gyrometer_sensor_fixedjoint" type="fixed">
    <parent link="torso"/>
    <child link="ImuTorsoGyrometer_frame"/>
    <origin rpy="0 0 0" xyz="-0.008 0.006 0.029"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="HeadTouchRear_frame"/>
  <joint name="Head/Touch/Rear_sensor_fixedjoint" type="fixed">
    <parent link="Head"/>
    <child link="HeadTouchRear_frame"/>
    <origin rpy="3.14159 -1.33319 -3.14159" xyz="-0.0257 0 0.1045"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="HeadTouchMiddle_frame"/>
  <joint name="Head/Touch/Middle_sensor_fixedjoint" type="fixed">
    <parent link="Head"/>
    <child link="HeadTouchMiddle_frame"/>
    <origin rpy="3.14159 -1.57079 -3.14159" xyz="0.001 0 0.1099"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="RInfraRed_frame"/>
  <joint name="InfraredR_sensor_fixedjoint" type="fixed">
    <parent link="Head"/>
    <child link="RInfraRed_frame"/>
    <origin rpy="0 0 0" xyz="0.0441 -0.0257 0.0466"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="ChestButton_frame"/>
  <joint name="ChestBoard/Button_sensor_fixedjoint" type="fixed">
    <parent link="torso"/>
    <child link="ChestButton_frame"/>
    <origin rpy="0 0 0" xyz="0.049871 0.001588 0.055163"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="LFootBumperLeft_frame"/>
  <joint name="LFoot/Bumper/Left_sensor_fixedjoint" type="fixed">
    <parent link="l_ankle"/>
    <child link="LFootBumperLeft_frame"/>
    <origin rpy="0 0 0" xyz="0.0876 0.019 -0.0353"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="LHandTouchRight_frame"/>
  <joint name="LHand/Touch/Right_sensor_fixedjoint" type="fixed">
    <parent link="l_wrist"/>
    <child link="LHandTouchRight_frame"/>
    <origin rpy="0 0 -1.5708" xyz="0.035 -0.025 0.0035"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="RFinger13_link">
    <inertial>
      <mass value="2e-06"/>
      <inertia ixx="1.1e-09" ixy="0" ixz="0" iyy="1.1e-09" iyz="0" izz="1.1e-09"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/RFinger13_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="Grey"/>
</visual>
    <collision>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/RFinger13_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="RFinger13" type="continuous">
    <parent link="RFinger12_link"/>
    <child link="RFinger13_link"/>
    <origin rpy="0 0 -0.999899" xyz="0.01436 0 0"/>
    <axis xyz="0 0 1.0"/>
    <mimic joint="RHand" multiplier="0.999899" offset="0"/>
  </joint>
  <link name="RFinger12_link">
    <inertial>
      <mass value="2e-06"/>
      <inertia ixx="1.1e-09" ixy="0" ixz="0" iyy="1.1e-09" iyz="0" izz="1.1e-09"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/RFinger12_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="Grey"/>
</visual>
    <collision>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/RFinger12_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="RFinger12" type="continuous">
    <parent link="RFinger11_link"/>
    <child link="RFinger12_link"/>
    <origin rpy="0 0 -0.999899" xyz="0.01436 0 0"/>
    <axis xyz="0 0 1.0"/>
    <mimic joint="RHand" multiplier="0.999899" offset="0"/>
  </joint>
  <link name="LFinger21_link">
    <inertial>
      <mass value="2e-06"/>
      <inertia ixx="1.1e-09" ixy="0" ixz="0" iyy="1.1e-09" iyz="0" izz="1.1e-09"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/LFinger21_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="Grey"/>
</visual>
    <collision>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/LFinger21_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="LFinger21" type="continuous">
    <parent link="l_wrist"/>
    <child link="LFinger21_link"/>
    <origin rpy="1.25539 0.976662 -0.264067" xyz="0.06907 -0.01157 -0.00304"/>
    <axis xyz="0 0 1.0"/>
    <mimic joint="LHand" multiplier="0.999899" offset="0"/>
  </joint>
  <link name="LFinger13_link">
    <inertial>
      <mass value="2e-06"/>
      <inertia ixx="1.1e-09" ixy="0" ixz="0" iyy="1.1e-09" iyz="0" izz="1.1e-09"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/LFinger13_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="Grey"/>
</visual>
    <collision>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/LFinger13_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="LFinger13" type="continuous">
    <parent link="LFinger12_link"/>
    <child link="LFinger13_link"/>
    <origin rpy="0 0 -0.999899" xyz="0.01436 0 0"/>
    <axis xyz="0 0 1.0"/>
    <mimic joint="LHand" multiplier="0.999899" offset="0"/>
  </joint>
  <link name="LFinger11_link">
    <inertial>
      <mass value="2e-06"/>
      <inertia ixx="1.1e-09" ixy="0" ixz="0" iyy="1.1e-09" iyz="0" izz="1.1e-09"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/LFinger11_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="Grey"/>
</visual>
    <collision>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/LFinger11_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="LFinger11" type="continuous">
    <parent link="l_wrist"/>
    <child link="LFinger11_link"/>
    <origin rpy="1.8862 0.976662 0.264067" xyz="0.06907 0.01157 -0.00304"/>
    <axis xyz="0 0 1.0"/>
    <mimic joint="LHand" multiplier="0.999899" offset="0"/>
  </joint>
  <link name="RFinger22_link">
    <inertial>
      <mass value="2e-06"/>
      <inertia ixx="1.1e-09" ixy="0" ixz="0" iyy="1.1e-09" iyz="0" izz="1.1e-09"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/RFinger22_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="Grey"/>
</visual>
    <collision>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/RFinger22_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="RFinger22" type="continuous">
    <parent link="RFinger21_link"/>
    <child link="RFinger22_link"/>
    <origin rpy="0 0 -0.999899" xyz="0.01436 0 0"/>
    <axis xyz="0 0 1.0"/>
    <mimic joint="RHand" multiplier="0.999899" offset="0"/>
  </joint>
  <link name="LFinger22_link">
    <inertial>
      <mass value="2e-06"/>
      <inertia ixx="1.1e-09" ixy="0" ixz="0" iyy="1.1e-09" iyz="0" izz="1.1e-09"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/LFinger22_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="Grey"/>
</visual>
    <collision>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/LFinger22_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="LFinger22" type="continuous">
    <parent link="LFinger21_link"/>
    <child link="LFinger22_link"/>
    <origin rpy="0 0 -0.999899" xyz="0.01436 0 0"/>
    <axis xyz="0 0 1.0"/>
    <mimic joint="LHand" multiplier="0.999899" offset="0"/>
  </joint>
  <link name="RFinger21_link">
    <inertial>
      <mass value="2e-06"/>
      <inertia ixx="1.1e-09" ixy="0" ixz="0" iyy="1.1e-09" iyz="0" izz="1.1e-09"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/RFinger21_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="Grey"/>
</visual>
    <collision>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/RFinger21_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="RFinger21" type="continuous">
    <parent link="r_wrist"/>
    <child link="RFinger21_link"/>
    <origin rpy="1.25539 0.976662 -0.264067" xyz="0.06907 -0.01157 -0.00304"/>
    <axis xyz="0 0 1.0"/>
    <mimic joint="RHand" multiplier="0.999899" offset="0"/>
  </joint>
  <link name="LFinger12_link">
    <inertial>
      <mass value="2e-06"/>
      <inertia ixx="1.1e-09" ixy="0" ixz="0" iyy="1.1e-09" iyz="0" izz="1.1e-09"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/LFinger12_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="Grey"/>
</visual>
    <collision>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/LFinger12_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="LFinger12" type="continuous">
    <parent link="LFinger11_link"/>
    <child link="LFinger12_link"/>
    <origin rpy="0 0 -0.999899" xyz="0.01436 0 0"/>
    <axis xyz="0 0 1.0"/>
    <mimic joint="LHand" multiplier="0.999899" offset="0"/>
  </joint>
  <link name="RFinger23_link">
    <inertial>
      <mass value="2e-06"/>
      <inertia ixx="1.1e-09" ixy="0" ixz="0" iyy="1.1e-09" iyz="0" izz="1.1e-09"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/RFinger23_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="Grey"/>
</visual>
    <collision>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/RFinger23_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="RFinger23" type="continuous">
    <parent link="RFinger22_link"/>
    <child link="RFinger23_link"/>
    <origin rpy="0 0 -0.999899" xyz="0.01436 0 0"/>
    <axis xyz="0 0 1.0"/>
    <mimic joint="RHand" multiplier="0.999899" offset="0"/>
  </joint>
  <link name="RFinger11_link">
    <inertial>
      <mass value="2e-06"/>
      <inertia ixx="1.1e-09" ixy="0" ixz="0" iyy="1.1e-09" iyz="0" izz="1.1e-09"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/RFinger11_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="Grey"/>
</visual>
    <collision>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/RFinger11_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="RFinger11" type="continuous">
    <parent link="r_wrist"/>
    <child link="RFinger11_link"/>
    <origin rpy="1.8862 0.976662 0.264067" xyz="0.06907 0.01157 -0.00304"/>
    <axis xyz="0 0 1.0"/>
    <mimic joint="RHand" multiplier="0.999899" offset="0"/>
  </joint>
  <link name="LFinger23_link">
    <inertial>
      <mass value="2e-06"/>
      <inertia ixx="1.1e-09" ixy="0" ixz="0" iyy="1.1e-09" iyz="0" izz="1.1e-09"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/LFinger23_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="Grey"/>
</visual>
    <collision>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/LFinger23_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="LFinger23" type="continuous">
    <parent link="LFinger22_link"/>
    <child link="LFinger23_link"/>
    <origin rpy="0 0 -0.999899" xyz="0.01436 0 0"/>
    <axis xyz="0 0 1.0"/>
    <mimic joint="LHand" multiplier="0.999899" offset="0"/>
  </joint>
  <link name="LThumb1_link">
    <inertial>
      <mass value="2e-06"/>
      <inertia ixx="1.1e-09" ixy="0" ixz="0" iyy="1.1e-09" iyz="0" izz="1.1e-09"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/LThumb1_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="Grey"/>
</visual>
    <collision>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/LThumb1_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="LThumb1" type="continuous">
    <parent link="l_wrist"/>
    <child link="LThumb1_link"/>
    <origin rpy="-1.5708 0.0472984 -3.26826e-08" xyz="0.04895 0 -0.02638"/>
    <axis xyz="0 0 1.0"/>
    <mimic joint="LHand" multiplier="0.999899" offset="0"/>
  </joint>
  <link name="RThumb1_link">
    <inertial>
      <mass value="2e-06"/>
      <inertia ixx="1.1e-09" ixy="0" ixz="0" iyy="1.1e-09" iyz="0" izz="1.1e-09"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/RThumb1_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="Grey"/>
</visual>
    <collision>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/RThumb1_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="RThumb1" type="continuous">
    <parent link="r_wrist"/>
    <child link="RThumb1_link"/>
    <origin rpy="-1.5708 0.0472984 -3.26826e-08" xyz="0.04895 0 -0.02638"/>
    <axis xyz="0 0 1.0"/>
    <mimic joint="RHand" multiplier="0.999899" offset="0"/>
  </joint>
  <link name="RThumb2_link">
    <inertial>
      <mass value="2e-06"/>
      <inertia ixx="1.1e-09" ixy="0" ixz="0" iyy="1.1e-09" iyz="0" izz="1.1e-09"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/RThumb2_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="Grey"/>
</visual>
    <collision>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/RThumb2_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="RThumb2" type="continuous">
    <parent link="RThumb1_link"/>
    <child link="RThumb2_link"/>
    <origin rpy="0 0 -0.999899" xyz="0.01436 0 0"/>
    <axis xyz="0 0 1.0"/>
    <mimic joint="RHand" multiplier="0.999899" offset="0"/>
  </joint>
  <link name="LThumb2_link">
    <inertial>
      <mass value="2e-06"/>
      <inertia ixx="1.1e-09" ixy="0" ixz="0" iyy="1.1e-09" iyz="0" izz="1.1e-09"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/LThumb2_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <material name="Grey"/>
</visual>
    <collision>
      <geometry>
        <mesh filename="nao_meshes/meshes/V40/LThumb2_0.10.stl" scale="0.1 0.1 0.1"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0"/>
    </collision>
  </link>
  <joint name="LThumb2" type="continuous">
    <parent link="LThumb1_link"/>
    <child link="LThumb2_link"/>
    <origin rpy="0 0 -0.999899" xyz="0.01436 0 0"/>
    <axis xyz="0 0 1.0"/>
    <mimic joint="LHand" multiplier="0.999899" offset="0"/>
  </joint>
  <!-- GAZEBO ROS CONTROL AND IMU PLUGIN -->
  <gazebo>
    <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
      <robotNamespace>/nao_dcm</robotNamespace>
      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
    </plugin>
    <!--        <plugin filename="libgazebo_ros_imu.so" name="gazebo_ros_imu_controller">
            <robotNamespace>/nao_dcm</robotNamespace>
            <topicName>imu_data</topicName>
            <bodyName>imu</bodyName>
            <frameId>ImuAccelerometer_frame</frameId>
            <gaussianNoise>2.89e-08</gaussianNoise>
            <accelGaussianNoise>2.89e-08 2.89e-08 2.89e-08</accelGaussianNoise>
            <rateGaussianNoise>2.89e-08 2.89e-08 2.89e-08</rateGaussianNoise>
            <headingGaussianNoise>2.89e-08 2.89e-08 2.89e-08</headingGaussianNoise>
            <xyzOffsets>0 0 0</xyzOffsets>
            <rpyOffsets>0 0 0</rpyOffsets>
            <updateRate>30.0</updateRate>
            <alwaysOn>true</alwaysOn>
        </plugin>
-->
    <!--        <plugin name="gazebo_ros_imu_controller" filename="${imu_plugin}">
            <robotNamespace>/nao_dcm</robotNamespace>
            <topicName>imu_data</topicName>
            <bodyName>imu</bodyName>
            <frameId>imu</frameId>
            <gaussianNoise>2.89e-08</gaussianNoise>
            <accelGaussianNoise>2.89e-08 2.89e-08 2.89e-08</accelGaussianNoise>
            <rateGaussianNoise>2.89e-08 2.89e-08 2.89e-08</rateGaussianNoise>
            <headingGaussianNoise>2.89e-08 2.89e-08 2.89e-08</headingGaussianNoise>
            <xyzOffsets>0 0 0</xyzOffsets>
            <rpyOffsets>0 0 0</rpyOffsets>
            <updateRate>30.0</updateRate>
            <alwaysOn>true</alwaysOn>
        </plugin>
-->
    <!-- Mimic joints through plugin develop by costashatz based on implementation by Goncalo Cabrita -->
    <plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="mimic_hipyawpitch">
      <joint>LHipYawPitch</joint>
      <mimicJoint>RHipYawPitch</mimicJoint>
      <multiplier>1.0</multiplier>
      <offset>0.0</offset>
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="mimic_LFinger11">
      <joint>LHand</joint>
      <mimicJoint>LFinger11</mimicJoint>
      <multiplier>-0.785398163398</multiplier>
      <offset>0.785398163398</offset>
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="mimic_LFinger12">
      <joint>LHand</joint>
      <mimicJoint>LFinger12</mimicJoint>
      <multiplier>-0.785398163398</multiplier>
      <offset>0.785398163398</offset>
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="mimic_LFinger13">
      <joint>LHand</joint>
      <mimicJoint>LFinger13</mimicJoint>
      <multiplier>-0.785398163398</multiplier>
      <offset>0.785398163398</offset>
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="mimic_LFinger21">
      <joint>LHand</joint>
      <mimicJoint>LFinger21</mimicJoint>
      <multiplier>-0.785398163398</multiplier>
      <offset>0.785398163398</offset>
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="mimic_LFinger22">
      <joint>LHand</joint>
      <mimicJoint>LFinger22</mimicJoint>
      <multiplier>-0.785398163398</multiplier>
      <offset>0.785398163398</offset>
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="mimic_LFinger23">
      <joint>LHand</joint>
      <mimicJoint>LFinger23</mimicJoint>
      <multiplier>-0.785398163398</multiplier>
      <offset>0.785398163398</offset>
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="mimic_RFinger11">
      <joint>RHand</joint>
      <mimicJoint>RFinger11</mimicJoint>
      <multiplier>-0.785398163398</multiplier>
      <offset>0.785398163398</offset>
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="mimic_RFinger12">
      <joint>RHand</joint>
      <mimicJoint>RFinger12</mimicJoint>
      <multiplier>-0.785398163398</multiplier>
      <offset>0.785398163398</offset>
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="mimic_RFinger13">
      <joint>RHand</joint>
      <mimicJoint>RFinger13</mimicJoint>
      <multiplier>-0.785398163398</multiplier>
      <offset>0.785398163398</offset>
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="mimic_RFinger21">
      <joint>RHand</joint>
      <mimicJoint>RFinger21</mimicJoint>
      <multiplier>-0.785398163398</multiplier>
      <offset>0.785398163398</offset>
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="mimic_RFinger22">
      <joint>RHand</joint>
      <mimicJoint>RFinger22</mimicJoint>
      <multiplier>-0.785398163398</multiplier>
      <offset>0.785398163398</offset>
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="mimic_RFinger23">
      <joint>RHand</joint>
      <mimicJoint>RFinger23</mimicJoint>
      <multiplier>-0.785398163398</multiplier>
      <offset>0.785398163398</offset>
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="LThumb1">
      <joint>LHand</joint>
      <mimicJoint>LThumb1</mimicJoint>
      <multiplier>-0.785398163398</multiplier>
      <offset>0.785398163398</offset>
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="LThumb2">
      <joint>LHand</joint>
      <mimicJoint>LThumb2</mimicJoint>
      <multiplier>-0.785398163398</multiplier>
      <offset>0.785398163398</offset>
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="RThumb1">
      <joint>RHand</joint>
      <mimicJoint>RThumb1</mimicJoint>
      <multiplier>-0.785398163398</multiplier>
      <offset>0.785398163398</offset>
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_mimic_joint_plugin.so" name="RThumb2">
      <joint>RHand</joint>
      <mimicJoint>RThumb2</mimicJoint>
      <multiplier>-0.785398163398</multiplier>
      <offset>0.785398163398</offset>
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_disable_link_plugin.so" name="LFinger11_link">
      <link>LFinger11_link</link>
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_disable_link_plugin.so" name="LFinger12_link">
      <link>LFinger12_link</link>
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_disable_link_plugin.so" name="LFinger13_link">
      <link>LFinger13_link</link>
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_disable_link_plugin.so" name="LFinger21_link">
      <link>LFinger21_link</link>
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_disable_link_plugin.so" name="LFinger22_link">
      <link>LFinger22_link</link>
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_disable_link_plugin.so" name="LFinger23_link">
      <link>LFinger23_link</link>
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_disable_link_plugin.so" name="LThumb1_link">
      <link>LThumb1_link</link>
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_disable_link_plugin.so" name="LThumb2_link">
      <link>LThumb2_link</link>
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_disable_link_plugin.so" name="l_gripper">
      <link>l_gripper</link>
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_disable_link_plugin.so" name="RFinger11_link">
      <link>RFinger11_link</link>
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_disable_link_plugin.so" name="RFinger12_link">
      <link>RFinger12_link</link>
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_disable_link_plugin.so" name="RFinger13_link">
      <link>RFinger13_link</link>
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_disable_link_plugin.so" name="RFinger21_link">
      <link>RFinger21_link</link>
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_disable_link_plugin.so" name="RFinger22_link">
      <link>RFinger22_link</link>
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_disable_link_plugin.so" name="RFinger23_link">
      <link>RFinger23_link</link>
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_disable_link_plugin.so" name="RThumb1_link">
      <link>RThumb1_link</link>
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_disable_link_plugin.so" name="RThumb2_link">
      <link>RThumb2_link</link>
    </plugin>
    <plugin filename="libroboticsgroup_gazebo_disable_link_plugin.so" name="r_gripper">
      <link>r_gripper</link>
    </plugin>
  </gazebo>
  <!-- END OF GAZEBO ROS CONTROL PLUGIN -->
  <!-- GAZEBO MATERIALS -->
  <gazebo reference="torso">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <gazebo reference="Neck">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <gazebo reference="Head">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <gazebo reference="gaze">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <!--    <gazebo reference="LShoulderDummy">
        <mu1>0.5</mu1>
        <mu2>0.5</mu2>
        <selfCollide>false</selfCollide>
        <turnGravityOff>false</turnGravityOff>
    </gazebo>
    <gazebo reference="RShoulderDummy">
        <mu1>0.5</mu1>
        <mu2>0.5</mu2>
        <selfCollide>false</selfCollide>
        <turnGravityOff>false</turnGravityOff>
    </gazebo> -->
  <!-- ARMS -->
  <gazebo reference="LShoulder">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <gazebo reference="LBicep">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <gazebo reference="LElbow">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <gazebo reference="LForeArm">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <gazebo reference="l_wrist">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <gazebo reference="l_gripper">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>true</turnGravityOff>
  </gazebo>
  <gazebo reference="RShoulder">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <gazebo reference="RBicep">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <gazebo reference="RElbow">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <gazebo reference="RForeArm">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <gazebo reference="r_wrist">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <gazebo reference="r_gripper">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>true</turnGravityOff>
  </gazebo>
  <!--    <gazebo reference="LElbowDummy">
        <mu1>0.5</mu1>
        <mu2>0.5</mu2>
        <selfCollide>false</selfCollide>
        <turnGravityOff>false</turnGravityOff>
    </gazebo>
-->
  <gazebo reference="LFinger11_link">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>true</turnGravityOff>
  </gazebo>
  <gazebo reference="LFinger12_link">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>true</turnGravityOff>
  </gazebo>
  <gazebo reference="LFinger13_link">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>true</turnGravityOff>
  </gazebo>
  <gazebo reference="LFinger21_link">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>true</turnGravityOff>
  </gazebo>
  <gazebo reference="LFinger22_link">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>true</turnGravityOff>
  </gazebo>
  <gazebo reference="LFinger23_link">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>true</turnGravityOff>
  </gazebo>
  <gazebo reference="LThumb1_link">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>true</turnGravityOff>
  </gazebo>
  <gazebo reference="LThumb2_link">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>true</turnGravityOff>
  </gazebo>
  <gazebo reference="LPelvis">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <gazebo reference="LHip">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <gazebo reference="LThigh">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <gazebo reference="LTibia">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <gazebo reference="LAnklePitch">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <gazebo reference="l_ankle">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>false</turnGravityOff>
    <fdir1>1 0 0</fdir1>
    <maxVel>0.1</maxVel>
    <minDepth>0.003</minDepth>
  </gazebo>
  <gazebo reference="RPelvis">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <gazebo reference="RHip">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <gazebo reference="RThigh">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <gazebo reference="RTibia">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <gazebo reference="RAnklePitch">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>false</turnGravityOff>
  </gazebo>
  <gazebo reference="r_ankle">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>false</turnGravityOff>
    <fdir1>1 0 0</fdir1>
    <maxVel>0.1</maxVel>
    <minDepth>0.003</minDepth>
  </gazebo>
  <gazebo reference="RFinger11_link">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>true</turnGravityOff>
  </gazebo>
  <gazebo reference="RFinger12_link">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>true</turnGravityOff>
  </gazebo>
  <gazebo reference="RFinger13_link">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>true</turnGravityOff>
  </gazebo>
  <gazebo reference="RFinger21_link">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>true</turnGravityOff>
  </gazebo>
  <gazebo reference="RFinger22_link">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>true</turnGravityOff>
  </gazebo>
  <gazebo reference="RFinger23_link">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>true</turnGravityOff>
  </gazebo>
  <gazebo reference="RThumb1_link">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>true</turnGravityOff>
  </gazebo>
  <gazebo reference="RThumb2_link">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>true</turnGravityOff>
  </gazebo>
  <!--

        RGB CAMERAS: Working
-->
  <gazebo reference="CameraTop_frame">
    <sensor name="CameraTop" type="camera">
      <update_rate>5.0</update_rate>
      <camera name="camera_top">
        <horizontal_fov>1.06290551</horizontal_fov>
        <image>
          <width>640</width>
          <height>480</height>
          <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.3</near>
          <far>500</far>
        </clip>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.007</stddev>
        </noise>
      </camera>
      <plugin filename="libgazebo_ros_camera.so" name="camera_controller">
        <robotNamespace>nao_robot</robotNamespace>
        <alwaysOn>true</alwaysOn>
        <updateRate>5.0</updateRate>
        <cameraName>camera/top</cameraName>
        <imageTopicName>image_raw</imageTopicName>
        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
        <frameName>CameraTop_optical_frame</frameName>
        <hackBaseline>0.07</hackBaseline>
        <distortionK1>-0.0545211</distortionK1>
        <distortionK2>0.06919734</distortionK2>
        <distortionK3>-0.0241095</distortionK3>
        <distortionT1>-0.0112245</distortionT1>
        <distortionT2>0.0</distortionT2>
      </plugin>
    </sensor>
  </gazebo>
  <gazebo reference="CameraBottom_frame">
    <sensor name="CameraBottom" type="camera">
      <update_rate>5.0</update_rate>
      <camera name="camera_bottom">
        <horizontal_fov>1.06290551</horizontal_fov>
        <image>
          <width>640</width>
          <height>480</height>
          <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.05</near>
          <far>500</far>
        </clip>
        <noise>
          <type>gaussian</type>
          <mean>0.0</mean>
          <stddev>0.007</stddev>
        </noise>
      </camera>
      <plugin filename="libgazebo_ros_camera.so" name="camera_controller">
        <robotNamespace>/nao_robot</robotNamespace>
        <alwaysOn>true</alwaysOn>
        <updateRate>5.0</updateRate>
        <cameraName>camera_bottom</cameraName>
        <imageTopicName>image_raw</imageTopicName>
        <cameraInfoTopicName>camera_info</cameraInfoTopicName>
        <frameName>CameraBottom_optical_frame</frameName>
        <hackBaseline>0.07</hackBaseline>
        <distortionK1>-0.0648764</distortionK1>
        <distortionK2>0.06125202</distortionK2>
        <distortionK3>0.00382815</distortionK3>
        <distortionT1>-0.00551104</distortionT1>
        <distortionT2>0.0</distortionT2>
      </plugin>
    </sensor>
  </gazebo>
  <!--

        CameraDepth : not displayed at the right spot

<gazebo reference="CameraDepth3_frame">
    <sensor type="depth" name="openni_camera_camera2">
        <always_on>1</always_on>
        <visualize>true</visualize>
        <camera>
            <horizontal_fov>1.01229</horizontal_fov>
            <image>
                <width>320</width>
                <height>240</height>
                <format>R8G8B8</format>
            </image>
            <depth_camera>

            </depth_camera>
            <clip>
                <near>0.1</near>
                <far>100</far>
            </clip>
        </camera>
        <plugin name="depth_cam_controller2" filename="libgazebo_ros_openni_kinect.so">
            <baseline>0.2</baseline>
            <alwaysOn>true</alwaysOn>
            <updateRate>0.1</updateRate>
            <cameraName>xtion2</cameraName>
            <imageTopicName>/xtion2/ir/image_raw</imageTopicName>
            <cameraInfoTopicName>/xtion2/ir/camera_info</cameraInfoTopicName>
            <depthImageTopicName>/xtion2/depth/image_raw</depthImageTopicName>
            <depthImageInfoTopicName>/xtion2/depth/camera_info</depthImageInfoTopicName>
            <pointCloudTopicName>/xtion2/depth/points</pointCloudTopicName>
            <frameName>CameraDepth2_frame</frameName>
            <pointCloudCutoff>0.5</pointCloudCutoff>
            <distortionK1>0.00000001</distortionK1>
            <distortionK2>0.00000001</distortionK2>
            <distortionK3>0.00000001</distortionK3>
            <distortionT1>0.00000001</distortionT1>
            <distortionT2>0.00000001</distortionT2>
            <CxPrime>0</CxPrime>
            <Cx>0</Cx>
            <Cy>0</Cy>
            <focalLength>0</focalLength>
            <hackBaseline>0</hackBaseline>
        </plugin>
    </sensor>
</gazebo>
-->
  <!--
    BUMPERS + FSR
libgazebo_ros_bumper.so bugged right now, waiting for new release


<gazebo reference="LFootBumper_frame">
  <sensor type="contact" name="LFootBumper">
    <update_rate>10</update_rate>
    <contact>
      <collision>LFootBumper_frame_collision</collision>
    </contact>
    <plugin filename="libgazebo_ros_bumper.so" name="gazebo_nao_ros_bumper">
      <alwaysOn>true</alwaysOn>
      <bumperTopicName>LFootBumper</bumperTopicName>
      <frameName>LFootBumper_frame</frameName>
    </plugin>
  </sensor>
</gazebo>
-->
  <gazebo reference="RFsrFL_frame">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>false</turnGravityOff>
    <fdir1>1 0 0</fdir1>
    <maxVel>0.1</maxVel>
    <minDepth>0.003</minDepth>
    <sensor name="RFsrFL" type="contact">
      <always_on>1</always_on>
      <update_rate>20</update_rate>
      <visualize>false</visualize>
      <contact>
        <collision>RFsrFL_frame_collision</collision>
      </contact>
      <plugin filename="libgazebo_ros_bumper.so" name="gazebo_nao_RFsrFL">
        <robotNamespace>nao_robot</robotNamespace>
        <alwaysOn>true</alwaysOn>
        <bumperTopicName>FSR/RFoot/FrontLeft</bumperTopicName>
        <frameName>RFsrFL_frame</frameName>
      </plugin>
    </sensor>
  </gazebo>
  <gazebo reference="RFsrFR_frame">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>false</turnGravityOff>
    <fdir1>1 0 0</fdir1>
    <maxVel>0.1</maxVel>
    <minDepth>0.003</minDepth>
    <sensor name="RFsrFR" type="contact">
      <always_on>1</always_on>
      <update_rate>20</update_rate>
      <visualize>false</visualize>
      <contact>
        <collision>RFsrFR_frame_collision</collision>
      </contact>
      <plugin filename="libgazebo_ros_bumper.so" name="gazebo_nao_RFsrFR">
        <robotNamespace>nao_robot</robotNamespace>
        <alwaysOn>true</alwaysOn>
        <bumperTopicName>FSR/RFoot/FrontRight</bumperTopicName>
        <frameName>RFsrFR_frame</frameName>
      </plugin>
    </sensor>
  </gazebo>
  <gazebo reference="RFsrRL_frame">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>false</turnGravityOff>
    <fdir1>1 0 0</fdir1>
    <maxVel>0.1</maxVel>
    <minDepth>0.003</minDepth>
    <sensor name="RFsrRL" type="contact">
      <always_on>1</always_on>
      <update_rate>20</update_rate>
      <visualize>false</visualize>
      <contact>
        <collision>RFsrRL_frame_collision</collision>
      </contact>
      <plugin filename="libgazebo_ros_bumper.so" name="gazebo_nao_RFsrRL">
        <robotNamespace>nao_robot</robotNamespace>
        <alwaysOn>true</alwaysOn>
        <bumperTopicName>FSR/RFoot/RearLeft</bumperTopicName>
        <frameName>RFsrRL_frame</frameName>
      </plugin>
    </sensor>
  </gazebo>
  <gazebo reference="RFsrRR_frame">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>false</turnGravityOff>
    <fdir1>1 0 0</fdir1>
    <maxVel>0.1</maxVel>
    <minDepth>0.003</minDepth>
    <sensor name="RFsrRR" type="contact">
      <always_on>1</always_on>
      <update_rate>20</update_rate>
      <visualize>false</visualize>
      <contact>
        <collision>RFsrRR_frame_collision</collision>
      </contact>
      <plugin filename="libgazebo_ros_bumper.so" name="gazebo_nao_RFsrRR">
        <robotNamespace>nao_robot</robotNamespace>
        <alwaysOn>true</alwaysOn>
        <bumperTopicName>FSR/RFoot/RearRight</bumperTopicName>
        <frameName>RFsrRR_frame</frameName>
      </plugin>
    </sensor>
  </gazebo>
  <gazebo reference="LFsrFL_frame">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>false</turnGravityOff>
    <fdir1>1 0 0</fdir1>
    <maxVel>0.1</maxVel>
    <minDepth>0.003</minDepth>
    <sensor name="LFsrFL" type="contact">
      <always_on>1</always_on>
      <update_rate>20</update_rate>
      <visualize>false</visualize>
      <contact>
        <collision>LFsrFL_frame_collision</collision>
      </contact>
      <plugin filename="libgazebo_ros_bumper.so" name="gazebo_nao_LFsrFL">
        <robotNamespace>nao_robot</robotNamespace>
        <alwaysOn>true</alwaysOn>
        <bumperTopicName>FSR/LFoot/FrontLeft</bumperTopicName>
        <frameName>RFsrFL_frame</frameName>
      </plugin>
    </sensor>
  </gazebo>
  <gazebo reference="LFsrFR_frame">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>false</turnGravityOff>
    <fdir1>1 0 0</fdir1>
    <maxVel>0.1</maxVel>
    <minDepth>0.003</minDepth>
    <sensor name="LFsrFR" type="contact">
      <always_on>1</always_on>
      <update_rate>20</update_rate>
      <visualize>false</visualize>
      <contact>
        <collision>LFsrFR_frame_collision</collision>
      </contact>
      <plugin filename="libgazebo_ros_bumper.so" name="gazebo_nao_ros_bumper">
        <robotNamespace>nao_robot</robotNamespace>
        <alwaysOn>true</alwaysOn>
        <bumperTopicName>FSR/LFoot/FrontRight</bumperTopicName>
        <frameName>LFsrFR_frame</frameName>
      </plugin>
    </sensor>
  </gazebo>
  <gazebo reference="LFsrRL_frame">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>false</turnGravityOff>
    <fdir1>1 0 0</fdir1>
    <maxVel>0.1</maxVel>
    <minDepth>0.003</minDepth>
    <sensor name="LFsrRL" type="contact">
      <always_on>1</always_on>
      <update_rate>20</update_rate>
      <visualize>false</visualize>
      <contact>
        <collision>LFsrRL_frame_collision</collision>
      </contact>
      <plugin filename="libgazebo_ros_bumper.so" name="gazebo_nao_ros_bumper">
        <robotNamespace>nao_robot</robotNamespace>
        <alwaysOn>true</alwaysOn>
        <bumperTopicName>FSR/LFoot/RearLeft</bumperTopicName>
        <frameName>LFsrRL_frame</frameName>
      </plugin>
    </sensor>
  </gazebo>
  <gazebo reference="LFsrRR_frame">
    <mu1>0.5</mu1>
    <mu2>0.5</mu2>
    <selfCollide>false</selfCollide>
    <turnGravityOff>false</turnGravityOff>
    <fdir1>1 0 0</fdir1>
    <maxVel>0.1</maxVel>
    <minDepth>0.003</minDepth>
    <sensor name="LFsrRR" type="contact">
      <always_on>1</always_on>
      <update_rate>20</update_rate>
      <visualize>false</visualize>
      <contact>
        <collision>LFsrRR_frame_collision</collision>
      </contact>
      <plugin filename="libgazebo_ros_bumper.so" name="gazebo_nao_ros_bumper">
        <robotNamespace>nao_robot</robotNamespace>
        <alwaysOn>true</alwaysOn>
        <bumperTopicName>FSR/LFoot/RearRight</bumperTopicName>
        <frameName>LFsrRR_frame</frameName>
      </plugin>
    </sensor>
  </gazebo>
  <!--
    Sonars
-->
  <gazebo reference="LSonar_frame">
    <sensor name="LSonar" type="ray">
      <pose>0 0 0 0 0 0</pose>
      <update_rate>20</update_rate>
      <visualize>false</visualize>
      <ray>
        <scan>
          <horizontal>
            <samples>5</samples>
            <resolution>1</resolution>
            <min_angle>-0.2617993877991494</min_angle>
            <max_angle>0.2617993877991494</max_angle>
            <!--
          <min_angle>-0.10</min_angle>
          <max_angle>0.10</max_angle>
-->
          </horizontal>
          <vertical>
            <samples>5</samples>
            <resolution>1</resolution>
            <min_angle>-0.13</min_angle>
            <max_angle>0.13</max_angle>
            <!--
          <min_angle>-0.2617993877991494</min_angle>
          <max_angle>0.2617993877991494</max_angle>
-->
          </vertical>
        </scan>
        <range>
          <min>0.025</min>
          <max>2.55</max>
          <resolution>1</resolution>
        </range>
      </ray>
      <plugin filename="libgazebo_ros_range.so" name="gazebo_ros_range">
        <robotNamespace>nao_robot</robotNamespace>
        <gaussianNoise>0.05</gaussianNoise>
        <alwaysOn>true</alwaysOn>
        <updateRate>20</updateRate>
        <topicName>sonar_left</topicName>
        <frameName>LSonar_frame</frameName>
        <minRange>0.025</minRange>
        <maxRange>2.55</maxRange>
        <fov>0.5235987755982988</fov>
        <!--      <fov>0.1</fov> -->
        <radiation>ultrasound</radiation>
      </plugin>
    </sensor>
  </gazebo>
  <gazebo reference="RSonar_frame">
    <sensor name="RSonar" type="ray">
      <pose>0 0 0 0 0 0</pose>
      <update_rate>20</update_rate>
      <visualize>false</visualize>
      <ray>
        <scan>
          <horizontal>
            <samples>5</samples>
            <resolution>1</resolution>
            <min_angle>-0.2617993877991494</min_angle>
            <max_angle>0.2617993877991494</max_angle>
          </horizontal>
          <vertical>
            <samples>5</samples>
            <resolution>1</resolution>
            <min_angle>-0.13</min_angle>
            <max_angle>0.13</max_angle>
          </vertical>
        </scan>
        <range>
          <min>0.025</min>
          <max>2.55</max>
          <resolution>1</resolution>
        </range>
      </ray>
      <plugin filename="libgazebo_ros_range.so" name="gazebo_ros_range">
        <robotNamespace>nao_robot</robotNamespace>
        <gaussianNoise>0.05</gaussianNoise>
        <alwaysOn>true</alwaysOn>
        <updateRate>20</updateRate>
        <topicName>sonar_right</topicName>
        <frameName>RSonar_frame</frameName>
        <minRange>0.025</minRange>
        <maxRange>2.55</maxRange>
        <fov>0.5235987755982988</fov>
        <radiation>ultrasound</radiation>
      </plugin>
    </sensor>
  </gazebo>
  <!-- HEAD -->
  <transmission name="HeadYaw_Transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="HeadYaw">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="HeadYaw_Motor">
      <!-- Dummy Values -->
      <hardwareInterface>PositionJointInterface</hardwareInterface>
      <mechanicalReduction>150.27</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="HeadPitch_Transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="HeadPitch">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="HeadPitch_Motor">
      <!-- Dummy Values -->
      <hardwareInterface>PositionJointInterface</hardwareInterface>
      <mechanicalReduction>173.22</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="LShoulderPitch_Transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="LShoulderPitch">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="LShoulderPitch_Motor">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
      <mechanicalReduction>150.27</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="LShoulderRoll_Transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="LShoulderRoll">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="LShoulderRoll_Motor">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
      <mechanicalReduction>173.22</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="LElbowYaw_Transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="LElbowYaw">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="LElbowYaw_Motor">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
      <mechanicalReduction>150.27</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="LElbowRoll_Transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="LElbowRoll">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="LElbowRoll_Motor">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
      <mechanicalReduction>173.22</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="LWristYaw_Transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="LWristYaw">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="LWristYaw_Motor">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
      <mechanicalReduction>50.61</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="LHand_Transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="LHand">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="LHand_Motor">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
      <mechanicalReduction>36.24</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="RShoulderPitch_Transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="RShoulderPitch">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="RShoulderPitch_Motor">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
      <mechanicalReduction>150.27</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="RShoulderRoll_Transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="RShoulderRoll">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="RShoulderRoll_Motor">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
      <mechanicalReduction>173.22</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="RElbowYaw_Transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="RElbowYaw">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="RElbowYaw_Motor">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
      <mechanicalReduction>150.27</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="RElbowRoll_Transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="RElbowRoll">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="RElbowRoll_Motor">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
      <mechanicalReduction>173.22</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="RWristYaw_Transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="RWristYaw">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="RWristYaw_Motor">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
      <mechanicalReduction>50.61</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="RHand_Transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="RHand">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="RHand_Motor">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
      <mechanicalReduction>36.24</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="LHipYawPitch_Transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="LHipYawPitch">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="LHipYawPitch_Motor">
      <!-- Dummy Values -->
      <hardwareInterface>PositionJointInterface</hardwareInterface>
      <mechanicalReduction>201.3</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="LHipRoll_Transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="LHipRoll">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="LHipRoll_Motor">
      <!-- Dummy Values -->
      <hardwareInterface>PositionJointInterface</hardwareInterface>
      <mechanicalReduction>201.3</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="LHipPitch_Transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="LHipPitch">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="LHipPitch_Motor">
      <!-- Dummy Values -->
      <hardwareInterface>PositionJointInterface</hardwareInterface>
      <mechanicalReduction>130.85</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="LKneePitch_Transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="LKneePitch">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="LKneePitch_Motor">
      <!-- Dummy Values -->
      <hardwareInterface>PositionJointInterface</hardwareInterface>
      <mechanicalReduction>130.85</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="LAnklePitch_Transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="LAnklePitch">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="LAnklePitch_Motor">
      <!-- Dummy Values -->
      <hardwareInterface>PositionJointInterface</hardwareInterface>
      <mechanicalReduction>130.85</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="LAnkleRoll_Transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="LAnkleRoll">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="LAnkleRoll_Motor">
      <!-- Dummy Values -->
      <hardwareInterface>PositionJointInterface</hardwareInterface>
      <mechanicalReduction>201.3</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="RHipYawPitch_Transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="RHipYawPitch">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="RHipYawPitch_Motor">
      <!-- Dummy Values -->
      <hardwareInterface>PositionJointInterface</hardwareInterface>
      <mechanicalReduction>201.3</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="RHipRoll_Transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="RHipRoll">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="RHipRoll_Motor">
      <!-- Dummy Values -->
      <hardwareInterface>PositionJointInterface</hardwareInterface>
      <mechanicalReduction>201.3</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="RHipPitch_Transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="RHipPitch">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="RHipPitch_Motor">
      <!-- Dummy Values -->
      <hardwareInterface>PositionJointInterface</hardwareInterface>
      <mechanicalReduction>130.85</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="RKneePitch_Transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="RKneePitch">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="RKneePitch_Motor">
      <!-- Dummy Values -->
      <hardwareInterface>PositionJointInterface</hardwareInterface>
      <mechanicalReduction>130.85</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="RAnklePitch_Transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="RAnklePitch">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="RAnklePitch_Motor">
      <!-- Dummy Values -->
      <hardwareInterface>PositionJointInterface</hardwareInterface>
      <mechanicalReduction>130.85</mechanicalReduction>
    </actuator>
  </transmission>
  <transmission name="RAnkleRoll_Transmission">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="RAnkleRoll">
      <hardwareInterface>PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="RAnkleRoll_Motor">
      <!-- Dummy Values -->
      <hardwareInterface>PositionJointInterface</hardwareInterface>
      <mechanicalReduction>201.3</mechanicalReduction>
    </actuator>
  </transmission>
</robot>