<robot name="iCubGenova02">
  <link name="root_link">
    <inertial>
      <origin xyz="0.0248821 0.000103947 -0.0440806" rpy="1.5707963267948961 0 -1.5707963267948961"/>
      <mass value="5.09143"/>
      <inertia ixx="0.0139679" ixy="-5.93488e-06" ixz="-1.55458e-05" iyy="0.0202112" iyz="-0.00170968" izz="0.0249124"/>
    </inertial>
    <visual>
      <origin xyz="-0.0364 0 0.032" rpy="1.5707963267948961 0 -1.5707963267948961"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/root.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="green">
        <color rgba="0 1 0 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="-0.0364 0 0.032" rpy="1.5707963267948961 0 -1.5707963267948961"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/root.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <link name="l_hip_1">
    <inertial>
      <origin xyz="0.041678 -6.3e-05 -0.0004013" rpy="0 0 0"/>
      <mass value="0.915608"/>
      <inertia ixx="0.000402555" ixy="1.07708e-06" ixz="-1.569e-06" iyy="0.000570806" iyz="-6.5378e-07" izz="0.00057937"/>
    </inertial>
    <visual>
      <origin xyz="-0.0223861 0.151913 0.0428515000001" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/l_hip_1.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="black">
        <color rgba="0 0 0 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="-0.0223861 0.151913 0.0428515000001" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/l_hip_1.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="l_hip_pitch" type="revolute">
    <origin xyz="0.0064515 -0.0223861 -0.119913" rpy="1.5707963267948961 0 -1.5707963267948961"/>
    <axis xyz="-1.0 0.0 0.0"/>
    <parent link="root_link"/>
    <child link="l_hip_1"/>
    <limit effort="50000" lower="-0.5235987755982988" upper="1.4835298641951802" velocity="50000"/>
    <dynamics damping="0.223"/>
  </joint>
  <link name="l_hip_2">
    <inertial>
      <origin xyz="-1.94e-05 -0.044047 0.0239587" rpy="0 0 0"/>
      <mass value="0.421161"/>
      <inertia ixx="0.000494475" ixy="1.54783e-07" ixz="-1.86267e-07" iyy="0.000288821" iyz="1.53903e-05" izz="0.000319114"/>
    </inertial>
    <visual>
      <origin xyz="-0.0701 0.151913 0.0683914998093" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/l_hip_2.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="red">
        <color rgba="1 0 0 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="-0.0701 0.151913 0.0683914998093" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/l_hip_2.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="l_hip_roll" type="revolute">
    <origin xyz="0.0477139 0 -0.02554" rpy="0 0 0"/>
    <axis xyz="0.0 0.0 1.0"/>
    <parent link="l_hip_1"/>
    <child link="l_hip_2"/>
    <limit effort="50000" lower="0.0" upper="1.5707963267948966" velocity="50000"/>
    <dynamics damping="0.223"/>
  </joint>
  <link name="l_hip_3">
    <inertial>
      <origin xyz="0.0002823 -0.0001229 0.008718" rpy="-1.5707963267948966 0 0"/>
      <mass value="0.126212"/>
      <inertia ixx="3.27938e-05" ixy="2.67901e-07" ixz="-4.41463e-09" iyy="5.69957e-05" iyz="3.32338e-08" izz="3.26449e-05"/>
    </inertial>
    <visual>
      <origin xyz="-0.0701 0.043409 -0.226213" rpy="-1.5707963267948966 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/l_hip_3.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="blue">
        <color rgba="0 0 1 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="-0.0701 0.043409 -0.226213" rpy="-1.5707963267948966 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/l_hip_3.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="l_leg_ft_sensor" type="fixed">
    <origin xyz="0 -0.0743 0.0246825" rpy="1.5707963267948961 -1.570796326794896 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.0001157 -0.078858 0.0033089" rpy="0 0 0"/>
      <mass value="2.20843"/>
      <inertia ixx="0.00791838" ixy="-0.000140684" ixz="-4.64418e-06" iyy="0.00176919" iyz="-0.000167361" izz="0.00791102"/>
    </inertial>
    <visual>
      <origin xyz="-0.0701 0.241013 0.043409" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/l_upper_leg.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="yellow">
        <color rgba="1 1 0 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="-0.0701 0.241013 0.043409" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/l_upper_leg.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="l_hip_yaw" type="revolute">
    <origin xyz="0 0 0.0146" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
    <axis xyz="0.0 1.0 0.0"/>
    <parent link="l_hip_3"/>
    <child link="l_upper_leg"/>
    <limit effort="50000" lower="-1.2217304763960306" upper="1.2217304763960306" velocity="50000"/>
    <dynamics damping="0.223"/>
  </joint>
  <link name="l_lower_leg">
    <inertial>
      <origin xyz="0.0273768 -0.088559 0.000854" rpy="0 0 0"/>
      <mass value="1.87237"/>
      <inertia ixx="0.00540418" ixy="-0.000305905" ixz="8.58509e-05" iyy="0.0019275" iyz="-0.000293159" izz="0.00583249"/>
    </inertial>
    <visual>
      <origin xyz="-0.0463168001907 0.386638 0.043409" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/l_lower_leg.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="pink">
        <color rgba="1 0 1 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="-0.0463168001907 0.386638 0.043409" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/l_lower_leg.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="l_knee" type="revolute">
    <origin xyz="-0.0237832 -0.145625 0" rpy="0 0 0"/>
    <axis xyz="-1.0 0.0 0.0"/>
    <parent link="l_upper_leg"/>
    <child link="l_lower_leg"/>
    <limit effort="50000" lower="-1.7453292519943295" upper="0.0" velocity="50000"/>
    <dynamics damping="0.223"/>
  </joint>
  <link name="l_ankle_1">
    <inertial>
      <origin xyz="-0.0345937 6.3e-05 0.0152606" rpy="0 0 0"/>
      <mass value="0.879758"/>
      <inertia ixx="0.000524207" ixy="-1.45611e-06" ixz="3.96996e-05" iyy="0.000580223" iyz="3.73998e-06" izz="0.000400428"/>
    </inertial>
    <visual>
      <origin xyz="-0.105600000191 0.587138 0.043409" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/l_ankle_1.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="cyan">
        <color rgba="0 1 1 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="-0.105600000191 0.587138 0.043409" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/l_ankle_1.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="l_ankle_pitch" type="revolute">
    <origin xyz="0.0592832 -0.201 0" rpy="0 0 0"/>
    <axis xyz="1.0 0.0 0.0"/>
    <parent link="l_lower_leg"/>
    <child link="l_ankle_1"/>
    <limit effort="50000" lower="-0.3490658503988659" upper="0.5235987755982988" velocity="50000"/>
    <dynamics damping="0.223"/>
  </joint>
  <link name="l_ankle_2">
    <inertial>
      <origin xyz="-8.19999999999e-06 -0.038202 -0.04732025" rpy="0 0 0"/>
      <mass value="0.267485"/>
      <inertia ixx="0.000335682" ixy="-7.53335e-08" ixz="2.93681e-08" iyy="0.00025387" iyz="-4.26218e-07" izz="0.000154833"/>
    </inertial>
    <visual>
      <origin xyz="-0.0701000001907 0.587138 -0.00467855" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/l_ankle_2.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="green"/>
    </visual>
    <collision>
      <origin xyz="-0.0701000001907 0.587138 -0.00467855" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/l_ankle_2.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="l_ankle_roll" type="revolute">
    <origin xyz="-0.0355 0 0.04808755" rpy="0 0 0"/>
    <axis xyz="0.0 0.0 1.0"/>
    <parent link="l_ankle_1"/>
    <child link="l_ankle_2"/>
    <limit effort="50000" lower="-0.3490658503988659" upper="0.3490658503988659" velocity="50000"/>
    <dynamics damping="0.223"/>
  </joint>
  <link name="l_foot">
    <inertial>
      <origin xyz="0.0336906 -0.0053662 -0.001154" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
      <mass value="0.382372"/>
      <inertia ixx="0.00121061" ixy="-3.65345e-07" ixz="5.78649e-05" iyy="0.00141515" iyz="-7.67241e-06" izz="0.000273665"/>
    </inertial>
    <visual>
      <origin xyz="0.046909 0.0701000001907 -0.647438" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/l_foot.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="white">
        <color rgba="1 1 1 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0.046909 0.0701000001907 -0.647438" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/l_foot.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="l_foot_ft_sensor" type="fixed">
    <origin xyz="0 -0.0603 -0.05158755" rpy="1.5707963267948961 -1.570796326794896 0"/>
    <parent link="l_ankle_2"/>
    <child link="l_foot"/>
    <dynamics damping="0.1"/>
  </joint>
  <link name="torso_1">
    <inertial>
      <origin xyz="0.0270921359 0.02914798 -0.0004398" rpy="0 0 0"/>
      <mass value="0.67144"/>
      <inertia ixx="0.00040911" ixy="-1.04522e-06" ixz="-1.02516e-06" iyy="0.000384141" iyz="6.50708e-06" izz="0.000299445"/>
    </inertial>
    <visual>
      <origin xyz="0.0270462998093 0.032 0.0364" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/torso_1.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dblue">
        <color rgba="0 0 0.8 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0.0270462998093 0.032 0.0364" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/torso_1.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="torso_pitch" type="revolute">
    <origin xyz="0 0.0270463 0" rpy="1.5707963267948961 0 -1.5707963267948961"/>
    <axis xyz="1.0 0.0 0.0"/>
    <parent link="root_link"/>
    <child link="torso_1"/>
    <limit effort="50000" lower="-0.3490658503988659" upper="1.2217304763960306" velocity="50000"/>
    <dynamics damping="0.06"/>
  </joint>
  <link name="torso_2">
    <inertial>
      <origin xyz="-0.000112850809265 0.036182 0.0576606" rpy="0 0 0"/>
      <mass value="0.428161"/>
      <inertia ixx="0.000833014" ixy="2.2275e-08" ixz="8.1931e-08" iyy="0.000440576" iyz="-6.00823e-07" izz="0.000605488"/>
    </inertial>
    <visual>
      <origin xyz="0 0.1433 0.0389982" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/torso_2.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dgreen">
        <color rgba="0.1 0.8 0.1 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0 0.1433 0.0389982" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/torso_2.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="torso_roll" type="revolute">
    <origin xyz="0.0270462998093 0.032 -0.0576906" rpy="0 0 0"/>
    <axis xyz="0.0 0.0 1.0"/>
    <parent link="torso_1"/>
    <child link="torso_2"/>
    <limit effort="50000" lower="-0.5235987755982988" upper="0.5235987755982988" velocity="50000"/>
    <dynamics damping="0.06"/>
  </joint>
  <link name="chest">
    <inertial>
      <origin xyz="0.000188619190735 0.073091 -0.047238" rpy="0 0 0"/>
      <mass value="8.63802"/>
      <inertia ixx="0.0486666" ixy="3.29185e-05" ixz="0.000205812" iyy="0.0498553" iyz="-0.000485288" izz="0.0294678"/>
    </inertial>
    <visual>
      <origin xyz="0 0.0928 -0.0241889" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/bb_chest.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="gray">
        <color rgba="0.5 0.5 0.5 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0 0.0928 -0.0241889" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/bb_chest.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="torso_yaw" type="revolute">
    <origin xyz="0 0.05 0.0631871" rpy="0 0 0"/>
    <axis xyz="0.0 -1.0 0.0"/>
    <parent link="torso_2"/>
    <child link="chest"/>
    <limit effort="50000" lower="-0.8726646259971648" upper="0.8726646259971648" velocity="50000"/>
    <dynamics damping="0.06"/>
  </joint>
  <link name="l_shoulder_1">
    <inertial>
      <origin xyz="-0.008985 -3.3e-05 -0.0088999" rpy="0 0 0"/>
      <mass value="0.114882"/>
      <inertia ixx="2.24676e-05" ixy="-1.77064e-08" ixz="3.55951e-06" iyy="2.29712e-05" iyz="6.57597e-08" izz="1.00139e-05"/>
    </inertial>
    <visual>
      <origin xyz="-0.120257482 0 -0.032223" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/l_shoulder_1.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="green"/>
    </visual>
    <collision>
      <origin xyz="-0.120257482 0 -0.032223" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/l_shoulder_1.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="l_shoulder_pitch" type="revolute">
    <origin xyz="0.120016000191 0.0928 0.0079694" rpy="0 0 0"/>
    <axis xyz="0.965926 0.0 0.258819"/>
    <parent link="chest"/>
    <child link="l_shoulder_1"/>
    <limit effort="50000" lower="-1.6667894356545847" upper="0.17453292519943295" velocity="50000"/>
    <dynamics damping="0.06"/>
  </joint>
  <link name="l_shoulder_2">
    <inertial>
      <origin xyz="0.0214745701273 0.000268983781583 -0.0194106428522" rpy="0 0 0"/>
      <mass value="0.239074"/>
      <inertia ixx="0.000159206" ixy="1.51033e-07" ixz="-1.04783e-05" iyy="0.000207209" iyz="1.20634e-06" izz="0.000105106"/>
    </inertial>
    <visual>
      <origin xyz="-0.106586716581 1.3668283029e-07 -0.0407432874697" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/l_shoulder_2.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="black"/>
    </visual>
    <collision>
      <origin xyz="-0.106586716581 1.3668283029e-07 -0.0407432874697" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/l_shoulder_2.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="l_shoulder_roll" type="revolute">
    <origin xyz="-0.013671 0 0.0085203" rpy="0.2705973179159596 0.25268017754236044 -1.5015568855866173"/>
    <axis xyz="-0.258819 2.77555756156e-17 0.965926"/>
    <parent link="l_shoulder_1"/>
    <child link="l_shoulder_2"/>
    <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
    <dynamics damping="0.06"/>
  </joint>
  <link name="l_shoulder_3">
    <inertial>
      <origin xyz="0.0269780932383 -1.57895162464e-05 0.0147437513276" rpy="0 0 0"/>
      <mass value="0.35913"/>
      <inertia ixx="0.00014245" ixy="-2.22441e-07" ixz="-4.95846e-05" iyy="0.000197807" iyz="1.07538e-07" izz="0.000151706"/>
    </inertial>
    <visual>
      <origin xyz="-0.131721376204 0 -0.0352946238819" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/l_shoulder_3.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="red"/>
    </visual>
    <collision>
      <origin xyz="-0.131721376204 0 -0.0352946238819" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/l_shoulder_3.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="l_shoulder_yaw" type="revolute">
    <origin xyz="0.0251346596233 1.37517196103e-07 -0.00544866358786" rpy="0 0 0"/>
    <axis xyz="0.965925849201 6.1184868283e-09 0.258818959594"/>
    <parent link="l_shoulder_2"/>
    <child link="l_shoulder_3"/>
    <limit effort="50000" lower="-0.6457718232379019" upper="1.3962634015954636" velocity="50000"/>
    <dynamics damping="0.06"/>
  </joint>
  <link name="l_upper_arm">
    <inertial>
      <origin xyz="-0.00612078047688 -0.000188240813345 0.0293684" rpy="0 -1.30899702752112 -3.141592556630375"/>
      <mass value="0.746799"/>
      <inertia ixx="0.000509731" ixy="-6.90767e-07" ixz="1.20048e-05" iyy="0.00067086" iyz="3.11833e-06" izz="0.000601202"/>
    </inertial>
    <visual>
      <origin xyz="-0.0149998416451 2.50198270685e-07 -0.1933" rpy="0 -1.30899702752112 -3.141592556630375"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/l_upper_arm.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="blue"/>
    </visual>
    <collision>
      <origin xyz="-0.0149998416451 2.50198270685e-07 -0.1933" rpy="0 -1.30899702752112 -3.141592556630375"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/l_upper_arm.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="l_arm_ft_sensor" type="fixed">
    <origin xyz="0.0511098470381 2.52343498361e-07 0.0292238157865" rpy="4.5319568084916467e-07 -1.3089970275210927 3.1415921907414344"/>
    <parent link="l_shoulder_3"/>
    <child link="l_upper_arm"/>
    <dynamics damping="0.1"/>
  </joint>
  <link name="l_elbow_1">
    <inertial>
      <origin xyz="0.0904387274328 0.0298502110737 0.00921453059294" rpy="0 0 0"/>
      <mass value="1.17718"/>
      <inertia ixx="0.00107783" ixy="0.000310534" ixz="-0.00156091" iyy="0.00689571" iyz="0.000135264" izz="0.00628941"/>
    </inertial>
    <visual>
      <origin xyz="-0.259390480281 0.0339999483795 -0.0850325273781" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/l_elbow_1.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="yellow"/>
    </visual>
    <collision>
      <origin xyz="-0.259390480281 0.0339999483795 -0.0850325273781" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/l_elbow_1.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="l_elbow" type="revolute">
    <origin xyz="0 0.0340002020424 0.07926" rpy="0 -1.30899702752112 -3.141592556630374"/>
    <axis xyz="5.91000555439e-09 -1.00000015612 1.58358068325e-09"/>
    <parent link="l_upper_arm"/>
    <child link="l_elbow_1"/>
    <limit effort="50000" lower="0.2617993877991494" upper="1.8500490071139892" velocity="50000"/>
    <dynamics damping="0.06"/>
  </joint>
  <link name="r_shoulder_1">
    <inertial>
      <origin xyz="-0.0122656514133 3.2e-05 -0.00321072950703" rpy="0 0 0"/>
      <mass value="0.114862"/>
      <inertia ixx="2.24541e-05" ixy="-1.65173e-08" ixz="-3.55626e-06" iyy="2.29582e-05" iyz="-6.13337e-08" izz="1.00116e-05"/>
    </inertial>
    <visual>
      <origin xyz="0.0990078953076 0 -0.0265293007173" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/r_shoulder_1.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="pink"/>
    </visual>
    <collision>
      <origin xyz="0.0990078953076 0 -0.0265293007173" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/r_shoulder_1.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="r_shoulder_pitch" type="revolute">
    <origin xyz="-0.120015999809 0.0928 0.0079694" rpy="0 -0.523598883792035 3.141592653589793"/>
    <axis xyz="-0.965925926173 0.0 0.258819275525"/>
    <parent link="chest"/>
    <child link="r_shoulder_1"/>
    <limit effort="50000" lower="-1.6667894356545847" upper="0.17453292519943295" velocity="50000"/>
    <dynamics damping="0.06"/>
  </joint>
  <link name="r_shoulder_2">
    <inertial>
      <origin xyz="-0.0214141959438 0.000271699390655 -0.0194403277547" rpy="0 0 0"/>
      <mass value="0.238162"/>
      <inertia ixx="0.000158626" ixy="-1.12091e-07" ixz="1.02574e-05" iyy="0.000206526" iyz="1.19785e-06" izz="0.000104956"/>
    </inertial>
    <visual>
      <origin xyz="0.106587808568 2.31914715418e-07 -0.0407392121175" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/r_shoulder_2.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="cyan"/>
    </visual>
    <collision>
      <origin xyz="0.106587808568 2.31914715418e-07 -0.0407392121175" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/r_shoulder_2.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="r_shoulder_roll" type="revolute">
    <origin xyz="-0.00758046573227 0 0.0142102463639" rpy="-0.2705976115829963 -0.252680154648383 -1.5015568003498745"/>
    <axis xyz="-0.258819 -2.77555756156e-17 -0.965926"/>
    <parent link="r_shoulder_1"/>
    <child link="r_shoulder_2"/>
    <limit effort="50000" lower="0.0" upper="2.8064894372068823" velocity="50000"/>
    <dynamics damping="0.06"/>
  </joint>
  <link name="r_shoulder_3">
    <inertial>
      <origin xyz="-0.027209298157 -6.97407353582e-06 0.0148197627314" rpy="0 0 0"/>
      <mass value="0.35913"/>
      <inertia ixx="0.000142333" ixy="3.60361e-07" ixz="4.94445e-05" iyy="0.000197746" iyz="2.65919e-07" izz="0.000151821"/>
    </inertial>
    <visual>
      <origin xyz="0.131486656223 0 -0.0352317308747" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/r_shoulder_3.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="green"/>
    </visual>
    <collision>
      <origin xyz="0.131486656223 0 -0.0352317308747" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/r_shoulder_3.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="r_shoulder_yaw" type="revolute">
    <origin xyz="-0.024898847655 2.32747594447e-07 -0.00550748124286" rpy="0 0 0"/>
    <axis xyz="0.965925849201 -6.1184868283e-09 -0.258818959594"/>
    <parent link="r_shoulder_2"/>
    <child link="r_shoulder_3"/>
    <limit effort="50000" lower="-0.6457718232379019" upper="1.3962634015954636" velocity="50000"/>
    <dynamics damping="0.06"/>
  </joint>
  <link name="r_upper_arm">
    <inertial>
      <origin xyz="-0.00608688760704 0.000125888320419 0.0293121" rpy="0 1.308997027521121 0"/>
      <mass value="0.745104"/>
      <inertia ixx="0.000509527" ixy="1.19292e-06" ixz="-1.17426e-05" iyy="0.000668487" iyz="1.13363e-06" izz="0.000599254"/>
    </inertial>
    <visual>
      <origin xyz="-0.0149998416451 -2.5019827074e-07 -0.1933" rpy="0 1.308997027521121 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/r_upper_arm.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="-0.0149998416451 -2.5019827074e-07 -0.1933" rpy="0 1.308997027521121 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/r_upper_arm.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="r_arm_ft_sensor" type="fixed">
    <origin xyz="-0.0513445670194 2.52344985144e-07 0.0292867087936" rpy="-4.5319568070266037e-07 -1.3089970275210934 4.628483589970802e-07"/>
    <parent link="r_shoulder_3"/>
    <child link="r_upper_arm"/>
    <dynamics damping="0.1"/>
  </joint>
  <link name="r_elbow_1">
    <inertial>
      <origin xyz="-0.0905004145943 0.0184200801184 0.0091852237989" rpy="0 0 0"/>
      <mass value="1.17637"/>
      <inertia ixx="0.00107282" ixy="-0.000311989" ixz="0.00155336" iyy="0.00689615" iyz="0.000137135" izz="0.00629364"/>
    </inertial>
    <visual>
      <origin xyz="0.259390488255 0.0224999021101 -0.0850324973492" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/r_elbow_1.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dblue"/>
    </visual>
    <collision>
      <origin xyz="0.259390488255 0.0224999021101 -0.0850324973492" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/r_elbow_1.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="r_elbow" type="revolute">
    <origin xyz="0 -0.0225001557729 0.07926" rpy="0 1.308997027521121 0"/>
    <axis xyz="5.91000555439e-09 1.00000015612 -1.58358068325e-09"/>
    <parent link="r_upper_arm"/>
    <child link="r_elbow_1"/>
    <limit effort="50000" lower="0.2617993877991494" upper="1.8500490071139892" velocity="50000"/>
    <dynamics damping="0.06"/>
  </joint>
  <link name="neck_1">
    <inertial>
      <origin xyz="0.0143705608 0.006567 -0.0063841" rpy="0 0 0"/>
      <mass value="0.125793"/>
      <inertia ixx="4.0647e-05" ixy="8.09156e-08" ixz="-3.74998e-08" iyy="3.881e-05" iyz="9.39717e-06" izz="4.7535e-05"/>
    </inertial>
    <visual>
      <origin xyz="0.0142999998093 -0.079997 -0.0295007" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/neck_1.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dgreen"/>
    </visual>
    <collision>
      <origin xyz="0.0142999998093 -0.079997 -0.0295007" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/neck_1.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="neck_pitch" type="revolute">
    <origin xyz="-0.0142999998093 0.172797 0.0053118" rpy="0 0 0"/>
    <axis xyz="-1.0 0.0 0.0"/>
    <parent link="chest"/>
    <child link="neck_1"/>
    <limit effort="50000" lower="-0.6981317007977318" upper="0.3839724354387525" velocity="50000"/>
    <dynamics damping="0.06"/>
  </joint>
  <link name="neck_2">
    <inertial>
      <origin xyz="-8.8573009265e-05 0.035225 -0.01954521" rpy="0 0 0"/>
      <mass value="0.177087"/>
      <inertia ixx="0.000117943" ixy="-5.25591e-07" ixz="4.68307e-07" iyy="4.04013e-05" iyz="9.81476e-06" izz="0.000105186"/>
    </inertial>
    <visual>
      <origin xyz="0 -0.089497 -0.05030071" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/neck_2.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="gray"/>
    </visual>
    <collision>
      <origin xyz="0 -0.089497 -0.05030071" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/neck_2.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="neck_roll" type="revolute">
    <origin xyz="0.0142999998093 0.0095 0.02080001" rpy="0 0 0"/>
    <axis xyz="0.0 0.0 1.0"/>
    <parent link="neck_1"/>
    <child link="neck_2"/>
    <limit effort="50000" lower="-0.3490658503988659" upper="0.3490658503988659" velocity="50000"/>
    <dynamics damping="0.06"/>
  </joint>
  <link name="head">
    <inertial>
      <origin xyz="-0.000574157809265 0.113511 -0.0075392" rpy="0 0 0"/>
      <mass value="1.50691"/>
      <inertia ixx="0.00680478" ixy="-1.60267e-05" ixz="7.28166e-05" iyy="0.00634736" iyz="0.000292091" izz="0.00478116"/>
    </inertial>
    <visual>
      <origin xyz="0 -0.067153 -0.0295007" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/head.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="green"/>
    </visual>
    <collision>
      <origin xyz="0 -0.067153 -0.0295007" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/head.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="neck_yaw" type="revolute">
    <origin xyz="0 -0.022344 -0.02080001" rpy="0 0 0"/>
    <axis xyz="0.0 1.0 0.0"/>
    <parent link="neck_2"/>
    <child link="head"/>
    <limit effort="50000" lower="-0.8726646259971648" upper="0.8726646259971648" velocity="50000"/>
    <dynamics damping="0.06"/>
  </joint>
  <link name="imu_frame"/>
  <joint name="imu_frame_fixed_joint" type="fixed">
    <origin xyz="0.00950000019074 0.133444 0.0093" rpy="-1.5707963267948961 1.570796326794896 0"/>
    <parent link="head"/>
    <child link="imu_frame"/>
    <dynamics damping="0.1"/>
  </joint>
  <link name="r_hip_1">
    <inertial>
      <origin xyz="-0.040711 -5.9e-05 -0.0005234" rpy="0 0 0"/>
      <mass value="0.920354"/>
      <inertia ixx="0.000404796" ixy="-1.20388e-06" ixz="1.0611e-06" iyy="0.000575006" iyz="-5.32287e-07" izz="0.000580444"/>
    </inertial>
    <visual>
      <origin xyz="0.0233655 0.151913 0.0428515000001" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/r_hip_1.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="black"/>
    </visual>
    <collision>
      <origin xyz="0.0233655 0.151913 0.0428515000001" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/r_hip_1.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="r_hip_pitch" type="revolute">
    <origin xyz="0.0064515 0.0233655 -0.119913" rpy="1.5707963267948961 0 -1.5707963267948961"/>
    <axis xyz="-1.0 0.0 0.0"/>
    <parent link="root_link"/>
    <child link="r_hip_1"/>
    <limit effort="50000" lower="-0.5235987755982988" upper="1.4835298641951802" velocity="50000"/>
    <dynamics damping="0.223"/>
  </joint>
  <link name="r_hip_2">
    <inertial>
      <origin xyz="3.49999999999e-06 -0.044233 0.0248982" rpy="0 0 0"/>
      <mass value="0.4194"/>
      <inertia ixx="0.00048916" ixy="-2.70159e-07" ixz="1.85233e-07" iyy="0.00028697" iyz="1.28773e-05" izz="0.000315599"/>
    </inertial>
    <visual>
      <origin xyz="0.0701 0.151913 0.0691961998093" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/r_hip_2.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="red"/>
    </visual>
    <collision>
      <origin xyz="0.0701 0.151913 0.0691961998093" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/r_hip_2.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="r_hip_roll" type="revolute">
    <origin xyz="-0.0467345 0 -0.0263447" rpy="0 0 0"/>
    <axis xyz="0.0 0.0 -1.0"/>
    <parent link="r_hip_1"/>
    <child link="r_hip_2"/>
    <limit effort="50000" lower="0.0" upper="1.5707963267948966" velocity="50000"/>
    <dynamics damping="0.223"/>
  </joint>
  <link name="r_hip_3">
    <inertial>
      <origin xyz="-0.000123 -0.0002823 0.008718" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
      <mass value="0.126212"/>
      <inertia ixx="3.27938e-05" ixy="2.67901e-07" ixz="-4.41463e-09" iyy="5.69957e-05" iyz="3.32338e-08" izz="3.26449e-05"/>
    </inertial>
    <visual>
      <origin xyz="0.0437089998093 -0.0700999999999 -0.226213" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/r_hip_3.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="blue"/>
    </visual>
    <collision>
      <origin xyz="0.0437089998093 -0.0700999999999 -0.226213" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/r_hip_3.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="r_leg_ft_sensor" type="fixed">
    <origin xyz="0 -0.0743 0.0254872" rpy="1.5707963267948961 -1.570796326794896 0"/>
    <parent link="r_hip_2"/>
    <child link="r_hip_3"/>
    <dynamics damping="0.1"/>
  </joint>
  <link name="r_upper_leg">
    <inertial>
      <origin xyz="-3.89e-05 -0.078897 0.0033216" rpy="0 0 0"/>
      <mass value="2.20355"/>
      <inertia ixx="0.00791322" ixy="0.000147226" ixz="6.31387e-06" iyy="0.00176632" iyz="-0.000167232" izz="0.00790413"/>
    </inertial>
    <visual>
      <origin xyz="0.0700999999999 0.241013 0.0437089998093" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/r_upper_leg.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="yellow"/>
    </visual>
    <collision>
      <origin xyz="0.0700999999999 0.241013 0.0437089998093" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/r_upper_leg.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="r_hip_yaw" type="revolute">
    <origin xyz="0 0 0.0146" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
    <axis xyz="0.0 -1.0 0.0"/>
    <parent link="r_hip_3"/>
    <child link="r_upper_leg"/>
    <limit effort="50000" lower="-1.2217304763960306" upper="1.2217304763960306" velocity="50000"/>
    <dynamics damping="0.223"/>
  </joint>
  <link name="r_lower_leg">
    <inertial>
      <origin xyz="-0.0274172 -0.088787 0.0010913" rpy="0 0 0"/>
      <mass value="1.87201"/>
      <inertia ixx="0.00540828" ixy="0.000292508" ixz="-7.03247e-05" iyy="0.00192613" iyz="-0.000332689" izz="0.00583736"/>
    </inertial>
    <visual>
      <origin xyz="0.0462961001907 0.386638 0.0437089998095" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/r_lower_leg.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="pink"/>
    </visual>
    <collision>
      <origin xyz="0.0462961001907 0.386638 0.0437089998095" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/r_lower_leg.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="r_knee" type="revolute">
    <origin xyz="0.0238039 -0.145625 0" rpy="0 0 0"/>
    <axis xyz="-1.0 0.0 0.0"/>
    <parent link="r_upper_leg"/>
    <child link="r_lower_leg"/>
    <limit effort="50000" lower="-1.7453292519943295" upper="0.0" velocity="50000"/>
    <dynamics damping="0.223"/>
  </joint>
  <link name="r_ankle_1">
    <inertial>
      <origin xyz="0.0346282 6.3e-05 0.0152507" rpy="0 0 0"/>
      <mass value="0.880328"/>
      <inertia ixx="0.000524342" ixy="9.5836e-07" ixz="-3.92498e-05" iyy="0.000581159" iyz="3.73943e-06" izz="0.000401231"/>
    </inertial>
    <visual>
      <origin xyz="0.105600000191 0.587138 0.0437089998095" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/r_ankle_1.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="cyan"/>
    </visual>
    <collision>
      <origin xyz="0.105600000191 0.587138 0.0437089998095" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/r_ankle_1.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="r_ankle_pitch" type="revolute">
    <origin xyz="-0.0593039 -0.201 0" rpy="0 0 0"/>
    <axis xyz="1.0 0.0 0.0"/>
    <parent link="r_lower_leg"/>
    <child link="r_ankle_1"/>
    <limit effort="50000" lower="-0.3490658503988659" upper="0.5235987755982988" velocity="50000"/>
    <dynamics damping="0.223"/>
  </joint>
  <link name="r_ankle_2">
    <inertial>
      <origin xyz="-8.2e-06 -0.038202 0.0192673" rpy="0 0 0"/>
      <mass value="0.267485"/>
      <inertia ixx="0.000335682" ixy="-7.53335e-08" ixz="2.93681e-08" iyy="0.00025387" iyz="-4.26218e-07" izz="0.000154833"/>
    </inertial>
    <visual>
      <origin xyz="0.0701000001907 0.587138 0.0622089996187" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/r_ankle_2.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="green"/>
    </visual>
    <collision>
      <origin xyz="0.0701000001907 0.587138 0.0622089996187" rpy="0 0 0"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/r_ankle_2.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="r_ankle_roll" type="revolute">
    <origin xyz="0.0355 0 -0.0185" rpy="0 0 0"/>
    <axis xyz="0.0 0.0 -1.0"/>
    <parent link="r_ankle_1"/>
    <child link="r_ankle_2"/>
    <limit effort="50000" lower="-0.3490658503988659" upper="0.3490658503988659" velocity="50000"/>
    <dynamics damping="0.223"/>
  </joint>
  <link name="r_foot">
    <inertial>
      <origin xyz="0.0336237 0.0053646 -0.001138" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
      <mass value="0.38227"/>
      <inertia ixx="0.00120765" ixy="3.70545e-07" ixz="-5.78137e-05" iyy="0.00141242" iyz="-7.82502e-06" izz="0.000273419"/>
    </inertial>
    <visual>
      <origin xyz="0.0472089996187 -0.0701000001907 -0.647438" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/r_foot.dae" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0.0472089996187 -0.0701000001907 -0.647438" rpy="-1.5707963267948961 0 -1.5707963267948961"/>
      <geometry>
        <mesh filename="model://iCubGenova02/meshes/dae/r_foot.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>
  <joint name="r_foot_ft_sensor" type="fixed">
    <origin xyz="0 -0.0603 0.015" rpy="1.5707963267948961 -1.570796326794896 0"/>
    <parent link="r_ankle_2"/>
    <child link="r_foot"/>
    <dynamics damping="0.1"/>
  </joint>
  <link name="r_sole"/>
  <joint name="r_sole_fixed_joint" type="fixed">
    <origin xyz="0.0035 0 0.004" rpy="-3.141592653589793 0 0"/>
    <parent link="r_foot"/>
    <child link="r_sole"/>
    <dynamics damping="0.1"/>
  </joint>
  <gazebo reference="l_leg_ft_sensor">
    <sensor name="l_leg_ft_sensor" type="force_torque">
      <always_on>1</always_on>
      <update_rate>100</update_rate>
      <force_torque>
        <frame>child</frame>
        <measure_direction>child_to_parent</measure_direction>
      </force_torque>
    </sensor>
  </gazebo>
  <gazebo reference="r_leg_ft_sensor">
    <sensor name="r_leg_ft_sensor" type="force_torque">
      <always_on>1</always_on>
      <update_rate>100</update_rate>
      <force_torque>
        <frame>child</frame>
        <measure_direction>child_to_parent</measure_direction>
      </force_torque>
    </sensor>
  </gazebo>
  <gazebo reference="l_foot_ft_sensor">
    <sensor name="l_foot_ft_sensor" type="force_torque">
      <always_on>1</always_on>
      <update_rate>100</update_rate>
      <force_torque>
        <frame>child</frame>
        <measure_direction>child_to_parent</measure_direction>
      </force_torque>
    </sensor>
  </gazebo>
  <gazebo reference="r_foot_ft_sensor">
    <sensor name="r_foot_ft_sensor" type="force_torque">
      <always_on>1</always_on>
      <update_rate>100</update_rate>
      <force_torque>
        <frame>child</frame>
        <measure_direction>child_to_parent</measure_direction>
      </force_torque>
    </sensor>
  </gazebo>
  <gazebo reference="l_arm_ft_sensor">
    <sensor name="l_arm_ft_sensor" type="force_torque">
      <always_on>1</always_on>
      <update_rate>100</update_rate>
      <force_torque>
        <frame>child</frame>
        <measure_direction>child_to_parent</measure_direction>
      </force_torque>
    </sensor>
  </gazebo>
  <gazebo reference="r_arm_ft_sensor">
    <sensor name="r_arm_ft_sensor" type="force_torque">
      <always_on>1</always_on>
      <update_rate>100</update_rate>
      <force_torque>
        <frame>child</frame>
        <measure_direction>child_to_parent</measure_direction>
      </force_torque>
    </sensor>
  </gazebo>
  <gazebo reference="head">
    <sensor name="imu_frame" type="imu">
      <always_on>1</always_on>
      <update_rate>100</update_rate>
      <pose>0.00950000019074 0.133444 0.0093 -1.5707963267948963 1.5707963267948963 0.0</pose>
    </sensor>
  </gazebo>
  <gazebo>
	<plugin name="controlboard_torso" filename="libgazebo_yarp_controlboard.so">
		<yarpConfigurationFile>model://iCubGenova02/conf/gazebo_iCubGenova02_torso.ini</yarpConfigurationFile>
	</plugin>
	<plugin name="controlboard_head" filename="libgazebo_yarp_controlboard.so">
		<yarpConfigurationFile>model://iCubGenova02/conf/gazebo_iCubGenova02_head.ini</yarpConfigurationFile>
	</plugin>
	<plugin name="controlboard_left_arm" filename="libgazebo_yarp_controlboard.so">
		<yarpConfigurationFile>model://iCubGenova02/conf/gazebo_iCubGenova02_left_arm.ini</yarpConfigurationFile>
		<initialConfiguration>-0.52 0.52 0 0.785 0 0 0.698</initialConfiguration>
	</plugin>
	<plugin name="controlboard_right_arm" filename="libgazebo_yarp_controlboard.so">
		<yarpConfigurationFile>model://iCubGenova02/conf/gazebo_iCubGenova02_right_arm.ini</yarpConfigurationFile>
		<initialConfiguration>-0.52 0.52 0 0.785 0 0 0.698</initialConfiguration>
	</plugin>
	<plugin name="controlboard_right_leg" filename="libgazebo_yarp_controlboard.so">
		<yarpConfigurationFile>model://iCubGenova02/conf/gazebo_iCubGenova02_right_leg.ini</yarpConfigurationFile>
	</plugin>
	<plugin name="controlboard_left_leg" filename="libgazebo_yarp_controlboard.so">
		<yarpConfigurationFile>model://iCubGenova02/conf/gazebo_iCubGenova02_left_leg.ini</yarpConfigurationFile>
	</plugin>
	<plugin name="apply_external_wrench" filename="libgazebo_yarp_externalwrench.so">
		<robotNamefromConfigFile>model://iCubGenova02/conf/gazebo_iCubGenova02_robotname.ini</robotNamefromConfigFile>
	</plugin>
  </gazebo>
  <sensor name="root_link_ems_acc_eb5" type="accelerometer">
	<origin xyz="0.0493308 -0.0125771 -0.115691" rpy="3.141592653589793 1.2915440450258349 3.141592653589793"/>
	<parent link="root_link"/>
  </sensor>
  <sensor name="root_link_ems_gyro_eb5" type="gyroscope">
	<origin xyz="0.0493308 -0.0125771 -0.115691" rpy="3.141592653589793 1.2915440450258349 3.141592653589793"/>
	<parent link="root_link"/>
  </sensor>
  <sensor name="l_upper_leg_ems_acc_eb6" type="accelerometer">
	<origin xyz="-0.0404072 -0.083207 -0.0125771" rpy="1.5707963267948961 0 -1.5707963267948961"/>
	<parent link="l_upper_leg"/>
  </sensor>
  <sensor name="l_upper_leg_ems_acc_eb10" type="accelerometer">
	<origin xyz="0.040407 -0.040027 -0.0125751" rpy="1.5707963267948961 0 1.5707963267948961"/>
	<parent link="l_upper_leg"/>
  </sensor>
  <sensor name="l_upper_leg_ems_gyro_eb6" type="gyroscope">
	<origin xyz="-0.0404072 -0.087779 -0.013081" rpy="1.5707963267948961 0 -1.5707963267948961"/>
	<parent link="l_upper_leg"/>
  </sensor>
  <sensor name="l_upper_leg_ems_gyro_eb10" type="gyroscope">
    	<origin xyz="0.040407 -0.035455 -0.013079" rpy="1.5707963267948961 0 1.5707963267948961"/>
	<parent link="l_upper_leg"/>
  </sensor>
  <sensor name="l_upper_leg_mtb_acc_10b1" type="accelerometer">
	<origin xyz="0.031565 0.049332 0.04200163" rpy="0.8377580369611062 -8.629128296517655e-05 1.5708740239417236"/>
	<parent link="l_upper_leg"/>
  </sensor>
  <sensor name="l_upper_leg_mtb_acc_10b2" type="accelerometer">
	<origin xyz="0.0083839 0.039452 0.05335544" rpy="0.8377580369650546 -8.629128691368476e-05 1.5708740239399657"/>
	<parent link="l_upper_leg"/>
  </sensor>
  <sensor name="l_upper_leg_mtb_acc_10b3" type="accelerometer">
	<origin xyz="0.0150706 0.004382 0.053785" rpy="0 0 1.5707963267948966"/>
	<parent link="l_upper_leg"/>
  </sensor>
  <sensor name="l_upper_leg_mtb_acc_10b4" type="accelerometer">
    <origin xyz="-0.0144294 0.004382 0.053785" rpy="0 0 1.5707963267948966"/>
    <parent link="l_upper_leg"/>
  </sensor>
  <sensor name="l_upper_leg_mtb_acc_10b5" type="accelerometer">
    <origin xyz="-0.0414999 -0.004658 0.03412949" rpy="-1.186823762544959 0 1.5707963267948963"/>
    <parent link="l_upper_leg"/>
  </sensor>
  <sensor name="l_upper_leg_mtb_acc_10b6" type="accelerometer">
	<origin xyz="0.0178086 -0.026212 -0.043785" rpy="3.141592653589793 0 -1.5709124300543913"/>
	<parent link="l_upper_leg"/>
  </sensor>
  <sensor name="l_upper_leg_mtb_acc_10b7" type="accelerometer">
	<origin xyz="-0.0141918 -0.026211 -0.043785" rpy="3.141592653589793 0 -1.5709453146950971"/>
	<parent link="l_upper_leg"/>
  </sensor>
  <sensor name="l_lower_leg_ems_acc_eb7" type="accelerometer">
    <origin xyz="0.0350978 -0.092765 0.04351373" rpy="0 -0.21816654334094113 1.5707963267948974"/>
    <parent link="l_lower_leg"/>
  </sensor>
  <sensor name="l_lower_leg_ems_gyro_eb7" type="gyroscope">
    <origin xyz="0.0356017 -0.088301 0.044503292" rpy="0 -0.21816654334094113 1.5707963267948974"/>
    <parent link="l_lower_leg"/>
  </sensor>
  <sensor name="l_lower_leg_mtb_acc_10b8" type="accelerometer">
    <origin xyz="0.0099841 -0.004318 0.0544518" rpy="-0.34906563804548707 3.9714103784092166e-05 1.570905440483374"/>
    <parent link="l_lower_leg"/>
  </sensor>
  <sensor name="l_lower_leg_mtb_acc_10b9" type="accelerometer">
    <origin xyz="0.0665272 -0.043468 0.0259151" rpy="1.30899714593518 -0.00011215980287766697 1.5708263802148703"/>
    <parent link="l_lower_leg"/>
  </sensor>
  <sensor name="l_lower_leg_mtb_acc_10b10" type="accelerometer">
    <origin xyz="0.030989 -0.011037 0.05238133" rpy="-1.326450423673184 0 1.5707963267948961"/>
    <parent link="l_lower_leg"/>
  </sensor>
  <sensor name="l_lower_leg_mtb_acc_10b11" type="accelerometer">
    <origin xyz="-0.0179355 -0.092958 0.0184593" rpy="-1.3089971472018722 -2.543178857158298e-05 1.5707895123459004"/>
    <parent link="l_lower_leg"/>
  </sensor>
  <sensor name="l_foot_mtb_acc_10b12" type = "accelerometer">
    <origin xyz="0.1055224 0.0008072 -0.001215" rpy="0 0 1.57079632679"/>
    <parent link="l_foot"/>
  </sensor>
  <sensor name="l_foot_mtb_acc_10b13" type = "accelerometer">
    <origin xyz="0.0805224 0.0118072 -0.001215" rpy="0 0 1.57079632679"/>
    <parent link="l_foot"/>
  </sensor>
  <sensor name="chest_ems_acc_eb1" type="accelerometer">
	<origin xyz="0.0647855001907 0.064043 -0.064709" rpy="3.1415169122972952 -0.2617993091222473 0.0002926418802545251"/>
	<parent link="chest"/>
  </sensor>
  <sensor name="chest_ems_gyro_eb1" type="gyroscope">
	<origin xyz="0.0692016001907 0.064548 -0.0635257" rpy="3.1415169122972952 -0.2617993091222473 0.0002926418802545251"/>
    	<parent link="chest"/>
  </sensor>
  <sensor name="chest_ems_acc_eb2" type="accelerometer">
	<origin xyz="0.0204533001907 0.088063 -0.0641645" rpy="3.141590501042822 0.2617993198185103 3.1415843367812997"/> <parent link="chest"/>
	<child link="chest_ems_acc_eb2"/>
  </sensor>
  <sensor name="chest_ems_gyro_eb2" type="gyroscope">
	<origin xyz="0.0160371001907 0.087559 -0.0653478" rpy="3.141590501042822 0.2617993198185103 3.1415843367812997"/>
	<parent link="chest"/>
  </sensor>
  <sensor name="chest_ems_acc_eb3" type="accelerometer">
	<origin xyz="-0.0230587998093 0.064019 -0.0758985" rpy="3.141109162025268 0.26110051647116095 -0.00030907146255546275"/>
	<parent link="chest"/>
  </sensor>
  <sensor name="chest_ems_gyro_eb3" type="gyroscope">
	<origin xyz="-0.0186416998093 0.064521 -0.0770785" rpy="3.141109162025268 0.26110051647116095 -0.00030907146255546275"/>
	<parent link="chest"/>
  </sensor>
  <sensor name="chest_ems_acc_eb4" type="accelerometer">
    <origin xyz="-0.0621620998093 0.088063 -0.0529886" rpy="-3.1415905047625503 -0.26179931981852844 3.141584339870295"/>
    <parent link="chest"/>
  </sensor>
  <sensor name="chest_ems_gyro_eb4" type="gyroscope">
	<origin xyz="-0.0665783998093 0.087559 -0.0518053" rpy="-3.1415905047625503 -0.26179931981852844 3.141584339870295"/>
	<parent link="chest"/>
  </sensor>
  <sensor name="chest_mtb_acc_0b7" type="accelerometer">
	<origin xyz="-0.0607539998093 -0.0017836 0.0442342" rpy="0.8751677714915176 0.11636373439232421 -1.485973502467595"/>
	<parent link="chest"/>
  </sensor>
  <sensor name="chest_mtb_acc_0b8" type="accelerometer">
	<origin xyz="-0.0457046998093 -0.001846 0.0567427" rpy="0.5009600846888737 0.10731368898312443 -1.4778325291042227"/>
	<parent link="chest"/>
  </sensor>
  <sensor name="chest_mtb_acc_0b9" type="accelerometer">
	<origin xyz="0.0439036001907 -0.0017841 0.0577221" rpy="-0.5009599289728344 0.10731540424780756 -1.663760174692588"/>
	<parent link="chest"/>
  </sensor>
  <sensor name="chest_mtb_acc_0b10" type="accelerometer">
	<origin xyz="0.0594286001907 -0.0018543 0.0457982" rpy="-0.8751681534754425 0.11636202547856277 -1.6556180750511098"/>
	<parent link="chest"/>
  </sensor>
  <sensor name="l_upper_arm_mtb_acc_1b10" type="accelerometer">
	<origin xyz="-0.0430553865107 -0.0286807251034 0.0295279" rpy="-1.570796460941082 -1.5705035075902931 2.6825718625443584"/>
	<parent link="l_upper_arm"/>
  </sensor>
  <sensor name="l_upper_arm_mtb_acc_1b11" type="accelerometer">
	<origin xyz="-0.0412179516256 -0.0295882096119 0.0760604" rpy="-1.570796460941082 -1.5705035075902931 2.6825718625443584"/>
	<parent link="l_upper_arm"/>
  </sensor>
  <sensor name="l_upper_arm_mtb_acc_1b12" type="accelerometer">
	<origin xyz="-0.0412194005144 -0.0295878213836 0.0049921" rpy="-1.5707956830363743 -1.570783812267946 2.682571084586891"/>
	<parent link="l_upper_arm"/>
  </sensor>
  <sensor name="l_upper_arm_mtb_acc_1b13" type="accelerometer">
	<origin xyz="0.0105927619825 -0.0283506291435 -0.0086914" rpy="-1.5707963266769467 0.00021452011106801812 -2.4780369107521003"/>
	<parent link="l_upper_arm"/>
  </sensor>
  <sensor name="l_forearm_mtb_acc_1b7" type="accelerometer">
	<origin xyz="0.114369310123 0.0285176341319 -0.016529983678" rpy="0.2617994794375143 0.0003704457632296642 -1.570334469770784"/>
    	<parent link="l_elbow_1"/>
  </sensor>
  <sensor name="l_forearm_mtb_acc_1b8" type="accelerometer">
	<origin xyz="0.088920682324 0.0159719572896 0.0352687234784" rpy="-0.6611434090480407 0.14782568634942247 -3.0495222301717226"/>
    	<parent link="l_elbow_1"/>
  </sensor>
  <sensor name="l_forearm_mtb_acc_1b9" type="accelerometer">
	<origin xyz="0.0595956869538 0.0596564434985 -0.0205993985252" rpy="1.7668208948349358 0.2611001649697097 3.0875256913488913"/>
	<parent link="l_elbow_1"/>
  </sensor>
  <sensor name="r_upper_arm_mtb_acc_2b10" type="accelerometer">
	<origin xyz="-0.0412194005144 0.0295878213836 0.0295279" rpy="1.5707969705667262 1.5707838122679458 -2.682571084564997"/>
	<parent link="r_upper_arm"/>
  </sensor>
  <sensor name="r_upper_arm_mtb_acc_2b11" type="accelerometer">
	<origin xyz="-0.0430565765807 0.0286793709492 0.0760604" rpy="-1.5707961926490903 -1.5705035075902927 0.4590207910451026"/>
	<parent link="r_upper_arm"/>
  </sensor>
  <sensor name="r_upper_arm_mtb_acc_2b12" type="accelerometer">
	<origin xyz="-0.0430553865107 0.0286807251034 0.0049922" rpy="-1.111775401564534 -1.5707963267948961 0"/>
	<parent link="r_upper_arm"/>
  </sensor>
  <sensor name="r_upper_arm_mtb_acc_2b13" type="accelerometer">
	<origin xyz="0.010593416831 0.0283497693333 -0.010738" rpy="1.5707963269128467 -0.00021452011106801806 2.4780369107521003"/>
    	<parent link="r_upper_arm"/>
  </sensor>
 <sensor name="r_forearm_mtb_acc_2b7" type="accelerometer">
	<origin xyz="-0.11436927713 0.0279967173307 -0.0165298605327" rpy="0.2617994793812094 0.00037025313277796464 1.5712581194176245"/>
	<parent link="r_elbow_1"/>
  </sensor>
  <sensor name="r_forearm_mtb_acc_2b8" type="accelerometer">
	<origin xyz="-0.0891879779927 0.00375149154266 0.0348004899258" rpy="0.6611590015134917 0.14774579417419006 -0.0919672808581299"/>
    	<parent link="r_elbow_1"/>
  </sensor>
  <sensor name="r_forearm_mtb_acc_2b9" type="accelerometer">
	<origin xyz="-0.0591344983628 0.0504236337681 -0.0187310112996" rpy="-1.7668262973366173 0.26119181238162653 0.05404748777229077"/>
	<parent link="r_elbow_1"/>
  </sensor>
  <sensor name="r_upper_leg_ems_acc_eb8" type="accelerometer">
 	<parent link="r_upper_leg"/>
        <origin xyz="0.0404072 -0.040027 -0.0125751" rpy="1.5707963267948961 0 -1.5707963267948961"/>
  </sensor>
  <sensor name="r_upper_leg_ems_acc_eb11" type="accelerometer">
	<origin xyz="-0.040407 -0.083207 -0.0125771" rpy="1.5707963267948961 0 -1.5707963267948961"/>
	<parent link="r_upper_leg"/>
  </sensor>
  <sensor name="r_upper_leg_ems_gyro_eb8" type="gyroscope">
	<origin xyz="0.0404072 -0.035455 -0.013079" rpy="1.5707963267948961 0 -1.5707963267948961"/>
    	<parent link="r_upper_leg"/>
  </sensor>
  <sensor name="r_upper_leg_ems_gyro_eb11" type="gyroscope">
	<origin xyz="-0.040407 -0.087779 -0.013081" rpy="1.57079632679 0 -1.57079632679"/>
	<parent link="r_upper_leg"/>
  </sensor>
  <sensor name="r_upper_leg_mtb_acc_11b1" type="accelerometer">
  	<origin xyz="-0.030195 0.04914 0.043588612" rpy="-0.8377580538579834 -0.0003427907493437384 1.570874016517316"/>
    	<parent link="r_upper_leg"/>
  </sensor>
  <sensor name="r_upper_leg_mtb_acc_11b2" type="accelerometer">
	<origin xyz="-0.0064065 0.039256 0.0539467" rpy="-0.8377580538579834 -0.0003427907493437384 1.570874016517316"/>
    	<parent link="r_upper_leg"/>
  </sensor>
  <sensor name="r_upper_leg_mtb_acc_11b3" type="accelerometer">
	<origin xyz="-0.0130214 0.004186 0.0538313" rpy="0 -0.00042908999059361946 1.570912443546613"/>
    	<parent link="r_upper_leg"/>
  </sensor>
  <sensor name="r_upper_leg_mtb_acc_11b4" type="accelerometer">
  	<origin xyz="0.0422675 -0.004845 0.0322719" rpy="1.1868237508623853 -0.0005368537261927895 1.5708398310564209"/>
    	<parent link="r_upper_leg"/>
  </sensor>
  <sensor name="r_upper_leg_mtb_acc_11b5" type="accelerometer">
  	<origin xyz="0.0164786 0.004186 0.0538313" rpy="0 -0.00042908999059361946 1.570912443546613"/>
    	<parent link="r_upper_leg"/>
  </sensor>
  <sensor name="r_upper_leg_mtb_acc_11b6" type="accelerometer">
  	<origin xyz="-0.0157594 -0.026365 -0.0437518" rpy="3.141592653589793 0.0004290899934163925 -1.5707963267948968"/>
    	<parent link="r_upper_leg"/>
  </sensor>
  <sensor name="r_upper_leg_mtb_acc_11b7" type="accelerometer">
	<origin xyz="0.0162386 -0.026366 -0.0437518" rpy="3.1415926037711634 0.000429089990404397 -1.5709124300650796"/>
    	<parent link="r_upper_leg"/>
  </sensor>
  <sensor name="r_lower_leg_ems_acc_eb9" type="accelerometer">
	<origin xyz="-0.0099643 -0.092765 0.04351373" rpy="0 -0.21816654334094113 1.5707963267948974"/>
    	<parent link="r_lower_leg"/>
  </sensor>
  <sensor name="r_lower_leg_ems_gyro_eb9" type="gyroscope">
 	<origin xyz="-0.0094604 -0.088301 0.044503292" rpy="0 -0.21816654334094113 1.5707963267948974"/>
    	<parent link="r_lower_leg"/>
  </sensor>
  <sensor name="r_lower_leg_mtb_acc_11b8" type="accelerometer">
	<origin xyz="-0.008081 -0.004318 0.0537517" rpy="0.34906563804609786 -3.971410439477034e-05 1.5709054404832665"/>
	<parent link="r_lower_leg"/>
  </sensor>
  <sensor name="r_lower_leg_mtb_acc_11b9" type="accelerometer">
	<origin xyz="-0.0660179 -0.043468 0.0278945" rpy="-1.3089971473150512 0 1.5707963267948968"/>
	<parent link="r_lower_leg"/>
  </sensor>
  <sensor name="r_lower_leg_mtb_acc_11b10" type="accelerometer">
	<origin xyz="-0.0389997 -0.004318 0.05362922" rpy="-0.2443461645654741 2.8091104096089837e-05 1.5709089943462522"/>
	<parent link="r_lower_leg"/>
  </sensor>
  <sensor name="r_lower_leg_mtb_acc_11b11" type="accelerometer">
	<origin xyz="0.0184451 -0.092958 0.0164801" rpy="1.30899714593518 -0.00011215980287766697 1.5708263802148703"/>
	<parent link="r_lower_leg"/>
  </sensor>
  <sensor name="r_foot_mtb_acc_11b12" type="accelerometer">
	<origin xyz="0.1034744 -0.000807 -0.001215" rpy="0 0 -1.5707963267948966"/>
	<parent link="r_foot"/>
  </sensor>
  <sensor name="r_foot_mtb_acc_11b13" type="accelerometer">
	<origin xyz="0.0784744 -0.0118072 -0.001215" rpy="0 0 -1.5707963267948966"/>
	<parent link="r_foot"/>
  </sensor>
  <gazebo reference="l_arm_ft_sensor">
        <sensor name="left_arm_ft" type="force_torque">
            <force_torque>
                <frame>child</frame>
                <measure_direction>child_to_parent</measure_direction>
            </force_torque>
            <update_rate>100</update_rate>
            <always_on>1</always_on>
        </sensor>
    </gazebo>
    <sensor name="l_arm_ft_sensor" type="force_torque">
        <parent joint="l_arm_ft_sensor"/>
        <force_torque>
                <frame>child</frame>
                <measure_direction>child_to_parent</measure_direction>
        </force_torque>
    </sensor>
    <gazebo reference="r_arm_ft_sensor">
        <sensor name="right_arm_ft" type="force_torque">
            <force_torque>
                <frame>child</frame>
                <measure_direction>child_to_parent</measure_direction>
            </force_torque>
            <update_rate>100</update_rate>
            <always_on>1</always_on>
        </sensor>
    </gazebo>
    <sensor name="r_arm_ft_sensor" type="force_torque">
        <parent joint="r_arm_ft_sensor"/>
        <force_torque>
                <frame>child</frame>
                <measure_direction>child_to_parent</measure_direction>
        </force_torque>
    </sensor>
    <gazebo reference="l_leg_ft_sensor">
        <sensor name="left_leg_ft" type="force_torque">
            <force_torque>
                <frame>child</frame>
                <measure_direction>child_to_parent</measure_direction>
            </force_torque>
            <update_rate>100</update_rate>
            <always_on>1</always_on>
        </sensor>
    </gazebo>
    <sensor name="l_leg_ft_sensor" type="force_torque">
        <parent joint="l_leg_ft_sensor"/>
        <force_torque>
                <frame>child</frame>
                <measure_direction>child_to_parent</measure_direction>
        </force_torque>
    </sensor>
    <gazebo reference="r_leg_ft_sensor">
        <sensor name="right_leg_ft" type="force_torque">
            <force_torque>
                <frame>child</frame>
                <measure_direction>child_to_parent</measure_direction>
            </force_torque>
            <update_rate>100</update_rate>
            <always_on>1</always_on>
        </sensor>
    </gazebo>
    <sensor name="r_leg_ft_sensor" type="force_torque">
        <parent joint="r_leg_ft_sensor"/>
        <force_torque>
                <frame>child</frame>
                <measure_direction>child_to_parent</measure_direction>
        </force_torque>
    </sensor>
    <gazebo reference="l_foot_ft_sensor">
        <sensor name="left_foot_ft" type="force_torque">
            <force_torque>
                <frame>child</frame>
                <measure_direction>child_to_parent</measure_direction>
            </force_torque>
            <update_rate>100</update_rate>
            <always_on>1</always_on>
        </sensor>
    </gazebo>
    <sensor name="l_foot_ft_sensor" type="force_torque">
        <parent joint="l_foot_ft_sensor"/>
        <force_torque>
                <frame>child</frame>
                <measure_direction>child_to_parent</measure_direction>
        </force_torque>
    </sensor>
    <gazebo reference="r_foot_ft_sensor">
        <sensor name="right_foot_ft" type="force_torque">
            <force_torque>
                <frame>child</frame>
                <measure_direction>child_to_parent</measure_direction>
            </force_torque>
            <update_rate>100</update_rate>
            <always_on>1</always_on>
        </sensor>
    </gazebo>
    <sensor name="r_foot_ft_sensor" type="force_torque">
        <parent joint="r_foot_ft_sensor"/>
        <force_torque>
                <frame>child</frame>
                <measure_direction>child_to_parent</measure_direction>
        </force_torque>
    </sensor>
</robot>
