<?xml version="1.0" ?>
<robot name="centauro">
  <!-- none, ball, dagana_fixed, dagana -->
  <!-- scaling facto for available peack torque link side, from 0 to 1 -->
  <material name="black">
    <color rgba="0.10 0.10 0.11 1"/>
  </material>
  <material name="dark">
    <color rgba="0.23 0.23 0.25 1"/>
  </material>
  <material name="dark_grey">
    <color rgba="0.18 0.19 0.21 1"/>
  </material>
  <material name="light_grey">
    <color rgba="0.3 0.3 0.3 1"/>
  </material>
  <material name="orange">
    <color rgba="0.63 0.50 0.34 1"/>
  </material>
  <material name="D435_head_camera_aluminum">
    <color rgba="0.72 0.72 0.70 1"/>
  </material>
  <material name="D435i_camera_aluminum">
    <color rgba="0.72 0.72 0.70 1"/>
  </material>

  <link name="pelvis">
    <inertial>
      <origin xyz="-0.0016320207 0.0026534004000000003 0.079167428"/>
      <mass value="26.584529"/>
      <inertia ixx="0.39195477" ixy="0.024781258999999996" ixz="0.017108606999999998" iyy="1.0000708999999999" iyz="0.0052627106000000005" izz="1.0679988999999999"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/simple/pelvis.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/simple/pelvis_reduced.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </collision>
  </link>
  <link name="imu_link"/>
  <joint name="imu_joint" type="fixed">
    <parent link="pelvis"/>
    <origin rpy="0.0 0.0 0.0" xyz="0.248316 0.0 0.015"/>
    <child link="imu_link"/>
  </joint>
  <!-- torso links -->
  <link name="torso_2">
    <inertial>
      <origin xyz="-0.019497794 0.0045317835 0.13768283"/>
      <mass value="12.553731"/>
      <inertia ixx="0.063643392" ixy="8.939e-05" ixz="-0.00086873" iyy="0.02680235" iyz="-4.657e-05" izz="0.04743015"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/link1_no_head.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/simple/link1_no_head.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </collision>
  </link>
  <!-- /torso links -->
  <!-- torso joints -->
  <joint name="torso_yaw" type="revolute">
    <parent link="pelvis"/>
    <origin xyz="0.2 0.0 0.256"/>
    <child link="torso_2"/>
    <axis xyz="0 0 1"/>
    <limit effort="147" lower="-2.618" upper="2.618" velocity="5.7"/>
    <!-- TODO -->
  </joint>
  <!-- /torso joints -->
  <!-- /macro arms -->
  <!-- LINKS -->
  <!-- shoulder yaw-roll-->
  <link name="arm1_1">
    <inertial>
      <origin xyz="-0.0074457212 0.03410796 0.00010978102"/>
      <mass value="1.9628675"/>
      <inertia ixx="0.0053547717" ixy="0.00036428926" ixz="1.5089568e-05" iyy="0.0033923328" iyz="5.5692312e-05" izz="0.0068921413"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/simple/ShoulderPitch.STL" scale="0.001 -0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/simple/ShoulderPitch.STL" scale="0.001 -0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </collision>
  </link>
  <!-- shoulder yaw-roll-->
  <!-- shoulder roll-pitch-->
  <link name="arm1_2">
    <inertial>
      <origin xyz="0.058142302 5.7450803e-05 -0.077477683"/>
      <mass value="1.8595811"/>
      <inertia ixx="0.013776643" ixy="3.7788675e-05" ixz="0.0037690171" iyy="0.015677464" iyz="-9.4893549e-06" izz="0.0046317657"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/simple/ShoulderRoll.STL" scale="0.001 -0.001 0.001"/>
      </geometry>
      <material name="orange"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/simple/ShoulderRoll.STL" scale="0.001 -0.001 0.001"/>
      </geometry>
      <material name="orange"/>
    </collision>
  </link>
  <!-- shoulder roll-pitch-->
  <!-- shoulder-elbow -->
  <link name="arm1_3">
    <inertial>
      <origin xyz="0.014625194 0.0008172672 -0.028333545"/>
      <mass value="1.6678109"/>
      <inertia ixx="0.0064480435" ixy="-0.00015639093" ixz="0.0012205359" iyy="0.0073372077" iyz="8.9941532e-05" izz="0.0036738448"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/simple/ShoulderYaw.STL" scale="0.001 -0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/simple/ShoulderYaw.STL" scale="0.001 -0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </collision>
  </link>
  <!-- /shoulder-elbow -->
  <!-- elbow yaw-pitch -->
  <link name="arm1_4">
    <inertial>
      <origin xyz="-0.0076833067 -0.040302205 -0.043492779"/>
      <mass value="1.3157289"/>
      <inertia ixx="0.004330394" ixy="-0.00011737391" ixz="-0.00041923199" iyy="0.0038539919" iyz="-0.00079573038" izz="0.0017594689"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/Elbow.STL" scale="0.001 -0.001 0.001"/>
      </geometry>
      <material name="orange"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/simple/Elbow.STL" scale="0.001 -0.001 0.001"/>
      </geometry>
      <material name="orange"/>
    </collision>
  </link>
  <!-- /elbow yaw-pitch -->
  <!-- elbow-wrist -->
  <link name="arm1_5">
    <inertial>
      <origin xyz="-0.00011079615 0.011590836 -0.07816026"/>
      <mass value="1.4908547"/>
      <inertia ixx="0.0085692128" ixy="1.7856252e-05" ixz="1.9379365e-05" iyy="0.0077454159" iyz="-0.00032860094" izz="0.0027441921"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/Forearm.STL" scale="0.001 -0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/simple/Forearm.STL" scale="0.001 -0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </collision>
  </link>
  <!-- /elbow-wrist -->
  <!-- wrist yaw-pitch -->
  <link name="arm1_6">
    <inertial>
      <origin xyz="-4.6502396e-06 -0.038014094 -0.069926878"/>
      <mass value="1.1263612"/>
      <inertia ixx="0.0051871784" ixy="2.724437e-05" ixz="2.2833496e-06" iyy="0.0048037789" iyz="-0.00072165653" izz="0.0012771388"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/simple/wrist_pitch_right.stl" scale="0.001 -0.001 0.001"/>
      </geometry>
      <material name="orange"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/simple/wrist_pitch_right.stl" scale="0.001 -0.001 0.001"/>
      </geometry>
      <material name="orange"/>
    </collision>
  </link>
  <joint name="j_arm1_1" type="revolute">
    <parent link="torso_2"/>
    <child link="arm1_1"/>
    <origin rpy="0.174533 0.0 -0.5235988" xyz="0.0457475 0.169137 0.116626"/>
    <axis xyz="0 1 0"/>
    <limit effort="147.0" lower="-3.312" upper="1.615" velocity="3.86"/>
  </joint>
  <!-- /shoulder yaw -->
  <!-- shoulder roll -->
  <joint name="j_arm1_2" type="revolute">
    <parent link="arm1_1"/>
    <child link="arm1_2"/>
    <origin rpy="-0.1745327 0.0 0.0" xyz="-0.09015 0.062 0.0"/>
    <axis xyz="1 0 0"/>
    <limit effort="147.0" lower="0.02" upper="3.431" velocity="3.86"/>
  </joint>
  <!-- /shoulder roll -->
  <!-- shoulder pitch-->
  <joint name="j_arm1_3" type="revolute">
    <parent link="arm1_2"/>
    <child link="arm1_3"/>
    <origin xyz="0.09015 0.0 -0.21815"/>
    <axis xyz="0 0 1"/>
    <limit effort="147.0" lower="-2.552" upper="2.566" velocity="6.06"/>
  </joint>
  <!-- /shoulder pitch -->
  <!-- elbow yaw -->
  <joint name="j_arm1_4" type="revolute">
    <parent link="arm1_3"/>
    <child link="arm1_4"/>
    <origin xyz="0.045 0.05515 -0.074"/>
    <axis xyz="0 1 0"/>
    <limit effort="147.0" lower="-2.465" upper="0.28" velocity="6.06"/>
  </joint>
  <!-- /elbow yaw -->
  <!-- elbow pitch-->
  <joint name="j_arm1_5" type="revolute">
    <parent link="arm1_4"/>
    <child link="arm1_5"/>
    <origin xyz="-0.015 -0.05515 -0.095"/>
    <axis xyz="0 0 1"/>
    <limit effort="55.0" lower="-2.569" upper="2.562" velocity="11.72"/>
  </joint>
  <!-- /elbow pitch-->
  <!-- wrist yaw-->
  <joint name="j_arm1_6" type="revolute">
    <parent link="arm1_5"/>
    <child link="arm1_6"/>
    <origin xyz="0.0 0.049 -0.156"/>
    <axis xyz="0 1 0"/>
    <limit effort="55.0" lower="-1.529" upper="1.509" velocity="11.72"/>
  </joint>
  <link name="arm2_1">
    <inertial>
      <origin xyz="-0.0074457212 -0.03410796 0.00010978102"/>
      <mass value="1.9628675"/>
      <inertia ixx="0.0053547717" ixy="-0.00036428926" ixz="1.5089568e-05" iyy="0.0033923328" iyz="-5.5692312e-05" izz="0.0068921413"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/simple/ShoulderPitch.STL" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/simple/ShoulderPitch.STL" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </collision>
  </link>
  <!-- shoulder yaw-roll-->
  <!-- shoulder roll-pitch-->
  <link name="arm2_2">
    <inertial>
      <origin xyz="0.058142302 -5.7450803e-05 -0.077477683"/>
      <mass value="1.8595811"/>
      <inertia ixx="0.013776643" ixy="-3.7788675e-05" ixz="0.0037690171" iyy="0.015677464" iyz="9.4893549e-06" izz="0.0046317657"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/simple/ShoulderRoll.STL" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="orange"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/simple/ShoulderRoll.STL" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="orange"/>
    </collision>
  </link>
  <!-- shoulder roll-pitch-->
  <!-- shoulder-elbow -->
  <link name="arm2_3">
    <inertial>
      <origin xyz="0.014625194 -0.0008172672 -0.028333545"/>
      <mass value="1.6678109"/>
      <inertia ixx="0.0064480435" ixy="0.00015639093" ixz="0.0012205359" iyy="0.0073372077" iyz="-8.9941532e-05" izz="0.0036738448"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/ShoulderYaw.STL" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/simple/ShoulderYaw.STL" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </collision>
  </link>
  <!-- /shoulder-elbow -->
  <!-- elbow yaw-pitch -->
  <link name="arm2_4">
    <inertial>
      <origin xyz="-0.0076833067 0.040302205 -0.043492779"/>
      <mass value="1.3157289"/>
      <inertia ixx="0.004330394" ixy="0.00011737391" ixz="-0.00041923199" iyy="0.0038539919" iyz="0.00079573038" izz="0.0017594689"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/Elbow.STL" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="orange"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/simple/Elbow.STL" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="orange"/>
    </collision>
  </link>
  <!-- /elbow yaw-pitch -->
  <!-- elbow-wrist -->
  <link name="arm2_5">
    <inertial>
      <origin xyz="-0.00011079615 -0.011590836 -0.07816026"/>
      <mass value="1.4908547"/>
      <inertia ixx="0.0085692128" ixy="-1.7856252e-05" ixz="1.9379365e-05" iyy="0.0077454159" iyz="0.00032860094" izz="0.0027441921"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/Forearm.STL" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/simple/Forearm.STL" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </collision>
  </link>
  <!-- /elbow-wrist -->
  <!-- wrist yaw-pitch -->
  <link name="arm2_6">
    <inertial>
      <origin xyz="-4.6502396e-06 0.038014094 -0.069926878"/>
      <mass value="1.1263612"/>
      <inertia ixx="0.0051871784" ixy="-2.724437e-05" ixz="2.2833496e-06" iyy="0.0048037789" iyz="0.00072165653" izz="0.0012771388"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/simple/wrist_pitch_right.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="orange"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/simple/wrist_pitch_right.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="orange"/>
    </collision>
  </link>
  <joint name="j_arm2_1" type="revolute">
    <parent link="torso_2"/>
    <child link="arm2_1"/>
    <origin rpy="-0.174533 0.0 0.5235988" xyz="0.0457475 -0.169137 0.116626"/>
    <axis xyz="0 1 0"/>
    <limit effort="147.0" lower="-3.3458" upper="1.6012" velocity="3.86"/>
  </joint>
  <!-- /shoulder yaw -->
  <!-- shoulder roll -->
  <joint name="j_arm2_2" type="revolute">
    <parent link="arm2_1"/>
    <child link="arm2_2"/>
    <origin rpy="0.1745327 0.0 0.0" xyz="-0.09015 -0.062 0.0"/>
    <axis xyz="1 0 0"/>
    <limit effort="147.0" lower="-3.4258" upper="-0.0138" velocity="3.86"/>
  </joint>
  <!-- /shoulder roll -->
  <!-- shoulder pitch-->
  <joint name="j_arm2_3" type="revolute">
    <parent link="arm2_2"/>
    <child link="arm2_3"/>
    <origin xyz="0.09015 0.0 -0.21815"/>
    <axis xyz="0 0 1"/>
    <limit effort="147.0" lower="-2.5614" upper="2.5606" velocity="6.06"/>
  </joint>
  <!-- /shoulder pitch -->
  <!-- elbow yaw -->
  <joint name="j_arm2_4" type="revolute">
    <parent link="arm2_3"/>
    <child link="arm2_4"/>
    <origin xyz="0.045 -0.05515 -0.074"/>
    <axis xyz="0 1 0"/>
    <limit effort="147.0" lower="-2.4794" upper="0.2886" velocity="6.06"/>
  </joint>
  <!-- /elbow yaw -->
  <!-- elbow pitch-->
  <joint name="j_arm2_5" type="revolute">
    <parent link="arm2_4"/>
    <child link="arm2_5"/>
    <origin xyz="-0.015 0.05515 -0.095"/>
    <axis xyz="0 0 1"/>
    <limit effort="55.0" lower="-2.5394" upper="2.5546" velocity="11.72"/>
  </joint>
  <!-- /elbow pitch-->
  <!-- wrist yaw-->
  <joint name="j_arm2_6" type="revolute">
    <parent link="arm2_5"/>
    <child link="arm2_6"/>
    <origin xyz="0.0 -0.049 -0.156"/>
    <axis xyz="0 1 0"/>
    <limit effort="55.0" lower="-1.5154" upper="1.5156" velocity="11.72"/>
  </joint>
  <link name="ball1">
    <inertial>
      <origin rpy="0 0 0" xyz="-1.0072498e-05 3.7590658e-05 0.019772332"/>
      <mass value="1.27"/>
      <inertia ixx="0.0023061092" ixy="2.9181758e-07" ixz="-2.1246683999999997e-07" iyy="0.0023053155000000002" iyz="7.9290978e-07" izz="0.0030675615999999997"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry name="ball1_visual">
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/BallHand.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry name="ball1_collision">
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/simple/BallHand.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </collision>
  </link>
  <link name="ball1_tip"/>
  <joint name="j_arm1_8" type="fixed">
    <parent link="arm1_6"/>
    <child link="ball1"/>
    <origin rpy="0. 0. 0." xyz="0. -0.05 -0.105"/>
  </joint>
  <joint name="j_ball1_tip" type="fixed">
    <parent link="ball1"/>
    <child link="ball1_tip"/>
    <origin rpy="0 0 0" xyz="0 0 -0.07"/>
  </joint>
  <!-- add armX_8 link for legacy reasons (same as ballX) -->
  <joint name="j_ball1_fixed" type="fixed">
    <parent link="ball1"/>
    <child link="arm1_8"/>
  </joint>
  <link name="arm1_8">
    <inertial>
      <mass value="0"/>
      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
    </inertial>
  </link>
  <link name="ball2">
    <inertial>
      <origin rpy="0 0 0" xyz="-1.0072498e-05 3.7590658e-05 0.019772332"/>
      <mass value="1.27"/>
      <inertia ixx="0.0023061092" ixy="2.9181758e-07" ixz="-2.1246683999999997e-07" iyy="0.0023053155000000002" iyz="7.9290978e-07" izz="0.0030675615999999997"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry name="ball2_visual">
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/BallHand.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry name="ball2_collision">
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/simple/BallHand.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </collision>
  </link>
  <link name="ball2_tip"/>
  <joint name="j_arm2_8" type="fixed">
    <parent link="arm2_6"/>
    <child link="ball2"/>
    <origin rpy="0. 0. 0." xyz="0. 0.05 -0.105"/>
  </joint>
  <joint name="j_ball2_tip" type="fixed">
    <parent link="ball2"/>
    <child link="ball2_tip"/>
    <origin rpy="0 0 0" xyz="0 0 -0.07"/>
  </joint>
  <!-- add armX_8 link for legacy reasons (same as ballX) -->
  <joint name="j_ball2_fixed" type="fixed">
    <parent link="ball2"/>
    <child link="arm2_8"/>
  </joint>
  <link name="arm2_8">
    <inertial>
      <mass value="0"/>
      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
    </inertial>
  </link>
  <!-- LINKS -->
  <link name="neck_1">
    <inertial>
      <origin xyz="0 0 0"/>
      <mass value="0.33878686"/>
      <!-- MASS/INERTIA TO DO -->
      <inertia ixx="0.0003718568" ixy="-9.42983e-05" ixz="-2.80644e-05" iyy="0.0001886879" iyz="-1.54534e-05" izz="0.0005023443"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/simple/head-base_mesh.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/simple/head-base_mesh.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </collision>
  </link>
  <link name="velodyne_motor">
    <inertial>
      <origin xyz="0 0 0"/>
      <mass value="0.33878686"/>
      <!-- MASS/INERTIA TO DO -->
      <inertia ixx="0.0003718568" ixy="-9.42983e-05" ixz="-2.80644e-05" iyy="0.0001886879" iyz="-1.54534e-05" izz="0.0005023443"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/simple/lidar_mesh.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="black"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/simple/lidar_mesh.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="black"/>
    </collision>
  </link>
  <link name="d435_head_motor">
    <inertial>
      <origin xyz="0 0 0"/>
      <mass value="0.33878686"/>
      <!-- MASS/INERTIA TO DO -->
      <inertia ixx="0.0003718568" ixy="-9.42983e-05" ixz="-2.80644e-05" iyy="0.0001886879" iyz="-1.54534e-05" izz="0.0005023443"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/simple/neck-pitch_mesh.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/simple/neck-pitch_mesh.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </collision>
  </link>
  <!--JOINTS -->
  <joint name="neck_base" type="fixed">
    <parent link="torso_2"/>
    <origin rpy="0 0 0" xyz="0.0030637045 0.0061607221 0.158"/>
    <child link="neck_1"/>
    <axis xyz="0 0 1"/>
  </joint>
  <joint name="velodyne_joint" type="revolute">
    <parent link="neck_1"/>
    <origin rpy="0 0 0" xyz="0.0822724 0 0.242"/>
    <child link="velodyne_motor"/>
    <axis xyz="0 -1 0"/>
    <limit effort="35" lower="-1.1" upper="1.1" velocity="5.7"/>
    <dynamics friction="10.0"/>
  </joint>
  <joint name="d435_head_joint" type="revolute">
    <parent link="neck_1"/>
    <origin rpy="0 0 0" xyz="0.06 0 0.1375"/>
    <child link="d435_head_motor"/>
    <axis xyz="0 -1 0"/>
    <limit effort="35" lower="-1.1" upper="1.1" velocity="5.7"/>
    <dynamics friction="10.0"/>
  </joint>
  <!-- camera body, with origin at bottom screw mount -->
  <joint name="D435_head_camera_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0.03015 0 -0.0125"/>
    <parent link="d435_head_motor"/>
    <child link="D435_head_camera_bottom_screw_frame"/>
  </joint>
  <link name="D435_head_camera_bottom_screw_frame"/>
  <joint name="D435_head_camera_link_joint" type="fixed">
    <origin rpy="0 0 0" xyz="0 0.0175 0.0125"/>
    <parent link="D435_head_camera_bottom_screw_frame"/>
    <child link="D435_head_camera_link"/>
  </joint>
  <link name="D435_head_camera_link">
    <visual>
      <origin rpy="1.570796326795 0 1.570796326795" xyz="0.0149 -0.0175 0"/>
      <geometry>
        <box size="0.09 0.025 0.02505"/>
      </geometry>
      <material name="D435_head_camera_aluminum"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 -0.0175 0"/>
      <geometry>
        <box size="0.02505 0.09 0.025"/>
      </geometry>
      <material name="D435_head_camera_aluminum"/>
    </collision>
    <inertial>
      <!-- The following are not reliable values, and should not be used for modeling -->
      <mass value="0.564"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.003881243" ixy="0.0" ixz="0.0" iyy="0.000498940" iyz="0.0" izz="0.003879257"/>
    </inertial>
  </link>
  <!-- camera body, with origin at bottom screw mount -->
  <joint name="D435i_camera_joint" type="fixed">
    <origin rpy="0 0.54 0" xyz="0.272 0 0.084"/>
    <parent link="pelvis"/>
    <child link="D435i_camera_bottom_screw_frame"/>
  </joint>
  <link name="D435i_camera_bottom_screw_frame"/>
  <joint name="D435i_camera_link_joint" type="fixed">
    <origin rpy="0 0.0050 -0.008" xyz="0.053 0.0 -0.035"/>
    <parent link="D435i_camera_bottom_screw_frame"/>
    <child link="D435i_camera_link"/>
  </joint>
  <link name="D435i_camera_link">
    <visual>
      <origin rpy="1.570796326795 0 1.570796326795" xyz="0.053 -0.0 0"/>
      <geometry>
        <box size="0.090 0.025 0.02505"/>
      </geometry>
      <material name="D435i_camera_aluminum"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 -0.0 0"/>
      <geometry>
        <box size="0.02505 0.09 0.025"/>
      </geometry>
      <material name="D435i_camera_aluminum"/>
    </collision>
    <inertial>
      <!-- The following are not reliable values, and should not be used for modeling -->
      <mass value="0.564"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.003881243" ixy="0.0" ixz="0.0" iyy="0.000498940" iyz="0.0" izz="0.003879257"/>
    </inertial>
  </link>
  <!-- /macro centauro_wheel -->
  <!-- /macro arms -->
  <!-- /macro legs -->
  <link name="hip1_1">
    <inertial>
      <origin xyz="0.0003595855 0.033338818000000006 -0.059190309"/>
      <mass value="2.3349153"/>
      <!-- org: 2 -->
      <inertia ixx="0.0068405374" ixy="1.8572633e-05" ixz="-4.8325972e-05" iyy="0.0026261815" iyz="0.0001564273" izz="0.0066933354"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/hip-yaw.stl" scale="0.001 -0.001 0.001"/>
      </geometry>
      <material name="orange"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/collision/hip-yaw.stl" scale="0.001 -0.001 0.001"/>
      </geometry>
      <material name="orange"/>
    </collision>
  </link>
  <link name="hip2_1">
    <inertial>
      <origin xyz="-4.6285952e-06 -0.2214019 0.03316685"/>
      <mass value="3.4572118"/>
      <!-- org: 4.5 -->
      <inertia ixx="0.050469853" ixy="-7.784938699999999e-06" ixz="-9.220580999999999e-06" iyy="0.008181596999999999" iyz="0.0059796848" izz="0.048213808999999996"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/hip-pitch.stl" scale="0.001 0.001 -0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/collision/hip-pitch.stl" scale="0.001 0.001 -0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </collision>
  </link>
  <link name="knee_1">
    <inertial>
      <origin xyz="0.00012063381 0.14620674 0.029740711000000003"/>
      <mass value="2.1199315"/>
      <!-- org: 3.5, foot un modeled-->
      <inertia ixx="0.018426402999999997" ixy="2.8226635999999997e-05" ixz="-2.9204348e-05" iyy="0.0047685615" iyz="-0.002640771" izz="0.017793722"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/knee.stl" scale="0.001 0.001 -0.001"/>
      </geometry>
      <material name="orange"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/collision/knee.stl" scale="0.001 0.001 -0.001"/>
      </geometry>
      <material name="orange"/>
    </collision>
  </link>
  <link name="ankle1_1">
    <inertial>
      <origin xyz="0.00012360094 -0.06274712 -0.0049068453"/>
      <mass value="1.4023368"/>
      <!-- org: 3.5, foot un modeled-->
      <inertia ixx="0.0049928829" ixy="5.762355699999999e-06" ixz="1.9606394e-05" iyy="0.0017621464" iyz="0.00045315091" izz="0.0048706328"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/ankle-pitch.stl" scale="0.001 0.001 -0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/collision/ankle-pitch.stl" scale="0.001 0.001 -0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </collision>
  </link>
  <link name="ankle2_1">
    <inertial>
      <origin xyz="0.00018512273 0.0017369405000000001 -0.091340683"/>
      <mass value="2.440929"/>
      <!-- org: 3.5, foot un modeled-->
      <inertia ixx="0.017711499" ixy="1.0713141e-05" ixz="-2.2094697e-05" iyy="0.014263485" iyz="0.00033426702" izz="0.0080747557"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/mesh_ankle-yaw_wheel23.stl" scale="0.001 -0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/collision/mesh_ankle-yaw_wheel23.stl" scale="0.001 -0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </collision>
  </link>
  <joint name="j_wheel_1" type="revolute">  <!-- actually continuous-->
    <parent link="ankle2_1"/>
    <child link="wheel_1"/>
    <origin rpy="1.57079632679 0.0 0.0" xyz="0.0 -0.0 -0.14435"/>
    <axis xyz="0 0 -1"/>
    <limit effort="35" velocity="20.0"/>
    <!-- 23 Nm, 1
            rad/s-->
  </joint>
  <link name="wheel_1">
    <inertial>
      <origin xyz="0.0 0.0 0.00015403767"/>
      <mass value="1.8569388"/>
      <inertia ixx="0.008637359099999999" ixy="0.0" ixz="-0.0" iyy="0.0086373644" iyz="-0.0" izz="0.016376551"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/mesh_wheel23.stl" scale="0.001 0.001 -0.001"/>
      </geometry>
      <material name="light_grey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.03" radius="0.124"/>
      </geometry>
      <material name="light_grey"/>
    </collision>
  </link>
  <!-- *********** CONTACT FRAME ******************-->
  <link name="contact_1"/>
  <joint name="contact_joint_1" type="fixed">
    <parent link="ankle2_1"/>
    <child link="contact_1"/>
    <origin rpy="0 0 0" xyz="0 0 -0.26835"/>
  </joint>
  <!-- ******************************** -->
  <joint name="hip_yaw_1" type="revolute">
    <parent link="pelvis"/>
    <child link="hip1_1"/>
    <origin rpy="0.0 0.0 0.0" xyz="0.25 0.125 -0.056600000000000004"/>
    <axis xyz="0 0 -1"/>
    <limit effort="304" lower="-2.03" upper="2.51" velocity="8.8"/>
    <!-- 200 Nm, 1 rad/s-->
  </joint>
  <joint name="hip_pitch_1" type="revolute">
    <parent link="hip1_1"/>
    <child link="hip2_1"/>
    <origin rpy="1.57079632679 0.0 0.0" xyz="0.0 0.1146 -0.0625"/>
    <axis xyz="0 0 -1"/>
    <limit effort="304" lower="-2.0226" upper="2.02748" velocity="8.8"/>
    <!-- 200 Nm, 1 rad/s-->
  </joint>
  <joint name="knee_pitch_1" type="revolute">
    <parent link="hip2_1"/>
    <child link="knee_1"/>
    <origin rpy="3.14159265359 0.0 0.0" xyz="0.0 -0.3 0.1182"/>
    <axis xyz="0 0 -1"/>
    <limit effort="304" lower="-2.4056" upper="2.3994" velocity="8.8"/>
    <!-- 200 Nm, 1 rad/s-->
  </joint>
  <joint name="ankle_pitch_1" type="revolute">
    <parent link="knee_1"/>
    <child link="ankle1_1"/>
    <origin rpy="3.14159265359 0.0 0.0" xyz="0.0 0.2 0.10015"/>
    <axis xyz="0 0 -1"/>
    <limit effort="147" lower="-2.4056" upper="2.3184" velocity="8.1"/>
    <!-- 98 Nm, 1 rad/s-->
  </joint>
  <joint name="ankle_yaw_1" type="revolute">
    <parent link="ankle1_1"/>
    <child link="ankle2_1"/>
    <origin rpy="-1.57079632679 0.0 0.0" xyz="0.0 -0.139 -0.00085"/>
    <axis xyz="0 0 -1"/>
    <limit effort="35" lower="-2.5626" upper="2.5384" velocity="20.0"/>
    <!-- 23 Nm, 1 rad/s-->
  </joint>
  <link name="hip1_2">
    <inertial>
      <origin xyz="0.0003595855 -0.033338818000000006 -0.059190309"/>
      <mass value="2.3349153"/>
      <!-- org: 2 -->
      <inertia ixx="0.0068405374" ixy="-1.8572633e-05" ixz="-4.8325972e-05" iyy="0.0026261815" iyz="-0.0001564273" izz="0.0066933354"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/hip-yaw.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="orange"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/collision/hip-yaw.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="orange"/>
    </collision>
  </link>
  <link name="hip2_2">
    <inertial>
      <origin xyz="-4.6285952e-06 -0.2214019 -0.03316685"/>
      <mass value="3.4572118"/>
      <!-- org: 4.5 -->
      <inertia ixx="0.050469853" ixy="-7.784938699999999e-06" ixz="9.220580999999999e-06" iyy="0.008181596999999999" iyz="-0.0059796848" izz="0.048213808999999996"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/hip-pitch.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/collision/hip-pitch.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </collision>
  </link>
  <link name="knee_2">
    <inertial>
      <origin xyz="0.00012063381 0.14620674 -0.029740711000000003"/>
      <mass value="2.1199315"/>
      <!-- org: 3.5, foot un modeled-->
      <inertia ixx="0.018426402999999997" ixy="2.8226635999999997e-05" ixz="2.9204348e-05" iyy="0.0047685615" iyz="0.002640771" izz="0.017793722"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/knee.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="orange"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/collision/knee.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="orange"/>
    </collision>
  </link>
  <link name="ankle1_2">
    <inertial>
      <origin xyz="0.00012360094 -0.06274712 0.0049068453"/>
      <mass value="1.4023368"/>
      <!-- org: 3.5, foot un modeled-->
      <inertia ixx="0.0049928829" ixy="5.762355699999999e-06" ixz="-1.9606394e-05" iyy="0.0017621464" iyz="-0.00045315091" izz="0.0048706328"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/ankle-pitch.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/collision/ankle-pitch.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </collision>
  </link>
  <link name="ankle2_2">
    <inertial>
      <origin xyz="0.00018512273 -0.0017369405000000001 -0.091340683"/>
      <mass value="2.440929"/>
      <!-- org: 3.5, foot un modeled-->
      <inertia ixx="0.017711499" ixy="-1.0713141e-05" ixz="-2.2094697e-05" iyy="0.014263485" iyz="-0.00033426702" izz="0.0080747557"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/mesh_ankle-yaw_wheel23.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/collision/mesh_ankle-yaw_wheel23.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </collision>
  </link>
  <joint name="j_wheel_2" type="revolute"> <!-- actually continuous-->
    <parent link="ankle2_2"/>
    <child link="wheel_2"/>
    <origin rpy="1.57079632679 0.0 0.0" xyz="0.0 0.0 -0.14435"/>
    <axis xyz="0 0 1"/>
    <limit effort="35" velocity="20.0"/>
    <!-- 23 Nm, 1
            rad/s-->
  </joint>
  <link name="wheel_2">
    <inertial>
      <origin xyz="0.0 0.0 -0.00015403767"/>
      <mass value="1.8569388"/>
      <inertia ixx="0.008637359099999999" ixy="0.0" ixz="0.0" iyy="0.0086373644" iyz="0.0" izz="0.016376551"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/mesh_wheel23.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="light_grey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.03" radius="0.124"/>
      </geometry>
      <material name="light_grey"/>
    </collision>
  </link>
  <!-- *********** CONTACT FRAME ******************-->
  <link name="contact_2"/>
  <joint name="contact_joint_2" type="fixed">
    <parent link="ankle2_2"/>
    <child link="contact_2"/>
    <origin rpy="0 0 0" xyz="0 0 -0.26835"/>
  </joint>
  <!-- ******************************** -->
  <joint name="hip_yaw_2" type="revolute">
    <parent link="pelvis"/>
    <child link="hip1_2"/>
    <origin rpy="0.0 0.0 0.0" xyz="0.25 -0.125 -0.056600000000000004"/>
    <axis xyz="0 0 -1"/>
    <limit effort="304" lower="-2.51" upper="2.03" velocity="8.8"/>
    <!-- 200 Nm, 1 rad/s-->
  </joint>
  <joint name="hip_pitch_2" type="revolute">
    <parent link="hip1_2"/>
    <child link="hip2_2"/>
    <origin rpy="1.57079632679 0.0 0.0" xyz="0.0 -0.1146 -0.0625"/>
    <axis xyz="0 0 1"/>
    <limit effort="304" lower="-2.0156" upper="2.0354" velocity="8.8"/>
    <!-- 200 Nm, 1 rad/s-->
  </joint>
  <joint name="knee_pitch_2" type="revolute">
    <parent link="hip2_2"/>
    <child link="knee_2"/>
    <origin rpy="3.14159265359 0.0 0.0" xyz="0.0 -0.3 -0.1182"/>
    <axis xyz="0 0 1"/>
    <limit effort="304" lower="-2.4006" upper="2.4034" velocity="8.8"/>
    <!-- 200 Nm, 1 rad/s-->
  </joint>
  <joint name="ankle_pitch_2" type="revolute">
    <parent link="knee_2"/>
    <child link="ankle1_2"/>
    <origin rpy="3.14159265359 0.0 0.0" xyz="0.0 0.2 -0.10015"/>
    <axis xyz="0 0 1"/>
    <limit effort="147" lower="-2.3266" upper="2.3914" velocity="8.1"/>
    <!-- 98 Nm, 1 rad/s-->
  </joint>
  <joint name="ankle_yaw_2" type="revolute">
    <parent link="ankle1_2"/>
    <child link="ankle2_2"/>
    <origin rpy="-1.57079632679 0.0 0.0" xyz="0.0 -0.139 0.00085"/>
    <axis xyz="0 0 -1"/>
    <limit effort="35" lower="-2.5546" upper="2.5484" velocity="20.0"/>
    <!-- 23 Nm, 1 rad/s-->
  </joint>
  <link name="hip1_3">
    <inertial>
      <origin xyz="0.0003595855 0.033338818000000006 -0.059190309"/>
      <mass value="2.3349153"/>
      <!-- org: 2 -->
      <inertia ixx="0.0068405374" ixy="1.8572633e-05" ixz="-4.8325972e-05" iyy="0.0026261815" iyz="0.0001564273" izz="0.0066933354"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/hip-yaw.stl" scale="0.001 -0.001 0.001"/>
      </geometry>
      <material name="orange"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/collision/hip-yaw.stl" scale="0.001 -0.001 0.001"/>
      </geometry>
      <material name="orange"/>
    </collision>
  </link>
  <link name="hip2_3">
    <inertial>
      <origin xyz="-4.6285952e-06 -0.2214019 0.03316685"/>
      <mass value="3.4572118"/>
      <!-- org: 4.5 -->
      <inertia ixx="0.050469853" ixy="-7.784938699999999e-06" ixz="-9.220580999999999e-06" iyy="0.008181596999999999" iyz="0.0059796848" izz="0.048213808999999996"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/hip-pitch.stl" scale="0.001 0.001 -0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/collision/hip-pitch.stl" scale="0.001 0.001 -0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </collision>
  </link>
  <link name="knee_3">
    <inertial>
      <origin xyz="0.00012063381 0.14620674 0.029740711000000003"/>
      <mass value="2.1199315"/>
      <!-- org: 3.5, foot un modeled-->
      <inertia ixx="0.018426402999999997" ixy="2.8226635999999997e-05" ixz="-2.9204348e-05" iyy="0.0047685615" iyz="-0.002640771" izz="0.017793722"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/knee.stl" scale="0.001 0.001 -0.001"/>
      </geometry>
      <material name="orange"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/collision/knee.stl" scale="0.001 0.001 -0.001"/>
      </geometry>
      <material name="orange"/>
    </collision>
  </link>
  <link name="ankle1_3">
    <inertial>
      <origin xyz="0.00012360094 -0.06274712 -0.0049068453"/>
      <mass value="1.4023368"/>
      <!-- org: 3.5, foot un modeled-->
      <inertia ixx="0.0049928829" ixy="5.762355699999999e-06" ixz="1.9606394e-05" iyy="0.0017621464" iyz="0.00045315091" izz="0.0048706328"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/ankle-pitch.stl" scale="0.001 0.001 -0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/collision/ankle-pitch.stl" scale="0.001 0.001 -0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </collision>
  </link>
  <link name="ankle2_3">
    <inertial>
      <origin xyz="0.00018512273 0.0017369405000000001 -0.091340683"/>
      <mass value="2.440929"/>
      <!-- org: 3.5, foot un modeled-->
      <inertia ixx="0.017711499" ixy="1.0713141e-05" ixz="-2.2094697e-05" iyy="0.014263485" iyz="0.00033426702" izz="0.0080747557"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/mesh_ankle-yaw_wheel23.stl" scale="0.001 -0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/collision/mesh_ankle-yaw_wheel23.stl" scale="0.001 -0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </collision>
  </link>
  <joint name="j_wheel_3" type="revolute"> <!-- actually continuous-->
    <parent link="ankle2_3"/>
    <child link="wheel_3"/>
    <origin rpy="1.57079632679 0.0 0.0" xyz="0.0 -0.0 -0.14435"/>
    <axis xyz="0 0 -1"/>
    <limit effort="35" velocity="20.0"/>
    <!-- 23 Nm, 1
            rad/s-->
  </joint>
  <link name="wheel_3">
    <inertial>
      <origin xyz="0.0 0.0 0.00015403767"/>
      <mass value="1.8569388"/>
      <inertia ixx="0.008637359099999999" ixy="0.0" ixz="-0.0" iyy="0.0086373644" iyz="-0.0" izz="0.016376551"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/mesh_wheel23.stl" scale="0.001 0.001 -0.001"/>
      </geometry>
      <material name="light_grey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.03" radius="0.124"/>
      </geometry>
      <material name="light_grey"/>
    </collision>
  </link>
  <!-- *********** CONTACT FRAME ******************-->
  <link name="contact_3"/>
  <joint name="contact_joint_3" type="fixed">
    <parent link="ankle2_3"/>
    <child link="contact_3"/>
    <origin rpy="0 0 0" xyz="0 0 -0.26835"/>
  </joint>
  <!-- ******************************** -->
  <joint name="hip_yaw_3" type="revolute">
    <parent link="pelvis"/>
    <child link="hip1_3"/>
    <origin rpy="0.0 0.0 0.0" xyz="-0.25 0.125 -0.056600000000000004"/>
    <axis xyz="0 0 -1"/>
    <limit effort="304" lower="-2.51" upper="2.03" velocity="8.8"/>
    <!-- 200 Nm, 1 rad/s-->
  </joint>
  <joint name="hip_pitch_3" type="revolute">
    <parent link="hip1_3"/>
    <child link="hip2_3"/>
    <origin rpy="1.57079632679 0.0 0.0" xyz="0.0 0.1146 -0.0625"/>
    <axis xyz="0 0 -1"/>
    <limit effort="304" lower="-2.0126" upper="2.0384" velocity="8.8"/>
    <!-- 200 Nm, 1 rad/s-->
  </joint>
  <joint name="knee_pitch_3" type="revolute">
    <parent link="hip2_3"/>
    <child link="knee_3"/>
    <origin rpy="3.14159265359 0.0 0.0" xyz="0.0 -0.3 0.1182"/>
    <axis xyz="0 0 -1"/>
    <limit effort="304" lower="-2.4056" upper="2.4064" velocity="8.8"/>
    <!-- 200 Nm, 1 rad/s-->
  </joint>
  <joint name="ankle_pitch_3" type="revolute">
    <parent link="knee_3"/>
    <child link="ankle1_3"/>
    <origin rpy="3.14159265359 0.0 0.0" xyz="0.0 0.2 0.10015"/>
    <axis xyz="0 0 -1"/>
    <limit effort="147" lower="-2.4696" upper="2.4464" velocity="8.1"/>
    <!-- 98 Nm, 1 rad/s-->
  </joint>
  <joint name="ankle_yaw_3" type="revolute">
    <parent link="ankle1_3"/>
    <child link="ankle2_3"/>
    <origin rpy="-1.57079632679 0.0 0.0" xyz="0.0 -0.139 -0.00085"/>
    <axis xyz="0 0 -1"/>
    <limit effort="35" lower="-2.5606" upper="2.5454" velocity="20.0"/>
    <!-- 23 Nm, 1 rad/s-->
  </joint>
  <link name="hip1_4">
    <inertial>
      <origin xyz="0.0003595855 -0.033338818000000006 -0.059190309"/>
      <mass value="2.3349153"/>
      <!-- org: 2 -->
      <inertia ixx="0.0068405374" ixy="-1.8572633e-05" ixz="-4.8325972e-05" iyy="0.0026261815" iyz="-0.0001564273" izz="0.0066933354"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/hip-yaw.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="orange"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/collision/hip-yaw.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="orange"/>
    </collision>
  </link>
  <link name="hip2_4">
    <inertial>
      <origin xyz="-4.6285952e-06 -0.2214019 -0.03316685"/>
      <mass value="3.4572118"/>
      <!-- org: 4.5 -->
      <inertia ixx="0.050469853" ixy="-7.784938699999999e-06" ixz="9.220580999999999e-06" iyy="0.008181596999999999" iyz="-0.0059796848" izz="0.048213808999999996"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/hip-pitch.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/collision/hip-pitch.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </collision>
  </link>
  <link name="knee_4">
    <inertial>
      <origin xyz="0.00012063381 0.14620674 -0.029740711000000003"/>
      <mass value="2.1199315"/>
      <!-- org: 3.5, foot un modeled-->
      <inertia ixx="0.018426402999999997" ixy="2.8226635999999997e-05" ixz="2.9204348e-05" iyy="0.0047685615" iyz="0.002640771" izz="0.017793722"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/knee.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="orange"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/collision/knee.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="orange"/>
    </collision>
  </link>
  <link name="ankle1_4">
    <inertial>
      <origin xyz="0.00012360094 -0.06274712 0.0049068453"/>
      <mass value="1.4023368"/>
      <!-- org: 3.5, foot un modeled-->
      <inertia ixx="0.0049928829" ixy="5.762355699999999e-06" ixz="-1.9606394e-05" iyy="0.0017621464" iyz="-0.00045315091" izz="0.0048706328"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/ankle-pitch.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/collision/ankle-pitch.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </collision>
  </link>
  <link name="ankle2_4">
    <inertial>
      <origin xyz="0.00018512273 -0.0017369405000000001 -0.091340683"/>
      <mass value="2.440929"/>
      <!-- org: 3.5, foot un modeled-->
      <inertia ixx="0.017711499" ixy="-1.0713141e-05" ixz="-2.2094697e-05" iyy="0.014263485" iyz="-0.00033426702" izz="0.0080747557"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/mesh_ankle-yaw_wheel23.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </visual>
    <collision>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/collision/mesh_ankle-yaw_wheel23.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="dark_grey"/>
    </collision>
  </link>
  <joint name="j_wheel_4" type="revolute"> <!-- actually continuous-->
    <parent link="ankle2_4"/>
    <child link="wheel_4"/>
    <origin rpy="1.57079632679 0.0 0.0" xyz="0.0 0.0 -0.14435"/>
    <axis xyz="0 0 1"/>
    <limit effort="35" velocity="20.0"/>
    <!-- 23 Nm, 1
            rad/s-->
  </joint>
  <link name="wheel_4">
    <inertial>
      <origin xyz="0.0 0.0 -0.00015403767"/>
      <mass value="1.8569388"/>
      <inertia ixx="0.008637359099999999" ixy="0.0" ixz="0.0" iyy="0.0086373644" iyz="0.0" izz="0.016376551"/>
    </inertial>
    <visual>
      <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0"/>
      <geometry>
        <mesh filename="package://example-robot-data/robots/centauro_description/meshes/v2/mesh_wheel23.stl" scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="light_grey"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.03" radius="0.124"/>
      </geometry>
      <material name="light_grey"/>
    </collision>
  </link>
  <!-- *********** CONTACT FRAME ******************-->
  <link name="contact_4"/>
  <joint name="contact_joint_4" type="fixed">
    <parent link="ankle2_4"/>
    <child link="contact_4"/>
    <origin rpy="0 0 0" xyz="0 0 -0.26835"/>
  </joint>
  <!-- ******************************** -->
  <joint name="hip_yaw_4" type="revolute">
    <parent link="pelvis"/>
    <child link="hip1_4"/>
    <origin rpy="0.0 0.0 0.0" xyz="-0.25 -0.125 -0.056600000000000004"/>
    <axis xyz="0 0 -1"/>
    <limit effort="304" lower="-2.03" upper="2.51" velocity="8.8"/>
    <!-- 200 Nm, 1 rad/s-->
  </joint>
  <joint name="hip_pitch_4" type="revolute">
    <parent link="hip1_4"/>
    <child link="hip2_4"/>
    <origin rpy="1.57079632679 0.0 0.0" xyz="0.0 -0.1146 -0.0625"/>
    <axis xyz="0 0 1"/>
    <limit effort="304" lower="-2.0326" upper="2.0524" velocity="8.8"/>
    <!-- 200 Nm, 1 rad/s-->
  </joint>
  <joint name="knee_pitch_4" type="revolute">
    <parent link="hip2_4"/>
    <child link="knee_4"/>
    <origin rpy="3.14159265359 0.0 0.0" xyz="0.0 -0.3 -0.1182"/>
    <axis xyz="0 0 1"/>
    <limit effort="304" lower="-2.4176" upper="2.3884" velocity="8.8"/>
    <!-- 200 Nm, 1 rad/s-->
  </joint>
  <joint name="ankle_pitch_4" type="revolute">
    <parent link="knee_4"/>
    <child link="ankle1_4"/>
    <origin rpy="3.14159265359 0.0 0.0" xyz="0.0 0.2 -0.10015"/>
    <axis xyz="0 0 1"/>
    <limit effort="147" lower="-2.3426" upper="2.3934" velocity="8.1"/>
    <!-- 98 Nm, 1 rad/s-->
  </joint>
  <joint name="ankle_yaw_4" type="revolute">
    <parent link="ankle1_4"/>
    <child link="ankle2_4"/>
    <origin rpy="-1.57079632679 0.0 0.0" xyz="0.0 -0.139 0.00085"/>
    <axis xyz="0 0 -1"/>
    <limit effort="35" lower="-2.6046" upper="2.5514" velocity="20.0"/>
    <!-- 23 Nm, 1 rad/s-->
  </joint>
</robot>
