model.urdf

<robot name="iCub">
  <link name="root_link">
    <inertial>
      <origin xyz="0.0035506 -0.00014959200000000001 0.024519000000000013" rpy="0 0 -1.5707963267948966"/>
      <mass value="2.5751"/>
      <inertia ixx="0.004051489764137304" ixy="-2.0525354991945143e-06" ixz="-1.200186769383287e-05" iyy="0.004466379288694287" iyz="0.00011702292411912923" izz="0.002537512875376926"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0.313858" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_root_link_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="root_link_color">
        <color rgba="0.9 0.9 0.9 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0.313858" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_root_link_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <link name="r_hip_1">
    <inertial>
      <origin xyz="-0.004106499999999999 -0.02810980000000001 -0.0008360000000000034" rpy="0 0 -1.5707963267948966"/>
      <mass value="2.1623"/>
      <inertia ixx="0.002675989970439473" ixy="6.426984653122192e-05" ixz="-6.447456052305588e-06" iyy="0.0021990073364667796" iyz="-1.5695577440979572e-05" izz="0.002366627191660443"/>
    </inertial>
    <visual>
      <origin xyz="-0.0216 0.04054999999999986 0.313858" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_hip_1_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="r_hip_1_color">
        <color rgba="0.9 0.9 0.9 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="-0.0216 0.04054999999999986 0.313858" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_hip_1_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="r_hip_pitch" type="revolute">
    <origin xyz="0.0216 -0.040549999999999996 0" rpy="0 0 0"/>
    <axis xyz="0.0 -1.0 0.0"/>
    <parent link="root_link"/>
    <child link="r_hip_1"/>
    <limit effort="50000" lower="-0.7853981633974483" upper="2.007128639793479" velocity="120.0"/>
    <dynamics damping="2.0" friction="0.0"/>
  </joint>
  <link name="r_hip_2">
    <inertial>
      <origin xyz="-0.003963499999999998 -7.010000000000349e-05 -0.03753499999999993" rpy="0 0 -1.5707963267948966"/>
      <mass value="1.7731"/>
      <inertia ixx="0.005229204654206729" ixy="4.628243459077994e-06" ixz="-3.2636330686460046e-06" iyy="0.0029659325947175245" iyz="0.0009467254134739666" izz="0.00327641680237346"/>
    </inertial>
    <visual>
      <origin xyz="0.025699999999999973 0.07364999999999984 0.3228580000000001" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_hip_2_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="r_hip_2_color">
        <color rgba="0.9 0.9 0.9 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0.025699999999999973 0.07364999999999984 0.3228580000000001" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_hip_2_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="r_hip_roll" type="revolute">
    <origin xyz="-0.0473 -0.03310000000000001 -0.009000000000000008" rpy="0 0 0"/>
    <axis xyz="-1.0 0.0 0.0"/>
    <parent link="r_hip_1"/>
    <child link="r_hip_2"/>
    <limit effort="50000" lower="-0.17453292519943295" upper="1.9547687622336491" velocity="120.0"/>
    <dynamics damping="2.0" friction="0.0"/>
  </joint>
  <link name="r_leg_ft"/>
  <joint name="r_leg_ft_fixed_joint" type="fixed">
    <origin xyz="0.036699999999999997 0 -0.08969999999999995" rpy="3.141592653589793 0 -2.6179934742070086"/>
    <parent link="r_hip_2"/>
    <child link="r_leg_ft"/>
    <dynamics damping="0.1"/>
  </joint>
  <link name="r_hip_3">
    <inertial>
      <origin xyz="0.0024329000000000017 0.01233370000000001 -0.038242000000000054" rpy="0 0 -1.5707963267948966"/>
      <mass value="2.1635"/>
      <inertia ixx="0.003977864038008573" ixy="-2.0706800781267456e-06" ixz="0.00012771514554905494" iyy="0.0037304828460840533" iyz="1.0430951778634928e-05" izz="0.003698882105935934"/>
    </inertial>
    <visual>
      <origin xyz="-0.011000000000000029 0.07364999999999984 0.41255800000000004" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_hip_3_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="r_hip_3_color">
        <color rgba="0.3 0.3 0.3 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="-0.011000000000000029 0.07364999999999984 0.41255800000000004" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_hip_3_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="r_leg_ft_sensor" type="fixed">
    <origin xyz="0.036699999999999997 0 -0.08969999999999995" rpy="0 0 0"/>
    <parent link="r_hip_2"/>
    <child link="r_hip_3"/>
    <dynamics damping="0.1"/>
  </joint>
  <link name="r_upper_leg">
    <inertial>
      <origin xyz="0.005240300000000003 -0.0014998000000000025 -0.05185799999999996" rpy="0 0 -1.5707963267948966"/>
      <mass value="0.90926"/>
      <inertia ixx="0.0018609457769770859" ixy="-2.472258346821965e-05" ixz="9.915722347596187e-05" iyy="0.0023282830098543227" iyz="0.00022002726995059012" izz="0.001011608294817175"/>
    </inertial>
    <visual>
      <origin xyz="-0.011000000000000012 0.05864999999999984 0.48345800000000005" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_upper_leg_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="r_upper_leg_color">
        <color rgba="0.9 0.9 0.9 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="-0.011000000000000012 0.05864999999999984 0.48345800000000005" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_upper_leg_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="r_hip_yaw" type="revolute">
    <origin xyz="0 0.015000000000000006 -0.07090000000000007" rpy="0 0 0"/>
    <axis xyz="0.0 0.0 -1.0"/>
    <parent link="r_hip_3"/>
    <child link="r_upper_leg"/>
    <limit effort="50000" lower="-1.3962634015954636" upper="1.3962634015954636" velocity="120.0"/>
    <dynamics damping="2.0" friction="0.0"/>
  </joint>
  <link name="r_lower_leg">
    <inertial>
      <origin xyz="-0.013102400000000002 0.021986699999999998 -0.127262" rpy="0 0 -1.5707963267948966"/>
      <mass value="5.0761"/>
      <inertia ixx="0.05584150942576732" ixy="-0.00010551678502849829" ixz="-0.00025239804506766437" iyy="0.05341052337773173" iyz="-0.00012711716941235637" izz="0.005785572738044439"/>
    </inertial>
    <visual>
      <origin xyz="-0.026000000000000037 0.0835499999999998 0.589658" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_lower_leg_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="r_lower_leg_color">
        <color rgba="0.3 0.3 0.3 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="-0.026000000000000037 0.0835499999999998 0.589658" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_lower_leg_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="r_knee" type="revolute">
    <origin xyz="0.015000000000000003 -0.0249 -0.10619999999999996" rpy="0 0 0"/>
    <axis xyz="0.0 -1.0 0.0"/>
    <parent link="r_upper_leg"/>
    <child link="r_lower_leg"/>
    <limit effort="50000" lower="-1.2217304763960306" upper="0.08726646259971647" velocity="120.0"/>
    <dynamics damping="2.0" friction="0.0"/>
  </joint>
  <link name="r_ankle_1">
    <inertial>
      <origin xyz="-0.005665800000000002 0.0004764000000000018 -0.015185999999999922" rpy="0 0 -1.5707963267948966"/>
      <mass value="0.36569"/>
      <inertia ixx="0.01" ixy="-3.63258049040399e-05" ixz="-6.07134864852001e-05" iyy="0.01" iyz="-2.6127082529919777e-05" izz="0.01"/>
    </inertial>
    <visual>
      <origin xyz="-0.026000000000000245 0.08304999999999924 0.8456470000000003" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_ankle_1_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="r_ankle_1_color">
        <color rgba="0.9 0.9 0.9 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="-0.026000000000000245 0.08304999999999924 0.8456470000000003" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_ankle_1_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="r_ankle_pitch" type="revolute">
    <origin xyz="0 0.0005000000000000004 -0.255989" rpy="0 0 0"/>
    <axis xyz="0.0 1.0 0.0"/>
    <parent link="r_lower_leg"/>
    <child link="r_ankle_1"/>
    <limit effort="50000" lower="-0.7853981633974483" upper="0.7853981633974483" velocity="120.0"/>
    <dynamics damping="2.0" friction="0.0"/>
  </joint>
  <link name="r_ankle_2">
    <inertial>
      <origin xyz="0.0417371 5.619999999997849e-05 -0.025592999999999977" rpy="0 0 -1.5707963267948966"/>
      <mass value="1.3771"/>
      <inertia ixx="0.01" ixy="-5.974924439638223e-06" ixz="1.456936368743329e-06" iyy="0.01" iyz="0.0011141795748834335" izz="0.01"/>
    </inertial>
    <visual>
      <origin xyz="0.026749999999999875 0.0648499999999391 0.8806470000000108" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_ankle_2_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="r_ankle_2_color">
        <color rgba="0.3 0.3 0.3 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0.026749999999999875 0.0648499999999391 0.8806470000000108" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_ankle_2_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="r_ankle_roll" type="revolute">
    <origin xyz="-0.052750000000000005 0.018200000000000008 -0.03500000000000003" rpy="0 0 0"/>
    <axis xyz="-1.0 0.0 0.0"/>
    <parent link="r_ankle_1"/>
    <child link="r_ankle_2"/>
    <limit effort="50000" lower="-0.4363323129985824" upper="0.4363323129985824" velocity="120.0"/>
    <dynamics damping="2.0" friction="0.0"/>
  </joint>
  <link name="r_foot_front_ft"/>
  <joint name="r_foot_front_ft_fixed_joint" type="fixed">
    <origin xyz="0.13525 0 -0.04349999999999998" rpy="0 0 -2.094395210586932"/>
    <parent link="r_ankle_2"/>
    <child link="r_foot_front_ft"/>
    <dynamics damping="0.1"/>
  </joint>
  <link name="r_foot_rear_ft"/>
  <joint name="r_foot_rear_ft_fixed_joint" type="fixed">
    <origin xyz="0.016 0 -0.04349999999999998" rpy="0 0 -2.094395210586932"/>
    <parent link="r_ankle_2"/>
    <child link="r_foot_rear_ft"/>
    <dynamics damping="0.1"/>
  </joint>
  <link name="r_foot_front">
    <inertial>
      <origin xyz="-0.0004180000000000017 0.0004849999999999993 0.005581000000000058" rpy="0 0 -1.5707963267948966"/>
      <mass value="0.17964"/>
      <inertia ixx="0.01" ixy="-1.1622018441861841e-05" ixz="2.9686391650024495e-05" iyy="0.01" iyz="7.056362242091155e-05" izz="0.01"/>
    </inertial>
    <visual>
      <origin xyz="-0.10850000000000012 0.0648499999999391 0.9464470000000105" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_foot_front_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="r_foot_front_color">
        <color rgba="1 0.51 0.0 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0.0 0.0 0.003" rpy="0.0 0.0 0.0"/>
      <geometry>
        <box size="0.117 0.1 0.006"/>
      </geometry>
    </collision>
  </link>
  <joint name="r_foot_front_ft_sensor" type="fixed">
    <origin xyz="0.13525 0 -0.06579999999999997" rpy="0 0 0"/>
    <parent link="r_ankle_2"/>
    <child link="r_foot_front"/>
    <dynamics damping="0.1"/>
  </joint>
  <link name="r_foot_rear">
    <inertial>
      <origin xyz="0.0006180999999999999 7.079999999998199e-05 0.003514000000000017" rpy="0 0 -1.5707963267948966"/>
      <mass value="0.17874"/>
      <inertia ixx="0.01" ixy="1.5972601165325875e-07" ixz="2.851962872955676e-06" iyy="0.01" iyz="-1.2898142814635283e-06" izz="0.01"/>
    </inertial>
    <visual>
      <origin xyz="0.010749999999999876 0.0648499999999391 0.9464470000000105" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_foot_rear_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="r_foot_rear_color">
        <color rgba="1 0.51 0.0 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0.0 0.0 0.003" rpy="0.0 0.0 0.0"/>
      <geometry>
        <box size="0.117 0.1 0.006"/>
      </geometry>
    </collision>
  </link>
  <joint name="r_foot_rear_ft_sensor" type="fixed">
    <origin xyz="0.016 0 -0.06579999999999997" rpy="0 0 0"/>
    <parent link="r_ankle_2"/>
    <child link="r_foot_rear"/>
    <dynamics damping="0.1"/>
  </joint>
  <link name="r_sole"/>
  <joint name="r_sole_fixed_joint" type="fixed">
    <origin xyz="0.05963000000000001 0 0" rpy="0 0 0"/>
    <parent link="r_foot_rear"/>
    <child link="r_sole"/>
    <dynamics damping="0.1"/>
  </joint>
  <link name="torso_1">
    <inertial>
      <origin xyz="-0.00799027 0.00030333300000000003 0.051940000000000014" rpy="0 0 -1.5707963267948966"/>
      <mass value="1.6998"/>
      <inertia ixx="0.003404164181974366" ixy="1.1169481599472077e-06" ixz="-4.43776767150407e-06" iyy="0.002616204579509964" iyz="-0.00032094515374069167" izz="0.002060525094007953"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0.2533580000000001" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_torso_1_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="torso_1_color">
        <color rgba="0.9 0.9 0.9 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0.2533580000000001" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_torso_1_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="torso_roll" type="revolute">
    <origin xyz="0 0 0.0605" rpy="0 0 0"/>
    <axis xyz="-1.0 0.0 0.0"/>
    <parent link="root_link"/>
    <child link="torso_1"/>
    <limit effort="50000" lower="-0.4014257279586958" upper="0.4014257279586958" velocity="120.0"/>
    <dynamics damping="2.0" friction="0.0"/>
  </joint>
  <link name="torso_2">
    <inertial>
      <origin xyz="0.00131935 0.00332192 0.026808" rpy="0 0 -1.5707963267948966"/>
      <mass value="0.77098"/>
      <inertia ixx="0.0008105413874308448" ixy="-3.5442504712774724e-06" ixz="-7.21480883330135e-05" iyy="0.0014602573028254073" iyz="-1.0808501368993641e-05" izz="0.0012107068713070723"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0.18085800000000016" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_torso_2_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="torso_2_color">
        <color rgba="0.9 0.9 0.9 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0.18085800000000016" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_torso_2_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="torso_pitch" type="revolute">
    <origin xyz="0 0 0.07250000000000001" rpy="0 0 0"/>
    <axis xyz="0.0 1.0 0.0"/>
    <parent link="torso_1"/>
    <child link="torso_2"/>
    <limit effort="50000" lower="-0.3141592653589793" upper="0.7853981633974483" velocity="120.0"/>
    <dynamics damping="2.0" friction="0.0"/>
  </joint>
  <link name="chest">
    <inertial>
      <origin xyz="-0.00597433 -7.26041e-05 0.06416630000000001" rpy="0 0 -1.5707963267948966"/>
      <mass value="7.5231"/>
      <inertia ixx="0.03385562771578811" ixy="5.2898427148176666e-05" ixz="0.00014073569655152914" iyy="0.03297344897602433" iyz="-0.0017469131032638945" izz="0.029559708315070623"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0.11638800000000014" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_chest_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="chest_color">
        <color rgba="0.3 0.3 0.3 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0.11638800000000014" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_chest_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="torso_yaw" type="revolute">
    <origin xyz="0 0 0.06447000000000001" rpy="0 0 0"/>
    <axis xyz="0.0 0.0 -1.0"/>
    <parent link="torso_2"/>
    <child link="chest"/>
    <limit effort="50000" lower="-0.7504915783575618" upper="0.7504915783575618" velocity="120.0"/>
    <dynamics damping="2.0" friction="0.0"/>
  </joint>
  <link name="chest_imu_0"/>
  <joint name="chest_imu_0_fixed_joint" type="fixed">
    <origin xyz="0.0894115 0.0175 0.09929950000000001" rpy="-1.5707963267948968 0 -1.5707963267948968"/>
    <parent link="chest"/>
    <child link="chest_imu_0"/>
    <dynamics damping="0.1"/>
  </joint>
  <link name="chest_rgb_frame"/>
  <joint name="chest_rgb_frame_fixed_joint" type="fixed">
    <origin xyz="0.0894115 -0.0115 0.09929950000000001" rpy="-1.5707963267948968 0 -1.5707963267948968"/>
    <parent link="chest"/>
    <child link="chest_rgb_frame"/>
    <dynamics damping="0.1"/>
  </joint>
  <link name="neck_1">
    <inertial>
      <origin xyz="-0.0021259300000000003 0.00033753499999999997 0.031454700000000016" rpy="-1.5707963267948968 0 -1.5707963267948968"/>
      <mass value="0.36804"/>
      <inertia ixx="0.000179701897883096" ixy="-8.833876708276077e-06" ixz="-1.4878426738622656e-05" iyy="0.0002675144279997793" iyz="5.57627455590208e-06" izz="0.00028375118819347193"/>
    </inertial>
    <visual>
      <origin xyz="0.00865 0 0.049000000000000016" rpy="-1.5707963267948968 0 -1.5707963267948968"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_head_neck_1_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="neck_1_color">
        <color rgba="0.9 0.9 0.9 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0.00865 0 0.049000000000000016" rpy="-1.5707963267948968 0 -1.5707963267948968"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_head_neck_1_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="neck_fixed_joint" type="fixed">
    <origin xyz="-0.00378849 0 0.1652995" rpy="0 0 0"/>
    <parent link="chest"/>
    <child link="neck_1"/>
    <dynamics damping="0.1"/>
  </joint>
  <link name="neck_2">
    <inertial>
      <origin xyz="-0.0063756 3.63093e-05 0.006597499999999992" rpy="-1.5707963267948968 0 -1.5707963267948968"/>
      <mass value="0.1214"/>
      <inertia ixx="3.922803192379071e-05" ixy="5.7011653935913464e-08" ixz="9.665431536690854e-08" iyy="3.7385115930831784e-05" iyz="-9.08258119140734e-06" izz="4.585137085918645e-05"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="-1.5707963267948968 0 -1.5707963267948968"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_head_neck_2_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="neck_2_color">
        <color rgba="0.9 0.9 0.9 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="-1.5707963267948968 0 -1.5707963267948968"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_head_neck_2_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="neck_pitch" type="revolute">
    <origin xyz="0.00865 0 0.049000000000000016" rpy="0 0 0"/>
    <axis xyz="0.0 -1.0 0.0"/>
    <parent link="neck_1"/>
    <child link="neck_2"/>
    <limit effort="50000" lower="-0.5235987755982988" upper="0.3839724354387525" velocity="120.0"/>
    <dynamics damping="2.0" friction="0.0"/>
  </joint>
  <link name="neck_3">
    <inertial>
      <origin xyz="0.0012653999999999999 -8.218820000000001e-05 0.036108999999999974" rpy="-1.5707963267948968 0 -1.5707963267948968"/>
      <mass value="0.18386"/>
      <inertia ixx="0.00011968746119821388" ixy="-5.240077207007726e-07" ixz="-3.6002165484161086e-07" iyy="3.99187384725429e-05" iyz="-9.97584076409671e-06" izz="0.00010705060973108298"/>
    </inertial>
    <visual>
      <origin xyz="0 0 -0.009500499999999995" rpy="-1.5707963267948968 0 -1.5707963267948968"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_head_neck_3_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="neck_3_color">
        <color rgba="0.9 0.9 0.9 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 -0.009500499999999995" rpy="-1.5707963267948968 0 -1.5707963267948968"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_head_neck_3_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="neck_roll" type="revolute">
    <origin xyz="0 0 0.009500499999999995" rpy="0 0 0"/>
    <axis xyz="1.0 0.0 0.0"/>
    <parent link="neck_2"/>
    <child link="neck_3"/>
    <limit effort="50000" lower="-0.3490658503988659" upper="0.3490658503988659" velocity="120.0"/>
    <dynamics damping="2.0" friction="0.0"/>
  </joint>
  <link name="head">
    <inertial>
      <origin xyz="-0.02065171 0.000526031 0.027896999999999977" rpy="-1.5707963267948968 0 -1.5707963267948968"/>
      <mass value="1.8728"/>
      <inertia ixx="0.007642647745719656" ixy="4.8998499721183834e-05" ixz="4.926393457113943e-05" iyy="0.007404636328713935" iyz="-0.00019328685350792046" izz="0.005594148247588756"/>
    </inertial>
    <visual>
      <origin xyz="0 0 -0.07865050000000001" rpy="-1.5707963267948968 0 -1.5707963267948968"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_head_head_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="head_color">
        <color rgba="0.9 0.9 0.9 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 -0.07865050000000001" rpy="-1.5707963267948968 0 -1.5707963267948968"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_head_head_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="neck_yaw" type="revolute">
    <origin xyz="0 0 0.06915000000000002" rpy="0 0 0"/>
    <axis xyz="0.0 0.0 1.0"/>
    <parent link="neck_3"/>
    <child link="head"/>
    <limit effort="50000" lower="-0.7853981633974483" upper="0.7853981633974483" velocity="120.0"/>
    <dynamics damping="2.0" friction="0.0"/>
  </joint>
  <link name="head_imu_0"/>
  <joint name="head_imu_0_fixed_joint" type="fixed">
    <origin xyz="0.0035000000000000005 0.0055 0.10133400000000001" rpy="3.141592653589793 0 -1.5707963267948966"/>
    <parent link="head"/>
    <child link="head_imu_0"/>
    <dynamics damping="0.1"/>
  </joint>
  <link name="r_shoulder_1">
    <inertial>
      <origin xyz="0.00016744811157206815 -0.08721740246539644 0.0004507343002208668" rpy="-0.06688725064450136 0.24445256247619251 -1.840815419889917"/>
      <mass value="0.78697"/>
      <inertia ixx="0.01" ixy="-0.0001736132173031554" ixz="-0.0001629290145974639" iyy="0.01" iyz="-4.081377037378354e-05" izz="0.01"/>
    </inertial>
    <visual>
      <origin xyz="0.030411233356184383 0.029140462447480246 0.01909577433393813" rpy="-0.06688725064450136 0.24445256247619251 -1.840815419889917"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_shoulder_1_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="r_shoulder_1_color">
        <color rgba="0.9 0.9 0.9 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0.030411233356184383 0.029140462447480246 0.01909577433393813" rpy="-0.06688725064450136 0.24445256247619251 -1.840815419889917"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_shoulder_1_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="r_shoulder_pitch" type="revolute">
    <origin xyz="-0.020835100000000002 -0.0397423 0.1052029" rpy="-0.25326164431680925 2.3014774394592079e-07 0.26179970938509745"/>
    <axis xyz="1.499044390098182e-07 0.9999999783534812 -1.2401537036323317e-07"/>
    <parent link="chest"/>
    <child link="r_shoulder_1"/>
    <limit effort="50000" lower="-3.0543261909900767" upper="0.3490658503988659" velocity="120.0"/>
    <dynamics damping="2.0" friction="0.0"/>
  </joint>
  <link name="r_shoulder_2">
    <inertial>
      <origin xyz="0.005108924921141532 -0.00021262473188485287 -0.020403359362654718" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
      <mass value="0.42492"/>
      <inertia ixx="0.01" ixy="4.8603409990214605e-05" ixz="6.290223922809094e-05" iyy="0.01" iyz="-7.540635367635347e-05" izz="0.01"/>
    </inertial>
    <visual>
      <origin xyz="0.023893259221697872 0.1406408982359711 0.017434528479984186" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_shoulder_2_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="r_shoulder_2_color">
        <color rgba="0.9 0.9 0.9 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0.023893259221697872 0.1406408982359711 0.017434528479984186" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_shoulder_2_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="r_shoulder_roll" type="revolute">
    <origin xyz="0.007489008185044645 -0.11150041046657978 0.0004027858966975245" rpy="0.17453306234069352 -0.05374779257913059 -6.132332688804098e-07"/>
    <axis xyz="-0.9999998262417886 4.210501434612913e-08 2.4630481182686846e-08"/>
    <parent link="r_shoulder_1"/>
    <child link="r_shoulder_2"/>
    <limit effort="50000" lower="0.20943951023931956" upper="2.8448866807507573" velocity="120.0"/>
    <dynamics damping="2.0" friction="0.0"/>
  </joint>
  <link name="r_arm_ft"/>
  <joint name="r_arm_ft_fixed_joint" type="fixed">
    <origin xyz="-0.006849946385389872 2.1904996058697002e-07 -0.05429998157789099" rpy="3.141592644717707 0 1.5707964185756365"/>
    <parent link="r_shoulder_2"/>
    <child link="r_arm_ft"/>
    <dynamics damping="0.1"/>
  </joint>
  <link name="r_shoulder_3">
    <inertial>
      <origin xyz="-8.296442829116082e-05 -3.0309267538636275e-05 -0.007966520489017762" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
      <mass value="0.2625"/>
      <inertia ixx="0.01" ixy="1.6702215931276847e-06" ixz="-1.0784251066431893e-05" iyy="0.01" iyz="-5.495630207834499e-06" izz="0.01"/>
    </inertial>
    <visual>
      <origin xyz="0.030743205607087747 0.1406406791860105 0.07173451005787518" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_shoulder_3_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="r_shoulder_3_color">
        <color rgba="0.9 0.9 0.9 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0.030743205607087747 0.1406406791860105 0.07173451005787518" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_shoulder_3_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="r_arm_ft_sensor" type="fixed">
    <origin xyz="-0.006849946385389872 2.1904996058697002e-07 -0.05429998157789099" rpy="0 0 0"/>
    <parent link="r_shoulder_2"/>
    <child link="r_shoulder_3"/>
    <dynamics damping="0.1"/>
  </joint>
  <link name="r_upper_arm">
    <inertial>
      <origin xyz="0.001047638776241297 -0.002559626059706027 -0.05065314965013519" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
      <mass value="1.6219"/>
      <inertia ixx="0.0028621730577300286" ixy="-6.813413059160908e-05" ixz="0.00046088710437670286" iyy="0.002840193682070483" iyz="0.00027786456283648023" izz="0.0014181870624638555"/>
    </inertial>
    <visual>
      <origin xyz="0.030743123698670136 0.14064025735327973 0.09423445143102875" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_upperarm_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="r_upper_arm_color">
        <color rgba="0.3 0.3 0.3 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0.030743123698670136 0.14064025735327973 0.09423445143102875" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_upperarm_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="r_shoulder_yaw" type="revolute">
    <origin xyz="0 4.2183273077367467e-07 -0.02249994137315358" rpy="0 0 0"/>
    <axis xyz="-3.787621790776764e-08 2.805197706023475e-08 1.0000002566182309"/>
    <parent link="r_shoulder_3"/>
    <child link="r_upper_arm"/>
    <limit effort="50000" lower="-0.8726646259971648" upper="1.3962634015954636" velocity="120.0"/>
    <dynamics damping="2.0" friction="0.0"/>
  </joint>
  <link name="r_elbow_1">
    <inertial>
      <origin xyz="-0.009961389267727768 -0.00048246745077001885 -0.014881572540082794" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
      <mass value="0.22157"/>
      <inertia ixx="0.01" ixy="1.4282851377772346e-06" ixz="2.3557271962747337e-05" iyy="0.01" iyz="-5.224075189187301e-06" izz="0.01"/>
    </inertial>
    <visual>
      <origin xyz="0.020743191776066316 0.1391406172212523 0.21793478892057525" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_elbow_1_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="r_elbow_1_color">
        <color rgba="0.3 0.3 0.3 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0.020743191776066316 0.1391406172212523 0.21793478892057525" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_elbow_1_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="r_elbow" type="revolute">
    <origin xyz="0.00999993192260382 0.0014996401320274133 -0.1237003374895465" rpy="0 0 0"/>
    <axis xyz="2.4949582807394525e-07 -1.0000002733514362 2.7942487010105877e-08"/>
    <parent link="r_upper_arm"/>
    <child link="r_elbow_1"/>
    <limit effort="50000" lower="-0.05235987755982989" upper="1.3089969389957472" velocity="120.0"/>
    <dynamics damping="2.0" friction="0.0"/>
  </joint>
  <link name="r_forearm">
    <inertial>
      <origin xyz="-0.005904319801167034 -3.4912323017555202e-06 -0.030210371584223267" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
      <mass value="0.8686"/>
      <inertia ixx="0.0012354585621581545" ixy="-2.385589710648161e-05" ixz="0.00012368703711331712" iyy="0.0012067292165090081" iyz="0.00013988125965072048" izz="0.0006565375890204164"/>
    </inertial>
    <visual>
      <origin xyz="0.03574306124487927 0.14114026712657127 0.27323485259309493" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_forearm_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="r_forearm_color">
        <color rgba="0.3 0.3 0.3 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0.03574306124487927 0.14114026712657127 0.27323485259309493" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_forearm_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="r_wrist_prosup" type="revolute">
    <origin xyz="-0.014999869468812953 -0.0019996499053189787 -0.05530006367251966" rpy="0 0 0"/>
    <axis xyz="-3.787621790776764e-08 2.805197706023475e-08 1.0000002566182309"/>
    <parent link="r_elbow_1"/>
    <child link="r_forearm"/>
    <limit effort="50000" lower="-1.53588974175501" upper="1.53588974175501" velocity="120.0"/>
    <dynamics damping="2.0" friction="0.0"/>
  </joint>
  <link name="r_wrist_1">
    <inertial>
      <origin xyz="-0.014340376761973205 0.000501617899800938 -0.00011512626658716218" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
      <mass value="0.033183"/>
      <inertia ixx="0.01" ixy="8.77585378818565e-07" ixz="-1.8326418199726652e-08" iyy="0.01" iyz="-1.496169319469983e-07" izz="0.01"/>
    </inertial>
    <visual>
      <origin xyz="0.019443107890930632 0.14314771001738308 0.3807349323052013" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_wrist_1_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="r_wrist_1_color">
        <color rgba="0.9 0.9 0.9 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0.019443107890930632 0.14314771001738308 0.3807349323052013" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_wrist_1_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="r_wrist_pitch" type="revolute">
    <origin xyz="0.016299953353948633 -0.002007442890811839 -0.10750007971210637" rpy="0 0 0"/>
    <axis xyz="0.9999998262417886 -4.210501434612913e-08 -2.4630481182686846e-08"/>
    <parent link="r_forearm"/>
    <child link="r_wrist_1"/>
    <limit effort="50000" lower="-1.2217304763960306" upper="0.4363323129985824" velocity="120.0"/>
    <dynamics damping="2.0" friction="0.0"/>
  </joint>
  <link name="r_hand">
    <inertial>
      <origin xyz="0.004771493181005178 0.00622015783889579 -0.06696908446509364" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
      <mass value="0.25705"/>
      <inertia ixx="0.01" ixy="4.072835073167062e-05" ixz="5.517886762216454e-05" iyy="0.01" iyz="2.68326820148215e-05" izz="0.01"/>
    </inertial>
    <visual>
      <origin xyz="0.03574323076520337 0.1361483199662913 0.3807350116020947" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_hand_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="r_hand_color">
        <color rgba="0.9 0.9 0.9 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0.03574323076520337 0.1361483199662913 0.3807350116020947" rpy="-0.12008540036084188 0.22978866753011018 -1.8533144378692001"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_r_hand_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="r_wrist_yaw" type="revolute">
    <origin xyz="-0.016300122874272734 0.006999390051091792 0" rpy="0 0 0"/>
    <axis xyz="-2.4949582807394525e-07 1.0000002733514362 -2.7942487010105877e-08"/>
    <parent link="r_wrist_1"/>
    <child link="r_hand"/>
    <limit effort="50000" lower="-0.2617993877991494" upper="0.6108652381980153" velocity="120.0"/>
    <dynamics damping="2.0" friction="0.0"/>
  </joint>
  <link name="l_shoulder_1">
    <inertial>
      <origin xyz="0.000167535101724358 0.08722598325113093 0.00045078764626123313" rpy="-0.0668874800658252 -0.24445247958725436 -1.300777660386542"/>
      <mass value="0.78644"/>
      <inertia ixx="0.01" ixy="0.00017359471090191915" ixz="0.00016289691907417792" iyy="0.01" iyz="-4.081880122662575e-05" izz="0.01"/>
    </inertial>
    <visual>
      <origin xyz="0.030411223823216114 -0.029140477503746667 0.019095766539677955" rpy="-0.0668874800658252 -0.24445247958725436 -1.300777660386542"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_shoulder_1_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="l_shoulder_1_color">
        <color rgba="0.9 0.9 0.9 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0.030411223823216114 -0.029140477503746667 0.019095766539677955" rpy="-0.0668874800658252 -0.24445247958725436 -1.300777660386542"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_shoulder_1_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="l_shoulder_pitch" type="revolute">
    <origin xyz="-0.020835100000000002 0.0397423 0.1052029" rpy="0.25326162380993705 -1.1977334179319483e-07 -0.26179930184968747"/>
    <axis xyz="3.323079797554136e-07 0.9999999783534342 1.4452224303420037e-07"/>
    <parent link="chest"/>
    <child link="l_shoulder_1"/>
    <limit effort="50000" lower="-3.0543261909900767" upper="0.3490658503988659" velocity="120.0"/>
    <dynamics damping="2.0" friction="0.0"/>
  </joint>
  <link name="l_shoulder_2">
    <inertial>
      <origin xyz="0.007668668705576271 0.00018849980855933923 -0.020403454228093053" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
      <mass value="0.42492"/>
      <inertia ixx="0.01" ixy="-4.916851307375464e-05" ixz="-6.324300296292925e-05" iyy="0.01" iyz="-7.587195452290467e-05" izz="0.01"/>
    </inertial>
    <visual>
      <origin xyz="0.026393096265395596 -0.14064030580326706 0.017434480978924874" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_shoulder_2_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="l_shoulder_2_color">
        <color rgba="0.9 0.9 0.9 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0.026393096265395596 -0.14064030580326706 0.017434480978924874" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_shoulder_2_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="l_shoulder_roll" type="revolute">
    <origin xyz="0.004992849331690818 0.111499838456099 0.00026865022773668243" rpy="-0.17453296140985003 -0.05374759931933849 0"/>
    <axis xyz="0.9999998262417685 2.0348252625938444e-07 -4.023022846062156e-08"/>
    <parent link="l_shoulder_1"/>
    <child link="l_shoulder_2"/>
    <limit effort="50000" lower="0.20943951023931956" upper="2.8448866807507573" velocity="120.0"/>
    <dynamics damping="2.0" friction="0.0"/>
  </joint>
  <link name="l_arm_ft"/>
  <joint name="l_arm_ft_fixed_joint" type="fixed">
    <origin xyz="-0.004350133156978318 3.632573233602887e-07 -0.05430003872406172" rpy="3.141592646862139 0 -1.5707962571980942"/>
    <parent link="l_shoulder_2"/>
    <child link="l_arm_ft"/>
    <dynamics damping="0.1"/>
  </joint>
  <link name="l_shoulder_3">
    <inertial>
      <origin xyz="-9.561931744893337e-05 3.089115052354319e-05 -0.007966608685190321" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
      <mass value="0.2625"/>
      <inertia ixx="0.01" ixy="-1.6986505676585652e-06" ixz="1.0777869051741286e-05" iyy="0.01" iyz="-5.511652932595611e-06" izz="0.01"/>
    </inertial>
    <visual>
      <origin xyz="0.030743229422373917 -0.14064066906059042 0.0717345197029866" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_shoulder_3_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="l_shoulder_3_color">
        <color rgba="0.9 0.9 0.9 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0.030743229422373917 -0.14064066906059042 0.0717345197029866" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_shoulder_3_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="l_arm_ft_sensor" type="fixed">
    <origin xyz="-0.004350133156978318 3.632573233602887e-07 -0.05430003872406172" rpy="0 0 0"/>
    <parent link="l_shoulder_2"/>
    <child link="l_shoulder_3"/>
    <dynamics damping="0.1"/>
  </joint>
  <link name="l_upper_arm">
    <inertial>
      <origin xyz="0.0010476375729993916 0.0025596225822598698 -0.05065314985074498" rpy="-0.06212860830092193 0.0714268908124578 0.27099931149673545"/>
      <mass value="1.67729"/>
      <inertia ixx="0.002960829234557446" ixy="-1.0441222117313817e-05" ixz="0.0002229885938828402" iyy="0.0030756376591531418" iyz="0.00020156448677983343" izz="0.001327263106289413"/>
    </inertial>
    <visual>
      <origin xyz="0.030743147864881363 -0.14064024560810592 0.09423446107611103" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_upperarm_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="l_upper_arm_color">
        <color rgba="0.3 0.3 0.3 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0.030743147864881363 -0.14064024560810592 0.09423446107611103" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_upperarm_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="l_shoulder_yaw" type="revolute">
    <origin xyz="0 -4.2345248449371375e-07 -0.022499941373124423" rpy="0 0 0"/>
    <axis xyz="2.2276474049102235e-08 -4.393786247302041e-08 -1.0000002566182309"/>
    <parent link="l_shoulder_3"/>
    <child link="l_upper_arm"/>
    <limit effort="50000" lower="-0.8726646259971648" upper="1.3962634015954636" velocity="120.0"/>
    <dynamics damping="2.0" friction="0.0"/>
  </joint>
  <link name="l_elbow_1">
    <inertial>
      <origin xyz="-0.009947433863160948 0.0004713243585720006 -0.014889454628597226" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
      <mass value="0.22161"/>
      <inertia ixx="0.01" ixy="-1.4473906751261063e-06" ixz="-2.4098487110118167e-05" iyy="0.01" iyz="-5.285154621856073e-06" izz="0.01"/>
    </inertial>
    <visual>
      <origin xyz="0.020743217629961962 -0.13914059818467703 0.21793479861369477" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_elbow_1_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="l_elbow_1_color">
        <color rgba="0.3 0.3 0.3 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0.020743217629961962 -0.13914059818467703 0.21793479861369477" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_elbow_1_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="l_elbow" type="revolute">
    <origin xyz="0.009999930234919404 -0.001499647423428896 -0.12370033753758375" rpy="0 0 0"/>
    <axis xyz="-8.811824174789715e-08 -1.0000002733514628 4.404736120498342e-08"/>
    <parent link="l_upper_arm"/>
    <child link="l_elbow_1"/>
    <limit effort="50000" lower="-0.05235987755982989" upper="1.3089969389957472" velocity="120.0"/>
    <dynamics damping="2.0" friction="0.0"/>
  </joint>
  <link name="l_forearm">
    <inertial>
      <origin xyz="-0.006323186767737077 -4.644477335560837e-05 -0.030456750179120307" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
      <mass value="0.87311"/>
      <inertia ixx="0.0012499578358808174" ixy="2.299990880778632e-05" ixz="-0.00013204802784752087" iyy="0.0012325327102048142" iyz="0.00013098820888298704" izz="0.0006619865197576301"/>
    </inertial>
    <visual>
      <origin xyz="0.03574308828413966 -0.14114024168831188 0.27323486219617454" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_forearm_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="l_forearm_color">
        <color rgba="0.3 0.3 0.3 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0.03574308828413966 -0.14114024168831188 0.27323486219617454" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_forearm_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="l_wrist_prosup" type="revolute">
    <origin xyz="-0.0149998706541777 0.0019996435036348514 -0.0553000635824798" rpy="0 0 0"/>
    <axis xyz="2.2276474049102235e-08 -4.393786247302041e-08 -1.0000002566182309"/>
    <parent link="l_elbow_1"/>
    <child link="l_forearm"/>
    <limit effort="50000" lower="-1.53588974175501" upper="1.53588974175501" velocity="120.0"/>
    <dynamics damping="2.0" friction="0.0"/>
  </joint>
  <link name="l_wrist_1">
    <inertial>
      <origin xyz="-0.014340376682819098 -0.0005016202223036004 -0.00011512600676955032" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
      <mass value="0.033183"/>
      <inertia ixx="0.01" ixy="-8.77585378818565e-07" ixz="1.8326321501111727e-08" iyy="0.01" iyz="-1.496169319469983e-07" izz="0.01"/>
    </inertial>
    <visual>
      <origin xyz="0.01914307234985348 -0.14314713690638445 0.38073450333871567" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_wrist_1_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="l_wrist_1_color">
        <color rgba="0.9 0.9 0.9 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0.01914307234985348 -0.14314713690638445 0.38073450333871567" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_wrist_1_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="l_wrist_pitch" type="revolute">
    <origin xyz="0.016600015934286182 0.0020068952180725685 -0.10749964114254112" rpy="0 0 0"/>
    <axis xyz="-0.9999998262417685 -2.0348252625938444e-07 4.023022846062156e-08"/>
    <parent link="l_forearm"/>
    <child link="l_wrist_1"/>
    <limit effort="50000" lower="-1.2217304763960306" upper="0.4363323129985824" velocity="120.0"/>
    <dynamics damping="2.0" friction="0.0"/>
  </joint>
  <link name="l_hand">
    <inertial>
      <origin xyz="0.004730263158814873 -0.006201146485818793 -0.06710705725234695" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
      <mass value="0.25639"/>
      <inertia ixx="0.01" ixy="-4.053526663925987e-05" ixz="-5.426688459725203e-05" iyy="0.01" iyz="2.727389831071015e-05" izz="0.01"/>
    </inertial>
    <visual>
      <origin xyz="0.03544319409458287 -0.13614774422481324 0.38073458187744647" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_hand_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="l_hand_color">
        <color rgba="0.9 0.9 0.9 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0.03544319409458287 -0.13614774422481324 0.38073458187744647" rpy="-0.12008543635729363 -0.22978873231718944 -1.2882780461440753"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_hand_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="l_wrist_yaw" type="revolute">
    <origin xyz="-0.016300121744729387 -0.006999392681571209 0" rpy="0 0 0"/>
    <axis xyz="8.811824174789715e-08 1.0000002733514628 -4.404736120498342e-08"/>
    <parent link="l_wrist_1"/>
    <child link="l_hand"/>
    <limit effort="50000" lower="-0.2617993877991494" upper="0.6108652381980153" velocity="120.0"/>
    <dynamics damping="2.0" friction="0.0"/>
  </joint>
  <link name="l_hip_1">
    <inertial>
      <origin xyz="-0.004074100000000001 0.028123299999999997 -0.0007250000000000312" rpy="0 0 -1.5707963267948966"/>
      <mass value="2.1611"/>
      <inertia ixx="0.002595352643142238" ixy="-6.263538591577025e-05" ixz="2.7731363927618977e-06" iyy="0.0021193350821535318" iyz="-1.2725390435109447e-05" izz="0.0023619854835245473"/>
    </inertial>
    <visual>
      <origin xyz="-0.021600000000000064 -0.04044999999999981 0.31385800000000025" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_hip_1_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="l_hip_1_color">
        <color rgba="0.9 0.9 0.9 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="-0.021600000000000064 -0.04044999999999981 0.31385800000000025" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_hip_1_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="l_hip_pitch" type="revolute">
    <origin xyz="0.0216 0.04045000000000001 0" rpy="0 0 0"/>
    <axis xyz="0.0 -1.0 0.0"/>
    <parent link="root_link"/>
    <child link="l_hip_1"/>
    <limit effort="50000" lower="-0.7853981633974483" upper="2.007128639793479" velocity="120.0"/>
    <dynamics damping="2.0" friction="0.0"/>
  </joint>
  <link name="l_hip_2">
    <inertial>
      <origin xyz="-0.003963800000000003 5.489999999999662e-05 -0.037538000000000016" rpy="0 0 -1.5707963267948966"/>
      <mass value="1.7731"/>
      <inertia ixx="0.005228411714595798" ixy="-4.083551966339624e-06" ixz="3.1651054869901453e-06" iyy="0.0029657585348029296" iyz="0.0009461829267401466" izz="0.0032766682222500967"/>
    </inertial>
    <visual>
      <origin xyz="0.025649999999999822 -0.07354999999999978 0.3228580000000002" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_hip_2_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="l_hip_2_color">
        <color rgba="0.9 0.9 0.9 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0.025649999999999822 -0.07354999999999978 0.3228580000000002" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_hip_2_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="l_hip_roll" type="revolute">
    <origin xyz="-0.04725 0.0331 -0.009000000000000008" rpy="0 0 0"/>
    <axis xyz="1.0 0.0 0.0"/>
    <parent link="l_hip_1"/>
    <child link="l_hip_2"/>
    <limit effort="50000" lower="-0.17453292519943295" upper="1.9547687622336491" velocity="120.0"/>
    <dynamics damping="2.0" friction="0.0"/>
  </joint>
  <link name="l_leg_ft"/>
  <joint name="l_leg_ft_fixed_joint" type="fixed">
    <origin xyz="0.036699999999999997 0 -0.08969999999999995" rpy="3.141592653589793 0 2.6179934742070086"/>
    <parent link="l_hip_2"/>
    <child link="l_leg_ft"/>
    <dynamics damping="0.1"/>
  </joint>
  <link name="l_hip_3">
    <inertial>
      <origin xyz="0.0024416999999999998 -0.012306900000000003 -0.03819900000000004" rpy="0 0 -1.5707963267948966"/>
      <mass value="2.1622"/>
      <inertia ixx="0.003936040074777389" ixy="2.9414824206049275e-06" ixz="-0.00013297418310621338" iyy="0.003688108994306721" iyz="9.301375413802509e-06" izz="0.0036942785028421664"/>
    </inertial>
    <visual>
      <origin xyz="-0.011050000000000178 -0.07354999999999977 0.41255800000000004" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_hip_3_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="l_hip_3_color">
        <color rgba="0.3 0.3 0.3 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="-0.011050000000000178 -0.07354999999999977 0.41255800000000004" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_hip_3_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="l_leg_ft_sensor" type="fixed">
    <origin xyz="0.036699999999999997 0 -0.08969999999999995" rpy="0 0 0"/>
    <parent link="l_hip_2"/>
    <child link="l_hip_3"/>
    <dynamics damping="0.1"/>
  </joint>
  <link name="l_upper_leg">
    <inertial>
      <origin xyz="0.005238099999999999 0.001499300000000009 -0.0518519999999999" rpy="0 0 -1.5707963267948966"/>
      <mass value="0.90926"/>
      <inertia ixx="0.0018580448030305287" ixy="2.4769869343548534e-05" ixz="-9.921040799831542e-05" iyy="0.0023253336863419895" iyz="0.00021961146368491691" izz="0.0010116953240355718"/>
    </inertial>
    <visual>
      <origin xyz="-0.011050000000000167 -0.05854999999999983 0.48345800000000017" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_upper_leg_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="l_upper_leg_color">
        <color rgba="0.9 0.9 0.9 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="-0.011050000000000167 -0.05854999999999983 0.48345800000000017" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_upper_leg_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="l_hip_yaw" type="revolute">
    <origin xyz="0 -0.015000000000000006 -0.07090000000000007" rpy="0 0 0"/>
    <axis xyz="0.0 0.0 1.0"/>
    <parent link="l_hip_3"/>
    <child link="l_upper_leg"/>
    <limit effort="50000" lower="-1.3962634015954636" upper="1.3962634015954636" velocity="120.0"/>
    <dynamics damping="2.0" friction="0.0"/>
  </joint>
  <link name="l_lower_leg">
    <inertial>
      <origin xyz="-0.0132211 -0.022063200000000005 -0.12639599999999995" rpy="0 0 -1.5707963267948966"/>
      <mass value="5.0486"/>
      <inertia ixx="0.055232975130819865" ixy="0.00010074823751369491" ixz="0.0003051273226174697" iyy="0.0528235153766003" iyz="-0.00017980969334906494" izz="0.005756497863977997"/>
    </inertial>
    <visual>
      <origin xyz="-0.026050000000000243 -0.08344999999999969 0.5896580000000001" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_lower_leg_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="l_lower_leg_color">
        <color rgba="0.3 0.3 0.3 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="-0.026050000000000243 -0.08344999999999969 0.5896580000000001" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_lower_leg_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="l_knee" type="revolute">
    <origin xyz="0.015 0.024900000000000012 -0.10619999999999996" rpy="0 0 0"/>
    <axis xyz="0.0 -1.0 0.0"/>
    <parent link="l_upper_leg"/>
    <child link="l_lower_leg"/>
    <limit effort="50000" lower="-1.2217304763960306" upper="0.08726646259971647" velocity="120.0"/>
    <dynamics damping="2.0" friction="0.0"/>
  </joint>
  <link name="l_ankle_1">
    <inertial>
      <origin xyz="-0.0056708999999999996 -0.0004776000000000086 -0.015197000000000016" rpy="0 0 -1.5707963267948966"/>
      <mass value="0.36569"/>
      <inertia ixx="0.01" ixy="3.642221266176245e-05" ixz="6.0306968718434186e-05" iyy="0.01" iyz="-2.6804257582408287e-05" izz="0.01"/>
    </inertial>
    <visual>
      <origin xyz="-0.026050000000000052 -0.08295000000000005 0.8456470000000003" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_ankle_1_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="l_ankle_1_color">
        <color rgba="0.9 0.9 0.9 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="-0.026050000000000052 -0.08295000000000005 0.8456470000000003" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_ankle_1_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="l_ankle_pitch" type="revolute">
    <origin xyz="0 -0.0005000000000000004 -0.255989" rpy="0 0 0"/>
    <axis xyz="0.0 1.0 0.0"/>
    <parent link="l_lower_leg"/>
    <child link="l_ankle_1"/>
    <limit effort="50000" lower="-0.7853981633974483" upper="0.7853981633974483" velocity="120.0"/>
    <dynamics damping="2.0" friction="0.0"/>
  </joint>
  <link name="l_ankle_2">
    <inertial>
      <origin xyz="0.0417048 -5.500000000001337e-05 -0.02565099999999998" rpy="0 0 -1.5707963267948966"/>
      <mass value="1.377"/>
      <inertia ixx="0.01" ixy="5.191850067413837e-06" ixz="-3.6717389188505937e-06" iyy="0.01" iyz="0.0011063555447880678" izz="0.01"/>
    </inertial>
    <visual>
      <origin xyz="0.026699999999999342 -0.0647500000000001 0.8806470000000008" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_ankle_2_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="l_ankle_2_color">
        <color rgba="0.3 0.3 0.3 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0.026699999999999342 -0.0647500000000001 0.8806470000000008" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_ankle_2_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="l_ankle_roll" type="revolute">
    <origin xyz="-0.052750000000000005 -0.018200000000000008 -0.03500000000000003" rpy="0 0 0"/>
    <axis xyz="1.0 0.0 0.0"/>
    <parent link="l_ankle_1"/>
    <child link="l_ankle_2"/>
    <limit effort="50000" lower="-0.4363323129985824" upper="0.4363323129985824" velocity="120.0"/>
    <dynamics damping="2.0" friction="0.0"/>
  </joint>
  <link name="l_foot_front_ft"/>
  <joint name="l_foot_front_ft_fixed_joint" type="fixed">
    <origin xyz="0.13524999999999998 0 -0.04349999999999998" rpy="0 0 -2.094395210586932"/>
    <parent link="l_ankle_2"/>
    <child link="l_foot_front_ft"/>
    <dynamics damping="0.1"/>
  </joint>
  <link name="l_foot_rear_ft"/>
  <joint name="l_foot_rear_ft_fixed_joint" type="fixed">
    <origin xyz="0.016 0 -0.04349999999999998" rpy="0 0 -2.094395210586932"/>
    <parent link="l_ankle_2"/>
    <child link="l_foot_rear_ft"/>
    <dynamics damping="0.1"/>
  </joint>
  <link name="l_foot_rear">
    <inertial>
      <origin xyz="0.0005056000000000001 3.350000000000575e-05 0.003658999999999968" rpy="0 0 -1.5707963267948966"/>
      <mass value="0.17851"/>
      <inertia ixx="0.01" ixy="-3.8140209422126887e-07" ixz="3.3779281136245455e-06" iyy="0.01" iyz="8.141588837428499e-06" izz="0.01"/>
    </inertial>
    <visual>
      <origin xyz="0.010699999999999342 -0.06475000000000009 0.9464470000000004" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_foot_rear_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="l_foot_rear_color">
        <color rgba="1 0.51 0.0 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0.0 0.0 0.003" rpy="0.0 0.0 0.0"/>
      <geometry>
        <box size="0.117 0.1 0.006"/>
      </geometry>
    </collision>
  </link>
  <joint name="l_foot_rear_ft_sensor" type="fixed">
    <origin xyz="0.016 0 -0.06579999999999997" rpy="0 0 0"/>
    <parent link="l_ankle_2"/>
    <child link="l_foot_rear"/>
    <dynamics damping="0.1"/>
  </joint>
  <link name="l_sole"/>
  <joint name="l_sole_fixed_joint" type="fixed">
    <origin xyz="0.05963 0 0" rpy="0 0 0"/>
    <parent link="l_foot_rear"/>
    <child link="l_sole"/>
    <dynamics damping="0.1"/>
  </joint>
  <link name="l_foot_front">
    <inertial>
      <origin xyz="7.700000000000762e-05 -3.990000000000937e-05 0.004256999999999955" rpy="0 0 -1.5707963267948966"/>
      <mass value="0.17896"/>
      <inertia ixx="0.01" ixy="3.5467003209527003e-07" ixz="2.7144049104682455e-07" iyy="0.01" iyz="3.4960507170104935e-05" izz="0.01"/>
    </inertial>
    <visual>
      <origin xyz="-0.10855000000000063 -0.06475000000000009 0.9464470000000004" rpy="0 0 -1.5707963267948966"/>
      <geometry>
        <mesh filename="package://iCub/meshes/simmechanics/sim_icub3_l_foot_front_prt.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="l_foot_front_color">
        <color rgba="1 0.51 0.0 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0.0 0.0 0.003" rpy="0.0 0.0 0.0"/>
      <geometry>
        <box size="0.117 0.1 0.006"/>
      </geometry>
    </collision>
  </link>
  <joint name="l_foot_front_ft_sensor" type="fixed">
    <origin xyz="0.13524999999999998 0 -0.06579999999999997" rpy="0 0 0"/>
    <parent link="l_ankle_2"/>
    <child link="l_foot_front"/>
    <dynamics damping="0.1"/>
  </joint>
  <link name="base_link"/>
  <joint name="base_link_fixed_joint" type="fixed">
  <origin xyz="0 0 0" rpy="0 -0 0"/>
  <axis xyz="0 0 0"/>
  <parent link="root_link"/>
  <child link="base_link"/>
</joint>
  <gazebo reference="r_arm_ft_sensor">
<preserveFixedJoint>true</preserveFixedJoint>
<!-- For compatibility with SDFormat < 4.4 -->
<disableFixedJointLumping>true</disableFixedJointLumping>
</gazebo>
  <gazebo reference="l_arm_ft_sensor">
<preserveFixedJoint>true</preserveFixedJoint>
<!-- For compatibility with SDFormat < 4.4 -->
<disableFixedJointLumping>true</disableFixedJointLumping>
</gazebo>
  <gazebo>
<plugin name="controlboard_torso" filename="libgazebo_yarp_controlboard.so">
  <yarpConfigurationFile>model://iCub/conf_icub3/gazebo_icub_torso.ini</yarpConfigurationFile>
</plugin>
</gazebo>
  <gazebo reference="torso_pitch">
    <implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
  <gazebo reference="torso_roll">
    <implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
  <gazebo reference="torso_yaw">
    <implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
  <gazebo reference="r_shoulder_pitch">
    <implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
  <gazebo reference="r_shoulder_roll">
    <implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
  <gazebo reference="r_shoulder_yaw">
    <implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
  <gazebo reference="l_shoulder_pitch">
    <implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
  <gazebo reference="l_shoulder_roll">
    <implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
  <gazebo reference="l_shoulder_yaw">
    <implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
  <gazebo reference="l_leg_ft_sensor">
<preserveFixedJoint>true</preserveFixedJoint>
<!-- For compatibility with SDFormat < 4.4 -->
<disableFixedJointLumping>true</disableFixedJointLumping>
</gazebo>
  <gazebo reference="l_foot_front_ft_sensor">
<preserveFixedJoint>true</preserveFixedJoint>
<!-- For compatibility with SDFormat < 4.4 -->
<disableFixedJointLumping>true</disableFixedJointLumping>
</gazebo>
  <gazebo reference="l_foot_rear_ft_sensor">
<preserveFixedJoint>true</preserveFixedJoint>
<!-- For compatibility with SDFormat < 4.4 -->
<disableFixedJointLumping>true</disableFixedJointLumping>
</gazebo>
  <gazebo reference="l_foot_front">
  <collision> <surface> <bounce>
    <restitution_coefficient>0</restitution_coefficient>
    <threshold>100000</threshold>
  </bounce> </surface> </collision>
  <maxContacts>4</maxContacts>
  <kp>18000000</kp>
  <kd>100</kd>
  <maxVel>100</maxVel>
  <minDepth>0.0001</minDepth>
  <mu1>1</mu1>
  <mu2>1</mu2>
  <fdir1>0 0 0</fdir1>
</gazebo>
  <gazebo reference="l_foot_rear">
  <collision> <surface> <bounce>
    <restitution_coefficient>0</restitution_coefficient>
    <threshold>100000</threshold>
  </bounce> </surface> </collision>
  <maxContacts>4</maxContacts>
  <kp>18000000</kp>
  <kd>100</kd>
  <maxVel>100</maxVel>
  <minDepth>0.0001</minDepth>
  <mu1>1</mu1>
  <mu2>1</mu2>
  <fdir1>0 0 0</fdir1>
</gazebo>
  <gazebo>
<plugin name="controlboard_left_leg" filename="libgazebo_yarp_controlboard.so">
<yarpConfigurationFile>model://iCub/conf_icub3/gazebo_icub_left_leg.ini</yarpConfigurationFile>
</plugin>
</gazebo>
  <gazebo reference="l_hip_pitch">
    <implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
  <gazebo reference="l_hip_roll">
    <implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
  <gazebo reference="l_hip_yaw">
    <implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
  <gazebo reference="l_knee">
    <implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
  <gazebo reference="l_ankle_pitch">
    <implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
  <gazebo reference="l_ankle_roll">
    <implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
  <gazebo reference="r_leg_ft_sensor">
<preserveFixedJoint>true</preserveFixedJoint>
<!-- For compatibility with SDFormat < 4.4 -->
<disableFixedJointLumping>true</disableFixedJointLumping>
</gazebo>
  <gazebo reference="r_foot_front_ft_sensor">
<preserveFixedJoint>true</preserveFixedJoint>
<!-- For compatibility with SDFormat < 4.4 -->
<disableFixedJointLumping>true</disableFixedJointLumping>
</gazebo>
  <gazebo reference="r_foot_rear_ft_sensor">
<preserveFixedJoint>true</preserveFixedJoint>
<!-- For compatibility with SDFormat < 4.4 -->
<disableFixedJointLumping>true</disableFixedJointLumping>
</gazebo>
  <gazebo reference="r_foot_front">
  <collision> <surface> <bounce>
    <restitution_coefficient>0</restitution_coefficient>
    <threshold>100000</threshold>
  </bounce> </surface> </collision>
  <maxContacts>4</maxContacts>
  <kp>18000000</kp>
  <kd>100</kd>
  <maxVel>100</maxVel>
  <minDepth>0.0001</minDepth>
  <mu1>1</mu1>
  <mu2>1</mu2>
  <fdir1>0 0 0</fdir1>
</gazebo>
  <gazebo reference="r_foot_rear">
  <collision> <surface> <bounce>
    <restitution_coefficient>0</restitution_coefficient>
    <threshold>100000</threshold>
  </bounce> </surface> </collision>
  <maxContacts>4</maxContacts>
  <kp>18000000</kp>
  <kd>100</kd>
  <maxVel>100</maxVel>
  <minDepth>0.0001</minDepth>
  <mu1>1</mu1>
  <mu2>1</mu2>
  <fdir1>0 0 0</fdir1>
</gazebo>
  <gazebo>
<plugin name="controlboard_right_leg" filename="libgazebo_yarp_controlboard.so">
<yarpConfigurationFile>model://iCub/conf_icub3/gazebo_icub_right_leg.ini</yarpConfigurationFile>
</plugin>
</gazebo>
  <gazebo reference="r_hip_pitch">
    <implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
  <gazebo reference="r_hip_roll">
    <implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
  <gazebo reference="r_hip_yaw">
    <implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
  <gazebo reference="r_knee">
    <implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
  <gazebo reference="r_ankle_pitch">
    <implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
  <gazebo reference="r_ankle_roll">
    <implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
  <gazebo>
<plugin name="controlboard_head_without_eyes" filename="libgazebo_yarp_controlboard.so">
<yarpConfigurationFile>model://iCub/conf_icub3/gazebo_icub_head_without_eyes_V2_7.ini</yarpConfigurationFile>
</plugin>
</gazebo>
  <gazebo reference="neck_pitch">
    <implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
  <gazebo reference="neck_roll">
    <implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
  <gazebo reference="neck_yaw">
    <implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
  <gazebo>
<plugin name="controlboard_left_arm_no_hand" filename="libgazebo_yarp_controlboard.so">
<yarpConfigurationFile>model://iCub/conf_icub3/gazebo_icub_left_arm_no_hand_for_no_hand_model.ini</yarpConfigurationFile>
<initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
</plugin>
</gazebo>
  <gazebo reference="l_shoulder_pitch">
  <implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
  <gazebo reference="l_shoulder_roll">
  <implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
  <gazebo reference="l_shoulder_yaw">
  <implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
  <gazebo reference="l_elbow">
  <implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
  <gazebo reference="l_wrist_prosup">
  <implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
  <gazebo reference="l_wrist_pitch">
  <implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
  <gazebo reference="l_wrist_yaw">
  <implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
  <gazebo>
<plugin name="controlboard_right_arm_no_hand" filename="libgazebo_yarp_controlboard.so">
<yarpConfigurationFile>model://iCub/conf_icub3/gazebo_icub_right_arm_no_hand_for_no_hand_model.ini</yarpConfigurationFile>
<initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
</plugin>
</gazebo>
  <gazebo reference="r_shoulder_pitch">
  <implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
  <gazebo reference="r_shoulder_roll">
  <implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
  <gazebo reference="r_shoulder_yaw">
  <implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
  <gazebo reference="r_elbow">
  <implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
  <gazebo reference="r_wrist_prosup">
  <implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
  <gazebo reference="r_wrist_pitch">
  <implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
  <gazebo reference="r_wrist_yaw">
  <implicitSpringDamper>1</implicitSpringDamper>
</gazebo>
  <gazebo>
  <plugin name="robotinterface" filename="libgazebo_yarp_robotinterface.so">
    <yarpRobotInterfaceConfigurationFile>model://iCub/conf_icub3/icub.xml</yarpRobotInterfaceConfigurationFile>
  </plugin>
</gazebo>
  <gazebo reference="l_arm_ft_sensor">
    <sensor name="l_arm_ft" type="force_torque">
      <always_on>1</always_on>
      <update_rate>100</update_rate>
      <force_torque>
        <frame>sensor</frame>
        <measure_direction>child_to_parent</measure_direction>
      </force_torque>
      <pose>3.469446951953614e-18 0.0 0.0 3.141592646862139 -3.10468332187885e-08 -1.5707962571980942</pose>
      <plugin name="left_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
  <yarpConfigurationFile>model://iCub/conf_icub3/FT/gazebo_icub_left_arm_ft.ini</yarpConfigurationFile>
</plugin>
    </sensor>
  </gazebo>
  <sensor name="l_arm_ft" type="force_torque">
    <parent joint="l_arm_ft_sensor"/>
    <force_torque>
      <frame>sensor</frame>
      <measure_direction>child_to_parent</measure_direction>
    </force_torque>
    <origin rpy="3.141592646862139 -3.10468332187885e-08 -1.5707962571980942" xyz="3.469446951953614e-18 0.0 0.0"/>
  </sensor>
  <gazebo reference="r_arm_ft_sensor">
    <sensor name="r_arm_ft" type="force_torque">
      <always_on>1</always_on>
      <update_rate>100</update_rate>
      <force_torque>
        <frame>sensor</frame>
        <measure_direction>child_to_parent</measure_direction>
      </force_torque>
      <pose>3.469446951953614e-18 0.0 0.0 3.141592644717707 4.094299285979593e-08 1.5707964185756365</pose>
      <plugin name="right_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
  <yarpConfigurationFile>model://iCub/conf_icub3/FT/gazebo_icub_right_arm_ft.ini</yarpConfigurationFile>
</plugin>
    </sensor>
  </gazebo>
  <sensor name="r_arm_ft" type="force_torque">
    <parent joint="r_arm_ft_sensor"/>
    <force_torque>
      <frame>sensor</frame>
      <measure_direction>child_to_parent</measure_direction>
    </force_torque>
    <origin rpy="3.141592644717707 4.094299285979593e-08 1.5707964185756365" xyz="3.469446951953614e-18 0.0 0.0"/>
  </sensor>
  <gazebo reference="l_leg_ft_sensor">
    <sensor name="l_leg_ft" type="force_torque">
      <always_on>1</always_on>
      <update_rate>100</update_rate>
      <force_torque>
        <frame>sensor</frame>
        <measure_direction>child_to_parent</measure_direction>
      </force_torque>
      <pose>0.0 0.0 0.0 3.141592653589793 -0.0 2.6179934742070086</pose>
      <plugin name="left_leg_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
  <yarpConfigurationFile>model://iCub/conf_icub3/FT/gazebo_icub_left_leg_ft.ini</yarpConfigurationFile>
</plugin>
    </sensor>
  </gazebo>
  <sensor name="l_leg_ft" type="force_torque">
    <parent joint="l_leg_ft_sensor"/>
    <force_torque>
      <frame>sensor</frame>
      <measure_direction>child_to_parent</measure_direction>
    </force_torque>
    <origin rpy="3.141592653589793 -0.0 2.6179934742070086" xyz="0.0 0.0 0.0"/>
  </sensor>
  <gazebo reference="l_foot_front_ft_sensor">
    <sensor name="l_foot_front_ft" type="force_torque">
      <always_on>1</always_on>
      <update_rate>100</update_rate>
      <force_torque>
        <frame>sensor</frame>
        <measure_direction>parent_to_child</measure_direction>
      </force_torque>
      <pose>0.0 0.0 0.022299999999999986 0.0 -0.0 -2.094395210586932</pose>
      <plugin name="left_foot_front_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
  <yarpConfigurationFile>model://iCub/conf_icub3/FT/gazebo_icub_left_foot_front_ft.ini</yarpConfigurationFile>
</plugin>
    </sensor>
  </gazebo>
  <sensor name="l_foot_front_ft" type="force_torque">
    <parent joint="l_foot_front_ft_sensor"/>
    <force_torque>
      <frame>sensor</frame>
      <measure_direction>parent_to_child</measure_direction>
    </force_torque>
    <origin rpy="0.0 -0.0 -2.094395210586932" xyz="0.0 0.0 0.022299999999999986"/>
  </sensor>
  <gazebo reference="l_foot_rear_ft_sensor">
    <sensor name="l_foot_rear_ft" type="force_torque">
      <always_on>1</always_on>
      <update_rate>100</update_rate>
      <force_torque>
        <frame>sensor</frame>
        <measure_direction>parent_to_child</measure_direction>
      </force_torque>
      <pose>0.0 0.0 0.022299999999999986 0.0 -0.0 -2.094395210586932</pose>
      <plugin name="left_foot_rear_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
  <yarpConfigurationFile>model://iCub/conf_icub3/FT/gazebo_icub_left_foot_rear_ft.ini</yarpConfigurationFile>
</plugin>
    </sensor>
  </gazebo>
  <sensor name="l_foot_rear_ft" type="force_torque">
    <parent joint="l_foot_rear_ft_sensor"/>
    <force_torque>
      <frame>sensor</frame>
      <measure_direction>parent_to_child</measure_direction>
    </force_torque>
    <origin rpy="0.0 -0.0 -2.094395210586932" xyz="0.0 0.0 0.022299999999999986"/>
  </sensor>
  <gazebo reference="r_leg_ft_sensor">
    <sensor name="r_leg_ft" type="force_torque">
      <always_on>1</always_on>
      <update_rate>100</update_rate>
      <force_torque>
        <frame>sensor</frame>
        <measure_direction>child_to_parent</measure_direction>
      </force_torque>
      <pose>0.0 0.0 0.0 3.141592653589793 0.0 -2.6179934742070086</pose>
      <plugin name="right_leg_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
  <yarpConfigurationFile>model://iCub/conf_icub3/FT/gazebo_icub_right_leg_ft.ini</yarpConfigurationFile>
</plugin>
    </sensor>
  </gazebo>
  <sensor name="r_leg_ft" type="force_torque">
    <parent joint="r_leg_ft_sensor"/>
    <force_torque>
      <frame>sensor</frame>
      <measure_direction>child_to_parent</measure_direction>
    </force_torque>
    <origin rpy="3.141592653589793 0.0 -2.6179934742070086" xyz="0.0 0.0 0.0"/>
  </sensor>
  <gazebo reference="r_foot_front_ft_sensor">
    <sensor name="r_foot_front_ft" type="force_torque">
      <always_on>1</always_on>
      <update_rate>100</update_rate>
      <force_torque>
        <frame>sensor</frame>
        <measure_direction>parent_to_child</measure_direction>
      </force_torque>
      <pose>0.0 0.0 0.022299999999999986 0.0 -0.0 -2.094395210586932</pose>
      <plugin name="right_foot_front_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
  <yarpConfigurationFile>model://iCub/conf_icub3/FT/gazebo_icub_right_foot_front_ft.ini</yarpConfigurationFile>
</plugin>
    </sensor>
  </gazebo>
  <sensor name="r_foot_front_ft" type="force_torque">
    <parent joint="r_foot_front_ft_sensor"/>
    <force_torque>
      <frame>sensor</frame>
      <measure_direction>parent_to_child</measure_direction>
    </force_torque>
    <origin rpy="0.0 -0.0 -2.094395210586932" xyz="0.0 0.0 0.022299999999999986"/>
  </sensor>
  <gazebo reference="r_foot_rear_ft_sensor">
    <sensor name="r_foot_rear_ft" type="force_torque">
      <always_on>1</always_on>
      <update_rate>100</update_rate>
      <force_torque>
        <frame>sensor</frame>
        <measure_direction>parent_to_child</measure_direction>
      </force_torque>
      <pose>0.0 0.0 0.022299999999999986 0.0 -0.0 -2.094395210586932</pose>
      <plugin name="right_foot_rear_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
  <yarpConfigurationFile>model://iCub/conf_icub3/FT/gazebo_icub_right_foot_rear_ft.ini</yarpConfigurationFile>
</plugin>
    </sensor>
  </gazebo>
  <sensor name="r_foot_rear_ft" type="force_torque">
    <parent joint="r_foot_rear_ft_sensor"/>
    <force_torque>
      <frame>sensor</frame>
      <measure_direction>parent_to_child</measure_direction>
    </force_torque>
    <origin rpy="0.0 -0.0 -2.094395210586932" xyz="0.0 0.0 0.022299999999999986"/>
  </sensor>
  <gazebo reference="head">
    <sensor name="head_imu_0" type="imu">
      <always_on>1</always_on>
      <update_rate>100</update_rate>
      <pose>0.0035000000000000005 0.0055 0.10133400000000001 3.141592653589793 -0.0 -1.5707963267948966</pose>
      <plugin name="iCub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
   <yarpConfigurationFile>model://iCub/conf_icub3/gazebo_icub_inertial.ini</yarpConfigurationFile>
</plugin>
    </sensor>
  </gazebo>
  <sensor name="head_imu_0" type="accelerometer">
    <parent link="head"/>
    <origin rpy="3.141592653589793 -0.0 -1.5707963267948966" xyz="0.0035000000000000005 0.0055 0.10133400000000001"/>
  </sensor>
  <gazebo reference="chest">
    <sensor name="realsense_chest_depth" type="depth">
      <always_on>1</always_on>
      <update_rate>30</update_rate>
      <pose>0.0894115 0.0175 0.09929950000000001 -1.5707963267948968 -0.0 -1.5707963267948968</pose>
      <camera name="intel_realsense_depth_camera">
  <pose>0 0 0 -1.57079 -1.57079 3.14159</pose>
  <horizontal_fov>1.57079</horizontal_fov>
  <distortion>
    <k1>0</k1>
    <k2>0</k2>
    <k3>0</k3>
    <p1>0</p1>
    <p2>0</p2>
    <center>319.5 239.5</center>
  </distortion>
  <image>
    <width>640</width>
    <height>480</height>
    <format>R8G8B8</format>
  </image>
  <clip>
    <near>0.175</near>
    <far>3000</far>
  </clip>
</camera>
      <visualize>false</visualize>
      <plugin name="iCub_yarp_gazebo_plugin_depthCamera" filename="libgazebo_yarp_depthCamera.so">
    <yarpConfigurationFile>model://iCub/conf_icub3/gazebo_icub_depth_camera.ini</yarpConfigurationFile>
</plugin>
    </sensor>
  </gazebo>
  <sensor name="realsense_chest_depth" type="depth">
    <parent link="chest"/>
    <origin rpy="-1.5707963267948968 -0.0 -1.5707963267948968" xyz="0.0894115 0.0175 0.09929950000000001"/>
  </sensor>
  <gazebo reference="chest">
    <sensor name="realsense_chest_rgb" type="camera">
      <always_on>1</always_on>
      <update_rate>30</update_rate>
      <pose>0.0894115 -0.0115 0.09929950000000001 -1.5707963267948968 -0.0 -1.5707963267948968</pose>
      <camera name="intel_realsense_rgb_camera">
  <pose>0 0 0 -1.57079 -1.57079 3.14159</pose>
  <horizontal_fov>1.2217</horizontal_fov>
  <distortion>
    <k1>0</k1>
    <k2>0</k2>
    <k3>0</k3>
    <p1>0</p1>
    <p2>0</p2>
    <center>319.5 239.5</center>
  </distortion>
  <image>
    <width>640</width>
    <height>480</height>
    <format>R8G8B8</format>
  </image>
  <clip>
    <near>0.02</near>
    <far>3000</far>
  </clip>
</camera>
      <visualize>false</visualize>
      <plugin name="iCub_yarp_gazebo_plugin_camera" filename="libgazebo_yarp_camera.so">
    <yarpConfigurationFile>model://iCub/conf_icub3/gazebo_icub_rgb_camera.ini</yarpConfigurationFile>
</plugin>
    </sensor>
  </gazebo>
  <sensor name="realsense_chest_rgb" type="camera">
    <parent link="chest"/>
    <origin rpy="-1.5707963267948968 -0.0 -1.5707963267948968" xyz="0.0894115 -0.0115 0.09929950000000001"/>
  </sensor>
  <gazebo reference="chest">
    <sensor name="chest_imu_0" type="imu">
      <always_on>1</always_on>
      <update_rate>100</update_rate>
      <pose>0.0894115 0.0175 0.09929950000000001 -1.5707963267948968 -0.0 -1.5707963267948968</pose>
      <plugin name="iCub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
    <yarpConfigurationFile>model://iCub/conf_icub3/gazebo_icub_chest_inertial.ini</yarpConfigurationFile>
</plugin>
    </sensor>
  </gazebo>
  <sensor name="chest_imu_0" type="accelerometer">
    <parent link="chest"/>
    <origin rpy="-1.5707963267948968 -0.0 -1.5707963267948968" xyz="0.0894115 0.0175 0.09929950000000001"/>
  </sensor>
  <gazebo>
    <pose>0.0 0.0 0.64 0.0 0.0 0.0</pose>
  </gazebo>
</robot>