<?xml version='1.0' encoding='UTF-8'?>
<robot name="stickBot">
  <link name="root_link">
    <inertial>
      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 -1.5707963267948966"/>
      <mass value="2.52649319"/>
      <inertia ixx="0.02" ixy="0.0" ixz="0.0" iyy="0.02" iyz="0.0" izz="0.02"/>
      <!--TODO: Real inertia for this link is 0.01 but this makes model unusable-->
      <!--remove when https://github.com/icub-tech-iit/ergocub-gazebo-simulations/issues/18 is fixed -->
    </inertial>
    <visual>
      <geometry>
        <box size="0.093 0.095 0.142"/>
      </geometry>
      <material name="green"/>
      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 -1.5707963267948966"/>
    </visual>
  </link>
  <link name="torso_1">
    <inertial>
      <origin xyz="0.0 0.0 0.075" rpy="0.0 0.0 -1.5707963267948966"/>
      <mass value="1.714262538"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <box size="0.081  0.124  0.1505"/>
      </geometry>
      <material name="black"/>
      <origin xyz="0.0 0.0 0.075" rpy="0.0 0.0 -1.5707963267948966"/>
    </visual>
  </link>
  <link name="torso_2">
    <inertial>
      <origin xyz="0.0 0.0 0.0669" rpy="0.0 0.0 -1.5707963267948966"/>
      <mass value="1.374672951"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <box size="0.1356 0.0897 0.1338"/>
      </geometry>
      <material name="red"/>
      <origin xyz="0.0 0.0 0.0669" rpy="0.0 0.0 -1.5707963267948966"/>
    </visual>
  </link>
  <link name="chest">
    <inertial>
      <origin xyz="0.0 0.0 0.09138800000000014" rpy="0.0 0.0 -1.5707963267948966"/>
      <mass value="5.771074695"/>
      <inertia ixx="0.0433194564569098" ixy="0.0" ixz="0.0" iyy="0.04108113070876732" iyz="0.0" izz="0.042305752759046314"/>
    </inertial>
    <visual>
      <geometry>
        <box size="0.2041 0.2152 0.2092"/>
      </geometry>
      <material name="blue"/>
      <origin xyz="0.0 0.0 0.09138800000000014" rpy="0.0 0.0 -1.5707963267948966"/>
    </visual>
  </link>
  <link name="r_shoulder_1">
    <inertial>
      <origin xyz="0.0 -0.07445 0.0" rpy="-0.19999999999999998 1.5549999999999993 -1.840815419889917"/>
      <mass value="1.380716446"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <cylinder radius="0.04" length="0.1569"/>
      </geometry>
      <material name="yellow"/>
      <origin xyz="0.0 -0.07445 0.0" rpy="-0.19999999999999998 1.5549999999999993 -1.840815419889917"/>
    </visual>
  </link>
  <link name="r_shoulder_2">
    <inertial>
      <origin xyz="0.0 0.0 0.0" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
      <mass value="0.304658373"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <sphere radius="0.06"/>
      </geometry>
      <material name="pink"/>
      <origin xyz="0.0 0.0 0.0" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
    </visual>
  </link>
  <link name="r_shoulder_3">
    <inertial>
      <origin xyz="0.0 0.0 -0.01" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
      <mass value="0.24467077"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <cylinder radius="0.03" length="0.03"/>
      </geometry>
      <material name="cyan"/>
      <origin xyz="0.0 0.0 -0.01" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
    </visual>
  </link>
  <link name="r_upper_arm">
    <inertial>
      <origin xyz="0.0 0.0 -0.05705" rpy="0.0 0.0 -1.8462350402422587"/>
      <mass value="1.960447492"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <cylinder radius="0.05" length="0.1581"/>
      </geometry>
      <material name="green"/>
      <origin xyz="0.0 0.0 -0.05705" rpy="0.0 0.0 -1.8462350402422587"/>
    </visual>
  </link>
  <link name="r_elbow_1">
    <inertial>
      <origin xyz="-0.02 0.0 -0.02" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
      <mass value="0.217609341"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <sphere radius="0.04"/>
      </geometry>
      <material name="white"/>
      <origin xyz="-0.02 0.0 -0.02" rpy="-0.07156446033079357 0.06196992338505823 -1.8462350402422587"/>
    </visual>
  </link>
  <link name="r_forearm">
    <inertial>
      <origin xyz="-0.005 0.0 -0.04001" rpy="0.0 0.0 -1.8462350402422587"/>
      <mass value="0.854281652"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <cylinder radius="0.05" length="0.1581"/>
      </geometry>
      <material name="dblue"/>
      <origin xyz="-0.005 0.0 -0.04001" rpy="0.0 0.0 -1.8462350402422587"/>
    </visual>
    <collision>
      <geometry>
        <cylinder radius="0.05" length="0.1581"/>
      </geometry>
      <origin xyz="-0.005 0.0 -0.04001" rpy="0.0 0.0 -1.8462350402422587"/>
    </collision>
  </link>
  <link name="r_wrist_1">
    <inertial>
      <origin xyz="-0.015 0.0 0.0" rpy="3.0700281932589997 1.3345926535897934 0.11659265358979323"/>
      <mass value="0.032535096"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <cylinder radius="0.011" length="0.04"/>
      </geometry>
      <material name="dgreen"/>
      <origin xyz="-0.015 0.0 0.0" rpy="3.0700281932589997 1.3345926535897934 0.11659265358979323"/>
    </visual>
  </link>
  <link name="r_hand">
    <inertial>
      <origin xyz="0.025 0.0 -0.08105" rpy="0.0 0.0 0.0"/>
      <mass value="0.2499009"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <box size="0.124  0.0736 0.1621"/>
      </geometry>
      <material name="gray"/>
      <origin xyz="0.025 0.0 -0.08105" rpy="0.0 0.0 0.0"/>
    </visual>
    <collision>
      <geometry>
        <box size="0.124  0.0736 0.1621"/>
      </geometry>
      <origin xyz="0.025 0.0 -0.08105" rpy="0.0 0.0 0.0"/>
    </collision>
  </link>
  <link name="l_shoulder_1">
    <inertial>
      <origin xyz="0.0 0.07445 0.0" rpy="0.19999999999999998 1.5549999999999993 1.840815419889917"/>
      <mass value="1.376687449"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <cylinder radius="0.04" length="0.1569"/>
      </geometry>
      <material name="green"/>
      <origin xyz="0.0 0.07445 0.0" rpy="0.19999999999999998 1.5549999999999993 1.840815419889917"/>
    </visual>
  </link>
  <link name="l_shoulder_2">
    <inertial>
      <origin xyz="0.0 0.0 0.0" rpy="-0.07156449498805928 -0.06196998841864677 -1.2953574498236577"/>
      <mass value="0.304658373"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <sphere radius="0.06"/>
      </geometry>
      <material name="black"/>
      <origin xyz="0.0 0.0 0.0" rpy="-0.07156449498805928 -0.06196998841864677 -1.2953574498236577"/>
    </visual>
  </link>
  <link name="l_shoulder_3">
    <inertial>
      <origin xyz="0.0 0.0 -0.01" rpy="-0.07156449498805928 -0.06196998841864677 -1.2953574498236577"/>
      <mass value="0.242239185"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <cylinder radius="0.03" length="0.03"/>
      </geometry>
      <material name="red"/>
      <origin xyz="0.0 0.0 -0.01" rpy="-0.07156449498805928 -0.06196998841864677 -1.2953574498236577"/>
    </visual>
  </link>
  <link name="l_upper_arm">
    <inertial>
      <origin xyz="0.0 0.0 -0.05705" rpy="0.0 0.0 -1.2953574498236577"/>
      <mass value="1.968325789"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <cylinder radius="0.05" length="0.1581"/>
      </geometry>
      <material name="blue"/>
      <origin xyz="0.0 0.0 -0.05705" rpy="0.0 0.0 -1.2953574498236577"/>
    </visual>
  </link>
  <link name="l_elbow_1">
    <inertial>
      <origin xyz="-0.02 0.0 -0.02" rpy="-0.07156449498805928 -0.06196998841864677 -1.2953574498236577"/>
      <mass value="0.217768231"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <sphere radius="0.04"/>
      </geometry>
      <material name="yellow"/>
      <origin xyz="-0.02 0.0 -0.02" rpy="-0.07156449498805928 -0.06196998841864677 -1.2953574498236577"/>
    </visual>
  </link>
  <link name="l_forearm">
    <inertial>
      <origin xyz="-0.005 0.0 -0.04001" rpy="0.0 0.0 -1.2953574498236577"/>
      <mass value="0.858696525"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <cylinder radius="0.05" length="0.1581"/>
      </geometry>
      <material name="pink"/>
      <origin xyz="-0.005 0.0 -0.04001" rpy="0.0 0.0 -1.2953574498236577"/>
    </visual>
    <collision>
      <geometry>
        <cylinder radius="0.05" length="0.1581"/>
      </geometry>
      <origin xyz="-0.005 0.0 -0.04001" rpy="0.0 0.0 -1.2953574498236577"/>
    </collision>
  </link>
  <link name="l_wrist_1">
    <inertial>
      <origin xyz="-0.015 0.0 0.0" rpy="3.0700281932589997 1.3345926535897934 0.11659265358979323"/>
      <mass value="0.032535096"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <cylinder radius="0.011" length="0.04"/>
      </geometry>
      <material name="cyan"/>
      <origin xyz="-0.015 0.0 0.0" rpy="3.0700281932589997 1.3345926535897934 0.11659265358979323"/>
    </visual>
  </link>
  <link name="l_hand">
    <inertial>
      <origin xyz="0.025 0.0 -0.08105" rpy="0.0 0.0 0.0"/>
      <mass value="0.249250207"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <box size="0.124  0.0736 0.1621"/>
      </geometry>
      <material name="green"/>
      <origin xyz="0.025 0.0 -0.08105" rpy="0.0 0.0 0.0"/>
    </visual>
    <collision>
      <geometry>
        <box size="0.124  0.0736 0.1621"/>
      </geometry>
      <origin xyz="0.025 0.0 -0.08105" rpy="0.0 0.0 0.0"/>
    </collision>
  </link>
  <link name="neck_1">
    <inertial>
      <origin xyz="0.004 0.0 0.03500000000000002" rpy="-1.5707963267948968 0.0 0.0"/>
      <mass value="0.359964395"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <cylinder radius="0.033" length="0.089"/>
      </geometry>
      <material name="white"/>
      <origin xyz="0.004 0.0 0.03500000000000002" rpy="-1.5707963267948968 0.0 0.0"/>
    </visual>
  </link>
  <link name="neck_2">
    <inertial>
      <origin xyz="-0.02 0.0 0.015" rpy="-1.5707963267948968 0.0 -1.5707963267948968"/>
      <mass value="0.118740971"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <cylinder radius="0.027" length="0.013"/>
      </geometry>
      <material name="dblue"/>
      <origin xyz="-0.02 0.0 0.015" rpy="-1.5707963267948968 0.0 -1.5707963267948968"/>
    </visual>
  </link>
  <link name="neck_3">
    <inertial>
      <origin xyz="0.0 0.0 0.028" rpy="0.0 0.0 -1.5707963267948968"/>
      <mass value="0.179822835"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <cylinder radius="0.0225" length="0.09"/>
      </geometry>
      <material name="dgreen"/>
      <origin xyz="0.0 0.0 0.028" rpy="0.0 0.0 -1.5707963267948968"/>
    </visual>
  </link>
  <link name="head">
    <inertial>
      <origin xyz="0.0 0.0 0.025" rpy="-1.5707963267948966 0.0 -1.5707963267948966"/>
      <mass value="1.831737118"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <sphere radius="0.08"/>
      </geometry>
      <material name="gray"/>
      <origin xyz="0.0 0.0 0.025" rpy="-1.5707963267948966 0.0 -1.5707963267948966"/>
    </visual>
  </link>
  <link name="camera_tilt">
    <inertial>
      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
      <mass value="0.06"/>
      <inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
    </inertial>
    <visual>
      <geometry>
        <cylinder radius="0.03" length="0.09"/>
      </geometry>
      <material name="blue"/>
      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
    </visual>
  </link>
  <link name="lidar">
    <inertial>
      <origin xyz="0.0 0.0 0.17" rpy="0.0 0.0 0.0"/>
      <mass value="0.2"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <cylinder radius="0.04" length="0.04"/>
      </geometry>
      <material name="blue"/>
      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
    </visual>
  </link>
  <link name="r_hip_1">
    <inertial>
      <origin xyz="-0.01 -0.041826 -0.01" rpy="1.5707963267948966 1.5707963267948966 0.0"/>
      <mass value="2.134290199"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <cylinder radius="0.0535" length="0.083652"/>
      </geometry>
      <material name="green"/>
      <origin xyz="-0.01 -0.041826 -0.01" rpy="1.5707963267948966 1.5707963267948966 0.0"/>
    </visual>
  </link>
  <link name="r_hip_2">
    <inertial>
      <origin xyz="0.02 0.0 -0.02" rpy="0.0 0.0 -1.5707963267948966"/>
      <mass value="1.779000764"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <box size="0.0702 0.1578 0.1315"/>
      </geometry>
      <material name="black"/>
      <origin xyz="0.02 0.0 -0.02" rpy="0.0 0.0 -1.5707963267948966"/>
    </visual>
  </link>
  <link name="r_hip_3">
    <inertial>
      <origin xyz="0.0 0.0 -0.049999999999999996" rpy="0.0 0.0 -1.5707963267948966"/>
      <mass value="2.1678368"/>
      <inertia ixx="0.0114770699784" ixy="0.0" ixz="0.0" iyy="0.0114770699784" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <cylinder radius="0.075" length="0.216"/>
      </geometry>
      <material name="red"/>
      <origin xyz="0.0 0.0 -0.049999999999999996" rpy="0.0 0.0 -1.5707963267948966"/>
    </visual>
  </link>
  <link name="r_upper_leg">
    <inertial>
      <origin xyz="0.0 0.0 -0.07" rpy="-1.5707963267948966 0.0 0.0"/>
      <mass value="0.980691904"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <box size="0.0908 0.1598 0.0878"/>
      </geometry>
      <material name="blue"/>
      <origin xyz="0.0 0.0 -0.07" rpy="-1.5707963267948966 0.0 0.0"/>
    </visual>
  </link>
  <link name="r_lower_leg">
    <inertial>
      <origin xyz="-0.02 0.015 -0.13" rpy="0.0 0.0 -1.5707963267948966"/>
      <mass value="5.074105512"/>
      <inertia ixx="0.022273208987050007" ixy="0.0" ixz="0.0" iyy="0.022273208987050007" iyz="0.0" izz="0.010719047894100001"/>
    </inertial>
    <visual>
      <geometry>
        <cylinder radius="0.065" length="0.2"/>
      </geometry>
      <material name="yellow"/>
      <origin xyz="-0.02 0.015 -0.13" rpy="0.0 0.0 -1.5707963267948966"/>
    </visual>
  </link>
  <link name="r_ankle_1">
    <inertial>
      <origin xyz="0.0 0.01 -0.015" rpy="0.0 1.5707963267948966 0.0"/>
      <mass value="0.346176903"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <box size="0.085207  0.0601674 0.14254  "/>
      </geometry>
      <material name="pink"/>
      <origin xyz="0.0 0.01 -0.015" rpy="0.0 1.5707963267948966 0.0"/>
    </visual>
  </link>
  <link name="r_ankle_2">
    <inertial>
      <origin xyz="0.06 0.0 -0.014" rpy="0.0 1.5707963267948966 0.0"/>
      <mass value="1.00828013"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <box size="0.0927 0.0634 0.2055"/>
      </geometry>
      <material name="cyan"/>
      <origin xyz="0.06 0.0 -0.014" rpy="0.0 1.5707963267948966 0.0"/>
    </visual>
  </link>
  <link name="r_foot_front">
    <inertial>
      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 -1.5707963267948966"/>
      <mass value="0.17569926"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <box size="0.1    0.117  0.0103"/>
      </geometry>
      <material name="green"/>
      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 -1.5707963267948966"/>
    </visual>
    <collision>
      <geometry>
        <box size="0.1    0.117  0.0103"/>
      </geometry>
      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 -1.5707963267948966"/>
    </collision>
  </link>
  <link name="r_foot_rear">
    <inertial>
      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 -1.5707963267948966"/>
      <mass value="0.174817799"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <box size="0.1    0.117  0.0103"/>
      </geometry>
      <material name="white"/>
      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 -1.5707963267948966"/>
    </visual>
    <collision>
      <geometry>
        <box size="0.1    0.117  0.0103"/>
      </geometry>
      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 -1.5707963267948966"/>
    </collision>
  </link>
  <link name="l_hip_1">
    <inertial>
      <origin xyz="-0.01 0.041826 -0.01" rpy="1.5707963267948966 1.5707963267948966 0.0"/>
      <mass value="2.108461113"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <cylinder radius="0.0535" length="0.083652"/>
      </geometry>
      <material name="dblue"/>
      <origin xyz="-0.01 0.041826 -0.01" rpy="1.5707963267948966 1.5707963267948966 0.0"/>
    </visual>
  </link>
  <link name="l_hip_2">
    <inertial>
      <origin xyz="0.02 0.0 -0.02" rpy="0.0 0.0 -1.5707963267948966"/>
      <mass value="1.781270621"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <box size="0.0702 0.1578 0.1315"/>
      </geometry>
      <material name="dgreen"/>
      <origin xyz="0.02 0.0 -0.02" rpy="0.0 0.0 -1.5707963267948966"/>
    </visual>
  </link>
  <link name="l_hip_3">
    <inertial>
      <origin xyz="0.0 0.0 -0.049999999999999996" rpy="0.0 0.0 -1.5707963267948966"/>
      <mass value="2.169151426"/>
      <inertia ixx="0.011484029937100501" ixy="0.0" ixz="0.0" iyy="0.011484029937100501" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <cylinder radius="0.075" length="0.216"/>
      </geometry>
      <material name="gray"/>
      <origin xyz="0.0 0.0 -0.049999999999999996" rpy="0.0 0.0 -1.5707963267948966"/>
    </visual>
  </link>
  <link name="l_upper_leg">
    <inertial>
      <origin xyz="0.0 0.0 -0.07" rpy="-1.5707963267948966 0.0 0.0"/>
      <mass value="0.968264435"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <box size="0.0908 0.1598 0.0878"/>
      </geometry>
      <material name="green"/>
      <origin xyz="0.0 0.0 -0.07" rpy="-1.5707963267948966 0.0 0.0"/>
    </visual>
  </link>
  <link name="l_lower_leg">
    <inertial>
      <origin xyz="-0.02 -0.015 -0.13" rpy="0.0 0.0 -1.5707963267948966"/>
      <mass value="5.063049414"/>
      <inertia ixx="0.022224677323537508" ixy="0.0" ixz="0.0" iyy="0.022224677323537508" iyz="0.0" izz="0.010695691887075"/>
    </inertial>
    <visual>
      <geometry>
        <cylinder radius="0.065" length="0.2"/>
      </geometry>
      <material name="black"/>
      <origin xyz="-0.02 -0.015 -0.13" rpy="0.0 0.0 -1.5707963267948966"/>
    </visual>
  </link>
  <link name="l_ankle_1">
    <inertial>
      <origin xyz="0.0 -0.01 -0.015" rpy="0.0 1.5707963267948966 0.0"/>
      <mass value="0.3461769035"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <box size="0.085207  0.0601674 0.14254  "/>
      </geometry>
      <material name="red"/>
      <origin xyz="0.0 -0.01 -0.015" rpy="0.0 1.5707963267948966 0.0"/>
    </visual>
  </link>
  <link name="l_ankle_2">
    <inertial>
      <origin xyz="0.06 0.0 -0.014" rpy="0.0 1.5707963267948966 0.0"/>
      <mass value="1.011533592"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <box size="0.0927 0.0634 0.2055"/>
      </geometry>
      <material name="blue"/>
      <origin xyz="0.06 0.0 -0.014" rpy="0.0 1.5707963267948966 0.0"/>
    </visual>
  </link>
  <link name="l_foot_rear">
    <inertial>
      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 -1.5707963267948966"/>
      <mass value="0.174597434"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <box size="0.1    0.117  0.0103"/>
      </geometry>
      <material name="yellow"/>
      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 -1.5707963267948966"/>
    </visual>
    <collision>
      <geometry>
        <box size="0.1    0.117  0.0103"/>
      </geometry>
      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 -1.5707963267948966"/>
    </collision>
  </link>
  <link name="l_foot_front">
    <inertial>
      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 -1.5707963267948966"/>
      <mass value="0.175038164"/>
      <inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.01" iyz="0.0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <box size="0.1    0.117  0.0103"/>
      </geometry>
      <material name="pink"/>
      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 -1.5707963267948966"/>
    </visual>
    <collision>
      <geometry>
        <box size="0.1    0.117  0.0103"/>
      </geometry>
      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 -1.5707963267948966"/>
    </collision>
  </link>
  <link name="base_link"/>
  <link name="l_sole"/>
  <link name="r_sole"/>
  <link name="imu_frame"/>
  <joint name="torso_roll" type="revolute">
    <dynamics damping="2.0" friction="0.0"/>
    <limit effort="50000.0" velocity="120.0" lower="-0.4014257279586958" upper="0.4014257279586958"/>
    <parent link="root_link"/>
    <child link="torso_1"/>
    <axis xyz="-1.  0.  0."/>
    <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="torso_pitch" type="revolute">
    <dynamics damping="2.0" friction="0.0"/>
    <limit effort="50000.0" velocity="120.0" lower="-0.3141592653589793" upper="0.7853981633974483"/>
    <parent link="torso_1"/>
    <child link="torso_2"/>
    <axis xyz="0. 1. 0."/>
    <origin xyz="0.0 0.0 0.0725" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="torso_yaw" type="revolute">
    <dynamics damping="2.0" friction="0.0"/>
    <limit effort="50000.0" velocity="120.0" lower="-0.7504915783575618" upper="0.7504915783575618"/>
    <parent link="torso_2"/>
    <child link="chest"/>
    <axis xyz=" 0.  0. -1."/>
    <origin xyz="0.0 0.0 0.06447" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="r_shoulder_pitch" type="revolute">
    <dynamics damping="2.0" friction="0.0"/>
    <limit effort="50000.0" velocity="120.0" lower="-1.53588974175501" upper="0.22689280275926285"/>
    <parent link="chest"/>
    <child link="r_shoulder_1"/>
    <axis xyz="0. 1. 0."/>
    <origin xyz="-0.03 -0.055555 0.163" rpy="-0.25326164431680925 0.0 0.26179970938509745"/>
  </joint>
  <joint name="r_shoulder_roll" type="revolute">
    <dynamics damping="2.0" friction="0.0"/>
    <limit effort="50000.0" velocity="120.0" lower="0.20943951023931956" upper="2.792526803190927"/>
    <parent link="r_shoulder_1"/>
    <child link="r_shoulder_2"/>
    <axis xyz="-1.00000000e+00  4.21050217e-08  2.46304855e-08"/>
    <origin xyz="0.007489008185044645 -0.11150041046657978 0.0004027858966975245" rpy="0.17453306234069357 -0.053747792579130614 0.0"/>
  </joint>
  <joint name="r_arm_ft_sensor" type="fixed">
    <dynamics damping="0.1"/>
    <parent link="r_shoulder_2"/>
    <child link="r_shoulder_3"/>
    <axis xyz="1. 0. 0."/>
    <origin xyz="-0.006849946385389875 2.1904996055921444e-07 -0.05429998157789099" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="r_shoulder_yaw" type="revolute">
    <dynamics damping="2.0" friction="0.0"/>
    <limit effort="50000.0" velocity="120.0" lower="-0.8726646259971648" upper="1.3962634015954636"/>
    <parent link="r_shoulder_3"/>
    <child link="r_upper_arm"/>
    <axis xyz="-3.78762082e-08  2.80519699e-08  1.00000000e+00"/>
    <origin xyz="0.0 4.2183273077367467e-07 -0.022499941373153573" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="r_elbow" type="revolute">
    <dynamics damping="2.0" friction="0.0"/>
    <limit effort="50000.0" velocity="120.0" lower="-0.05235987755982989" upper="1.3089969389957472"/>
    <parent link="r_upper_arm"/>
    <child link="r_elbow_1"/>
    <axis xyz=" 2.49495760e-07 -1.00000000e+00  2.79424794e-08"/>
    <origin xyz="0.009999931922603816 0.0014996401320274133 -0.12369999999999999" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="r_wrist_prosup" type="revolute">
    <dynamics damping="2.0" friction="0.0"/>
    <limit effort="50000.0" velocity="120.0" lower="-1.53588974175501" upper="1.53588974175501"/>
    <parent link="r_elbow_1"/>
    <child link="r_forearm"/>
    <axis xyz="-3.78762082e-08  2.80519699e-08  1.00000000e+00"/>
    <origin xyz="-0.014999869468812953 -0.001999649905318951 -0.05530006367251966" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="r_wrist_pitch" type="revolute">
    <dynamics damping="2.0" friction="0.0"/>
    <limit effort="50000.0" velocity="120.0" lower="-1.2217304763960306" upper="0.4363323129985824"/>
    <parent link="r_forearm"/>
    <child link="r_wrist_1"/>
    <axis xyz=" 1.00000000e+00 -4.21050217e-08 -2.46304855e-08"/>
    <origin xyz="0.016299953353948633 -0.002007442890811839 -0.10749999999999998" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="r_wrist_yaw" type="revolute">
    <dynamics damping="2.0"/>
    <limit effort="50000.0" velocity="120.0" lower="-0.2617993877991494" upper="0.6108652381980153"/>
    <parent link="r_wrist_1"/>
    <child link="r_hand"/>
    <axis xyz="-2.49495760e-07  1.00000000e+00 -2.79424794e-08"/>
    <origin xyz="-0.016300122874272734 0.0069993900510918194 0.0" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="l_shoulder_pitch" type="revolute">
    <dynamics damping="2.0" friction="0.0"/>
    <limit effort="50000.0" velocity="120.0" lower="-1.53588974175501" upper="0.22689280275926285"/>
    <parent link="chest"/>
    <child link="l_shoulder_1"/>
    <axis xyz="0. 1. 0."/>
    <origin xyz="-0.03 0.055555 0.163" rpy="0.25326164431680925 0.0 -0.26179970938509745"/>
  </joint>
  <joint name="l_shoulder_roll" type="revolute">
    <dynamics damping="2.0" friction="0.0"/>
    <limit effort="50000.0" velocity="120.0" lower="0.20943951023931956" upper="2.792526803190927"/>
    <parent link="l_shoulder_1"/>
    <child link="l_shoulder_2"/>
    <axis xyz=" 1.00000000e+00  2.03482562e-07 -4.02302354e-08"/>
    <origin xyz="0.007489008185044645 0.11150041046657978 0.0004027858966975245" rpy="-0.17453306234069357 -0.053747792579130614 0.0"/>
  </joint>
  <joint name="l_arm_ft_sensor" type="fixed">
    <dynamics damping="0.1"/>
    <parent link="l_shoulder_2"/>
    <child link="l_shoulder_3"/>
    <axis xyz="1. 0. 0."/>
    <origin xyz="-0.004350133156978329 3.632573233880443e-07 -0.05430003872406172" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="l_shoulder_yaw" type="revolute">
    <dynamics damping="2.0" friction="0.0"/>
    <limit effort="50000.0" velocity="120.0" lower="-0.8726646259971648" upper="1.3962634015954636"/>
    <parent link="l_shoulder_3"/>
    <child link="l_upper_arm"/>
    <axis xyz=" 2.22764683e-08 -4.39378512e-08 -1.00000000e+00"/>
    <origin xyz="0.0 -4.2345248449371375e-07 -0.02249994137312443" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="l_elbow" type="revolute">
    <dynamics damping="2.0" friction="0.0"/>
    <limit effort="50000.0" velocity="120.0" lower="-0.05235987755982989" upper="1.3089969389957472"/>
    <parent link="l_upper_arm"/>
    <child link="l_elbow_1"/>
    <axis xyz="-8.81182175e-08 -1.00000000e+00  4.40473491e-08"/>
    <origin xyz="0.009999930234919404 -0.001499647423428896 -0.12369999999999999" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="l_wrist_prosup" type="revolute">
    <dynamics damping="2.0" friction="0.0"/>
    <limit effort="50000.0" velocity="120.0" lower="-1.53588974175501" upper="1.53588974175501"/>
    <parent link="l_elbow_1"/>
    <child link="l_forearm"/>
    <axis xyz=" 2.22764683e-08 -4.39378512e-08 -1.00000000e+00"/>
    <origin xyz="-0.014999870654177704 0.0019996435036348514 -0.055300063582479825" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="l_wrist_pitch" type="revolute">
    <dynamics damping="2.0" friction="0.0"/>
    <limit effort="50000.0" velocity="120.0" lower="-1.2217304763960306" upper="0.4363323129985824"/>
    <parent link="l_forearm"/>
    <child link="l_wrist_1"/>
    <axis xyz="-1.00000000e+00 -2.03482562e-07  4.02302354e-08"/>
    <origin xyz="0.016600015934286182 0.002006895218072541 -0.10749999999999998" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="l_wrist_yaw" type="revolute">
    <dynamics damping="2.0" friction="0.0"/>
    <limit effort="50000.0" velocity="120.0" lower="-0.2617993877991494" upper="0.6108652381980153"/>
    <parent link="l_wrist_1"/>
    <child link="l_hand"/>
    <axis xyz=" 8.81182175e-08  1.00000000e+00 -4.40473491e-08"/>
    <origin xyz="-0.016300121744729387 -0.0069993926815711816 0.0" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="neck_fixed_joint" type="fixed">
    <dynamics damping="0.1"/>
    <parent link="chest"/>
    <child link="neck_1"/>
    <axis xyz="1. 0. 0."/>
    <origin xyz="-0.00378849 0.0 0.1683" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="neck_pitch" type="revolute">
    <dynamics damping="2.0" friction="0.0"/>
    <limit effort="50000.0" velocity="120.0" lower="-0.5235987755982988" upper="0.3839724354387525"/>
    <parent link="neck_1"/>
    <child link="neck_2"/>
    <axis xyz=" 0. -1.  0."/>
    <origin xyz="0.00865 0.0 0.049000000000000016" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="neck_roll" type="revolute">
    <dynamics damping="2.0" friction="0.0"/>
    <limit effort="50000.0" velocity="120.0" lower="-0.3490658503988659" upper="0.3490658503988659"/>
    <parent link="neck_2"/>
    <child link="neck_3"/>
    <axis xyz="1. 0. 0."/>
    <origin xyz="0.0 0.0 0.009500499999999995" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="neck_yaw" type="revolute">
    <dynamics damping="2.0" friction="0.0"/>
    <limit effort="50000.0" velocity="120.0" lower="-0.7853981633974483" upper="0.7853981633974483"/>
    <parent link="neck_3"/>
    <child link="head"/>
    <axis xyz="-1.62554992e-21 -1.09999837e-15  1.00000000e+00"/>
    <origin xyz="0.0 0.0 0.06915000000000002" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="camera_tilt_joint" type="revolute">
    <dynamics damping="0.06" friction="0.0"/>
    <limit effort="50000.0" velocity="120.0" lower="-0.7853981633974483" upper="0.7853981633974483"/>
    <parent link="head"/>
    <child link="camera_tilt"/>
    <axis xyz="0. 0. 1."/>
    <origin xyz="0.055513 0.00017 0.076925" rpy="1.5708 3.1415 0.0"/>
  </joint>
  <joint name="lidar_joint" type="revolute">
    <dynamics damping="0.06" friction="0.0"/>
    <limit effort="50000.0" velocity="120.0" lower="-0.7853981633974483" upper="0.7853981633974483"/>
    <parent link="head"/>
    <child link="lidar"/>
    <axis xyz="0. 0. 1."/>
    <origin xyz="0.00117 0.00115 0.1268" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="r_hip_pitch" type="revolute">
    <dynamics damping="2.0" friction="0.0"/>
    <limit effort="50000.0" velocity="120.0" lower="-0.7853981633974483" upper="2.007128639793479"/>
    <parent link="root_link"/>
    <child link="r_hip_1"/>
    <axis xyz=" 0. -1.  0."/>
    <origin xyz="0.0216 -0.040549999999999996 0.0" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="r_hip_roll" type="revolute">
    <dynamics damping="2.0" friction="0.0"/>
    <limit effort="50000.0" velocity="120.0" lower="-0.17453292519943295" upper="2.007128639793479"/>
    <parent link="r_hip_1"/>
    <child link="r_hip_2"/>
    <axis xyz="-1.  0.  0."/>
    <origin xyz="-0.0473 -0.03310000000000001 -0.009000000000000008" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="r_leg_ft_sensor" type="fixed">
    <dynamics damping="0.1"/>
    <parent link="r_hip_2"/>
    <child link="r_hip_3"/>
    <axis xyz="1. 0. 0."/>
    <origin xyz="0.036699999999999997 0.0 -0.08969999999999995" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="r_hip_yaw" type="revolute">
    <dynamics damping="2.0" friction="0.0"/>
    <limit effort="50000.0" velocity="120.0" lower="-1.3962634015954636" upper="1.3962634015954636"/>
    <parent link="r_hip_3"/>
    <child link="r_upper_leg"/>
    <axis xyz=" 0.  0. -1."/>
    <origin xyz="0.0 0.015000000000000006 -0.07089999999999999" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="r_knee" type="revolute">
    <dynamics damping="2.0" friction="0.0"/>
    <limit effort="50000.0" velocity="120.0" lower="-1.2217304763960306" upper="0.08726646259971647"/>
    <parent link="r_upper_leg"/>
    <child link="r_lower_leg"/>
    <axis xyz=" 0. -1.  0."/>
    <origin xyz="0.015000000000000003 -0.0249 -0.10619999999999999" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="r_ankle_pitch" type="revolute">
    <dynamics damping="2.0" friction="0.0"/>
    <limit effort="50000.0" velocity="120.0" lower="-0.7853981633974483" upper="0.7853981633974483"/>
    <parent link="r_lower_leg"/>
    <child link="r_ankle_1"/>
    <axis xyz="0. 1. 0."/>
    <origin xyz="0.0 0.0005000000000000004 -0.255989" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="r_ankle_roll" type="revolute">
    <dynamics damping="2.0" friction="0.0"/>
    <limit effort="50000.0" velocity="120.0" lower="-0.4363323129985824" upper="0.4363323129985824"/>
    <parent link="r_ankle_1"/>
    <child link="r_ankle_2"/>
    <axis xyz="-1.  0.  0."/>
    <origin xyz="-0.052750000000000005 0.018200000000000008 -0.03500000000000003" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="r_foot_front_ft_sensor" type="fixed">
    <dynamics damping="0.1"/>
    <parent link="r_ankle_2"/>
    <child link="r_foot_front"/>
    <axis xyz="1. 0. 0."/>
    <origin xyz="0.13525 0.0 -0.06579999999999997" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="r_foot_rear_ft_sensor" type="fixed">
    <dynamics damping="0.1"/>
    <parent link="r_ankle_2"/>
    <child link="r_foot_rear"/>
    <axis xyz="1. 0. 0."/>
    <origin xyz="0.016 0.0 -0.06579999999999997" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="l_hip_pitch" type="revolute">
    <dynamics damping="2.0" friction="0.0"/>
    <limit effort="50000.0" velocity="120.0" lower="-0.7853981633974483" upper="2.007128639793479"/>
    <parent link="root_link"/>
    <child link="l_hip_1"/>
    <axis xyz=" 0. -1.  0."/>
    <origin xyz="0.0216 0.04045000000000001 0.0" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="l_hip_roll" type="revolute">
    <dynamics damping="2.0" friction="0.0"/>
    <limit effort="50000.0" velocity="120.0" lower="-0.17453292519943295" upper="2.007128639793479"/>
    <parent link="l_hip_1"/>
    <child link="l_hip_2"/>
    <axis xyz="1. 0. 0."/>
    <origin xyz="-0.04725 0.0331 -0.009000000000000008" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="l_leg_ft_sensor" type="fixed">
    <dynamics damping="0.1"/>
    <parent link="l_hip_2"/>
    <child link="l_hip_3"/>
    <axis xyz="1. 0. 0."/>
    <origin xyz="0.036699999999999997 0.0 -0.08969999999999995" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="l_hip_yaw" type="revolute">
    <dynamics damping="2.0" friction="0.0"/>
    <limit effort="50000.0" velocity="120.0" lower="-1.3962634015954636" upper="1.3962634015954636"/>
    <parent link="l_hip_3"/>
    <child link="l_upper_leg"/>
    <axis xyz="0. 0. 1."/>
    <origin xyz="0.0 -0.015000000000000006 -0.07089999999999999" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="l_knee" type="revolute">
    <dynamics damping="2.0" friction="0.0"/>
    <limit effort="50000.0" velocity="120.0" lower="-1.2217304763960306" upper="0.08726646259971647"/>
    <parent link="l_upper_leg"/>
    <child link="l_lower_leg"/>
    <axis xyz=" 0. -1.  0."/>
    <origin xyz="0.015 0.024900000000000012 -0.10619999999999999" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="l_ankle_pitch" type="revolute">
    <dynamics damping="2.0" friction="0.0"/>
    <limit effort="50000.0" velocity="120.0" lower="-0.7853981633974483" upper="0.7853981633974483"/>
    <parent link="l_lower_leg"/>
    <child link="l_ankle_1"/>
    <axis xyz="0. 1. 0."/>
    <origin xyz="0.0 -0.0005000000000000004 -0.255989" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="l_ankle_roll" type="revolute">
    <dynamics damping="2.0" friction="0.0"/>
    <limit effort="50000.0" velocity="120.0" lower="-0.4363323129985824" upper="0.4363323129985824"/>
    <parent link="l_ankle_1"/>
    <child link="l_ankle_2"/>
    <axis xyz="1. 0. 0."/>
    <origin xyz="-0.052750000000000005 -0.018200000000000008 -0.03500000000000003" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="l_foot_rear_ft_sensor" type="fixed">
    <dynamics damping="0.1"/>
    <parent link="l_ankle_2"/>
    <child link="l_foot_rear"/>
    <axis xyz="1. 0. 0."/>
    <origin xyz="0.016 0.0 -0.06579999999999997" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="l_foot_front_ft_sensor" type="fixed">
    <dynamics damping="0.1"/>
    <parent link="l_ankle_2"/>
    <child link="l_foot_front"/>
    <axis xyz="1. 0. 0."/>
    <origin xyz="0.13524999999999998 0.0 -0.06579999999999997" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="l_sole_fixed_joint" type="fixed">
    <dynamics damping="0.1"/>
    <parent link="l_foot_rear"/>
    <child link="l_sole"/>
    <axis xyz="1. 0. 0."/>
    <origin xyz="0.05963 0.0 0.0" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="r_sole_fixed_joint" type="fixed">
    <dynamics damping="0.1"/>
    <parent link="r_foot_rear"/>
    <child link="r_sole"/>
    <axis xyz="1. 0. 0."/>
    <origin xyz="0.05963000000000001 0.0 0.0" rpy="0.0 0.0 0.0"/>
  </joint>
  <joint name="base_link_fixed_joint" type="fixed">
    <parent link="root_link"/>
    <child link="base_link"/>
    <axis xyz="0 0 0"/>
    <origin xyz="0.0 0.0 0.0" rpy="0.0 -0.0 0.0"/>
  </joint>
  <joint name="imu_frame_fixed_joint" type="fixed">
    <dynamics damping="0.1"/>
    <parent link="head"/>
    <child link="imu_frame"/>
    <axis xyz="1. 0. 0."/>
    <origin xyz="0.0034999999997785457 0.0054999999863750895 0.1013340000004441" rpy="-3.141592653589793 0.0 -1.570796326794897"/>
  </joint>
  <material name="green">
    <color rgba="0. 1. 0. 1."/>
  </material>
  <material name="black">
    <color rgba="0. 0. 0. 1."/>
  </material>
  <material name="red">
    <color rgba="1. 0. 0. 1."/>
  </material>
  <material name="blue">
    <color rgba="0. 0. 1. 1."/>
  </material>
  <material name="yellow">
    <color rgba="1. 1. 0. 1."/>
  </material>
  <material name="pink">
    <color rgba="1. 0. 1. 1."/>
  </material>
  <material name="cyan">
    <color rgba="0. 1. 1. 1."/>
  </material>
  <material name="white">
    <color rgba="1. 1. 1. 1."/>
  </material>
  <material name="dblue">
    <color rgba="0.  0.  0.8 1. "/>
  </material>
  <material name="dgreen">
    <color rgba="0.1 0.8 0.1 1. "/>
  </material>
  <material name="gray">
    <color rgba="0.5 0.5 0.5 1. "/>
  </material>
 <gazebo reference="r_arm_ft_sensor">
    <preserveFixedJoint>true</preserveFixedJoint>
    <disableFixedJointLumping>true</disableFixedJointLumping>
  </gazebo>
  <gazebo reference="l_arm_ft_sensor">
    <preserveFixedJoint>true</preserveFixedJoint>
    <disableFixedJointLumping>true</disableFixedJointLumping>
  </gazebo>
  <gazebo>
    <plugin name="controlboard_torso" filename="libgazebo_yarp_controlboard.so">
      <yarpConfigurationFile>model://iCub/conf_stickBot/gazebo_icub_torso/gazebo_icub_torso.ini</yarpConfigurationFile>
    </plugin>
  </gazebo>
  <gazebo reference="torso_pitch">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="torso_roll">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="torso_yaw">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="r_shoulder_pitch">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="r_shoulder_roll">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="r_shoulder_yaw">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="l_shoulder_pitch">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="l_shoulder_roll">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="l_shoulder_yaw">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="l_leg_ft_sensor">
    <preserveFixedJoint>true</preserveFixedJoint>
    <disableFixedJointLumping>true</disableFixedJointLumping>
  </gazebo>
  <gazebo reference="l_foot_front_ft_sensor">
    <preserveFixedJoint>true</preserveFixedJoint>
    <disableFixedJointLumping>true</disableFixedJointLumping>
  </gazebo>
  <gazebo reference="l_foot_rear_ft_sensor">
    <preserveFixedJoint>true</preserveFixedJoint>
    <disableFixedJointLumping>true</disableFixedJointLumping>
  </gazebo>
  <gazebo reference="l_foot_front">
    <collision>
      <surface>
        <bounce>
          <restitution_coefficient>0</restitution_coefficient>
          <threshold>100000</threshold>
        </bounce>
      </surface>
    </collision>
    <maxContacts>4</maxContacts>
    <kp>18000000</kp>
    <kd>100</kd>
    <maxVel>100</maxVel>
    <minDepth>0.0001</minDepth>
    <mu1>1</mu1>
    <mu2>1</mu2>
    <fdir1>0 0 0</fdir1>
  </gazebo>
  <gazebo reference="l_foot_rear">
    <collision>
      <surface>
        <bounce>
          <restitution_coefficient>0</restitution_coefficient>
          <threshold>100000</threshold>
        </bounce>
      </surface>
    </collision>
    <maxContacts>4</maxContacts>
    <kp>18000000</kp>
    <kd>100</kd>
    <maxVel>100</maxVel>
    <minDepth>0.0001</minDepth>
    <mu1>1</mu1>
    <mu2>1</mu2>
    <fdir1>0 0 0</fdir1>
  </gazebo>
  <gazebo>
    <plugin name="controlboard_left_leg" filename="libgazebo_yarp_controlboard.so">
      <yarpConfigurationFile>model://iCub/conf_stickBot/gazebo_icub_left_leg/gazebo_icub_left_leg.ini</yarpConfigurationFile>
    </plugin>
  </gazebo>
  <gazebo reference="l_hip_pitch">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="l_hip_roll">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="l_hip_yaw">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="l_knee">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="l_ankle_pitch">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="l_ankle_roll">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="r_leg_ft_sensor">
    <preserveFixedJoint>true</preserveFixedJoint>
    <disableFixedJointLumping>true</disableFixedJointLumping>
  </gazebo>
  <gazebo reference="r_foot_front_ft_sensor">
    <preserveFixedJoint>true</preserveFixedJoint>
    <disableFixedJointLumping>true</disableFixedJointLumping>
  </gazebo>
  <gazebo reference="r_foot_rear_ft_sensor">
    <preserveFixedJoint>true</preserveFixedJoint>
    <disableFixedJointLumping>true</disableFixedJointLumping>
  </gazebo>
  <gazebo reference="r_foot_front">
    <collision>
      <surface>
        <bounce>
          <restitution_coefficient>0</restitution_coefficient>
          <threshold>100000</threshold>
        </bounce>
      </surface>
    </collision>
    <maxContacts>4</maxContacts>
    <kp>18000000</kp>
    <kd>100</kd>
    <maxVel>100</maxVel>
    <minDepth>0.0001</minDepth>
    <mu1>1</mu1>
    <mu2>1</mu2>
    <fdir1>0 0 0</fdir1>
  </gazebo>
  <gazebo reference="r_foot_rear">
    <collision>
      <surface>
        <bounce>
          <restitution_coefficient>0</restitution_coefficient>
          <threshold>100000</threshold>
        </bounce>
      </surface>
    </collision>
    <maxContacts>4</maxContacts>
    <kp>18000000</kp>
    <kd>100</kd>
    <maxVel>100</maxVel>
    <minDepth>0.0001</minDepth>
    <mu1>1</mu1>
    <mu2>1</mu2>
    <fdir1>0 0 0</fdir1>
  </gazebo>
  <gazebo>
    <plugin name="controlboard_right_leg" filename="libgazebo_yarp_controlboard.so">
      <yarpConfigurationFile>model://iCub/conf_stickBot/gazebo_icub_right_leg/gazebo_icub_right_leg.ini</yarpConfigurationFile>
    </plugin>
  </gazebo>
  <gazebo reference="r_hip_pitch">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="r_hip_roll">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="r_hip_yaw">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="r_knee">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="r_ankle_pitch">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="r_ankle_roll">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo>
    <plugin name="controlboard_head_without_eyes" filename="libgazebo_yarp_controlboard.so">
      <yarpConfigurationFile>model://iCub/conf_stickBot/gazebo_icub_head_without_eyes_V2_7/gazebo_icub_head_without_eyes_V2_7.ini</yarpConfigurationFile>
    </plugin>
  </gazebo>
  <gazebo reference="neck_pitch">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="neck_roll">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="neck_yaw">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo>
    <plugin name="controlboard_left_arm_no_hand" filename="libgazebo_yarp_controlboard.so">
      <yarpConfigurationFile>model://iCub/conf_stickBot/gazebo_icub_left_arm_no_hand_for_no_hand_model/gazebo_icub_left_arm_no_hand_for_no_hand_model.ini</yarpConfigurationFile>
      <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
    </plugin>
  </gazebo>
  <gazebo reference="l_shoulder_pitch">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="l_shoulder_roll">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="l_shoulder_yaw">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="l_elbow">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="l_wrist_prosup">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="l_wrist_pitch">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="l_wrist_yaw">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo>
    <plugin name="controlboard_right_arm_no_hand" filename="libgazebo_yarp_controlboard.so">
      <yarpConfigurationFile>model://iCub/conf_stickBot/gazebo_icub_right_arm_no_hand_for_no_hand_model/gazebo_icub_right_arm_no_hand_for_no_hand_model.ini</yarpConfigurationFile>
      <initialConfiguration>-0.52 0.52 0 0.785 0 0 0.0</initialConfiguration>
    </plugin>
  </gazebo>
  <gazebo reference="r_shoulder_pitch">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="r_shoulder_roll">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="r_shoulder_yaw">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="r_elbow">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="r_wrist_prosup">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="r_wrist_pitch">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo reference="r_wrist_yaw">
    <implicitSpringDamper>1</implicitSpringDamper>
  </gazebo>
  <gazebo 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>sensor</frame>
        <measure_direction>child_to_parent</measure_direction>
      </force_torque>
      <pose>-0.004350133156978329 3.632573233880443e-07 -0.05430003872406172 3.141592646862139 -3.104683320142575e-08 -1.5707962571980942</pose>
      <plugin name="left_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
        <yarpConfigurationString>(yarpDeviceName left_upper_arm_strain) (disableImplicitNetworkWrapper)</yarpConfigurationString>
      </plugin>
    </sensor>
  </gazebo>
  <sensor name="l_arm_ft_sensor" type="force_torque">
    <parent joint="l_arm_ft_sensor"/>
    <force_torque>
      <frame>sensor</frame>
      <measure_direction>child_to_parent</measure_direction>
    </force_torque>
    <origin rpy="3.141592646862139 -3.104683320142575e-08 -1.5707962571980942" xyz="-0.004350133156978329 3.632573233880443e-07 -0.05430003872406172"/>
  </sensor>
  <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>sensor</frame>
        <measure_direction>child_to_parent</measure_direction>
      </force_torque>
      <pose>-0.006849946385389875 2.1904996055921444e-07 -0.05429998157789099 3.141592644717707 4.094299285979593e-08 1.5707964185756365</pose>
      <plugin name="right_arm_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
        <yarpConfigurationString>(yarpDeviceName right_upper_arm_strain) (disableImplicitNetworkWrapper)</yarpConfigurationString>
      </plugin>
    </sensor>
  </gazebo>
  <sensor name="r_arm_ft_sensor" type="force_torque">
    <parent joint="r_arm_ft_sensor"/>
    <force_torque>
      <frame>sensor</frame>
      <measure_direction>child_to_parent</measure_direction>
    </force_torque>
    <origin rpy="3.141592644717707 4.094299285979593e-08 1.5707964185756365" xyz="-0.006849946385389875 2.1904996055921444e-07 -0.05429998157789099"/>
  </sensor>
  <gazebo reference="l_foot_front_ft_sensor">
    <sensor name="l_foot_front_ft_sensor" type="force_torque">
      <always_on>1</always_on>
      <update_rate>100</update_rate>
      <force_torque>
        <frame>sensor</frame>
        <measure_direction>parent_to_child</measure_direction>
      </force_torque>
      <pose>0.0 0.0 0.022299999999999986 0.0 -0.0 -2.094395210586932</pose>
      <plugin name="left_foot_front_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
        <yarpConfigurationString>(yarpDeviceName left_lower_leg_front_strain) (disableImplicitNetworkWrapper)</yarpConfigurationString>
      </plugin>
    </sensor>
  </gazebo>
  <sensor name="l_foot_front_ft_sensor" type="force_torque">
    <parent joint="l_foot_front_ft_sensor"/>
    <force_torque>
      <frame>sensor</frame>
      <measure_direction>parent_to_child</measure_direction>
    </force_torque>
    <origin rpy="0.0 -0.0 -2.094395210586932" xyz="0.0 0.0 0.022299999999999986"/>
  </sensor>
  <gazebo reference="l_foot_rear_ft_sensor">
    <sensor name="l_foot_rear_ft_sensor" type="force_torque">
      <always_on>1</always_on>
      <update_rate>100</update_rate>
      <force_torque>
        <frame>sensor</frame>
        <measure_direction>parent_to_child</measure_direction>
      </force_torque>
      <pose>0.0 0.0 0.022299999999999986 0.0 -0.0 -2.094395210586932</pose>
      <plugin name="left_foot_rear_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
        <yarpConfigurationString>(yarpDeviceName left_lower_leg_rear_strain) (disableImplicitNetworkWrapper)</yarpConfigurationString>
      </plugin>
    </sensor>
  </gazebo>
  <sensor name="l_foot_rear_ft_sensor" type="force_torque">
    <parent joint="l_foot_rear_ft_sensor"/>
    <force_torque>
      <frame>sensor</frame>
      <measure_direction>parent_to_child</measure_direction>
    </force_torque>
    <origin rpy="0.0 -0.0 -2.094395210586932" xyz="0.0 0.0 0.022299999999999986"/>
  </sensor>
  <gazebo reference="r_foot_front_ft_sensor">
    <sensor name="r_foot_front_ft_sensor" type="force_torque">
      <always_on>1</always_on>
      <update_rate>100</update_rate>
      <force_torque>
        <frame>sensor</frame>
        <measure_direction>parent_to_child</measure_direction>
      </force_torque>
      <pose>0.0 0.0 0.022299999999999986 0.0 -0.0 -2.094395210586932</pose>
      <plugin name="right_foot_front_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
        <yarpConfigurationString>(yarpDeviceName right_lower_leg_front_strain) (disableImplicitNetworkWrapper)</yarpConfigurationString>
      </plugin>
    </sensor>
  </gazebo>
  <sensor name="r_foot_front_ft_sensor" type="force_torque">
    <parent joint="r_foot_front_ft_sensor"/>
    <force_torque>
      <frame>sensor</frame>
      <measure_direction>parent_to_child</measure_direction>
    </force_torque>
    <origin rpy="0.0 -0.0 -2.094395210586932" xyz="0.0 0.0 0.022299999999999986"/>
  </sensor>
  <gazebo reference="r_foot_rear_ft_sensor">
    <sensor name="r_foot_rear_ft_sensor" type="force_torque">
      <always_on>1</always_on>
      <update_rate>100</update_rate>
      <force_torque>
        <frame>sensor</frame>
        <measure_direction>parent_to_child</measure_direction>
      </force_torque>
      <pose>0.0 0.0 0.022299999999999986 0.0 -0.0 -2.094395210586932</pose>
      <plugin name="right_foot_rear_ft_plugin" filename="libgazebo_yarp_forcetorque.so">
        <yarpConfigurationString>(yarpDeviceName right_lower_leg_rear_strain) (disableImplicitNetworkWrapper)</yarpConfigurationString>
      </plugin>
    </sensor>
  </gazebo>
  <sensor name="r_foot_rear_ft_sensor" type="force_torque">
    <parent joint="r_foot_rear_ft_sensor"/>
    <force_torque>
      <frame>sensor</frame>
      <measure_direction>parent_to_child</measure_direction>
    </force_torque>
    <origin rpy="0.0 -0.0 -2.094395210586932" xyz="0.0 0.0 0.022299999999999986"/>
  </sensor>
  <gazebo reference="head">
    <sensor name="head_imu_acc_1x1" type="imu">
      <always_on>1</always_on>
      <update_rate>100</update_rate>
      <pose>0.0034999999997785457 0.0054999999863750895 0.1013340000004441 -3.141592653589793 -4.20000162559822e-15 -1.570796326794897</pose>
      <plugin name="iCub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
        <yarpConfigurationString>(yarpDeviceName head-inertial) (disableImplicitNetworkWrapper)</yarpConfigurationString>
      </plugin>
    </sensor>
  </gazebo>
  <sensor name="head_imu_acc_1x1" type="accelerometer">
    <parent link="head"/>
    <origin rpy="-3.141592653589793 -4.20000162559822e-15 -1.570796326794897" xyz="0.0034999999997785457 0.0054999999863750895 0.1013340000004441"/>
  </sensor>

  <gazebo reference="camera_tilt">
    <sensor name="realsense_head_rgbd" type="depth">
      <always_on>1</always_on>
      <update_rate>30</update_rate>
      <pose>-0.0113 0.0 0.0 0.0 -1.57079 0.0</pose>
      <camera name="intel_realsense_rgbd_camera">
        <pose>0 0 0 -1.57079 -1.57079 3.14159</pose>
        <horizontal_fov>2.1</horizontal_fov>
        <distortion>
          <k1>0</k1>
          <k2>0</k2>
          <k3>0</k3>
          <p1>0</p1>
          <p2>0</p2>
          <center>319.5 239.5</center>
        </distortion>
        <image>
          <width>640</width>
          <height>480</height>
          <format>R8G8B8</format>
        </image>
        <clip>
          <near>0.02</near>
          <far>3000</far>
        </clip>
      </camera>
      <visualize>false</visualize>
      <plugin name="depth_camera_plugin" filename="libgazebo_yarp_depthCamera.so">
        <yarpConfigurationFile>model://iCub/conf_stickBot/gazebo_icub_rgbd_camera_eyes/gazebo_icub_rgbd_camera_eyes.ini</yarpConfigurationFile>
      </plugin>
    </sensor>
  </gazebo>
  <sensor name="realsense_head_rgbd" type="camera">
    <parent link="camera_tilt"/>
    <origin rpy="0.0 -1.57079 0.0" xyz="-0.0113 0.0 0.0"/>
  </sensor>

  <gazebo reference="lidar">
    <sensor name="lasersensor_head" type="ray">
        <always_on>1</always_on>
        <update_rate>40</update_rate>
        <pose frame=''>0.0 0.0 0.0105 0.0 0.0 0.0</pose>
        <visualize>true</visualize>
        <ray>
          <scan>
            <horizontal>
              <samples>360</samples>
              <resolution>2</resolution>
              <min_angle>0</min_angle>
              <max_angle>6.28</max_angle>
            </horizontal>
          </scan>
          <range>
            <min>0.04</min>
            <max>5</max>
            <resolution>0.01</resolution>
          </range>
          <noise>
            <type>gaussian</type>
            <mean>0</mean>
            <stddev>0.005</stddev>
          </noise>
        </ray>
      <plugin name="lasersensor" filename="libgazebo_yarp_lasersensor.so">
        <yarpConfigurationFile>model://iCub/conf_stickBot/gazebo_icub_laser/gazebo_icub_laser.ini</yarpConfigurationFile>
      </plugin>
    </sensor>
  </gazebo>
  <sensor name="lasersensor_head" type="ray">
    <parent link="lidar"/>
    <origin rpy="0.0 0.0 0.0" xyz="0.0 0.0 0.0105"/>
  </sensor>

  <gazebo reference="chest">
    <sensor name="chest_imu_acc_1x1" type="imu">
      <always_on>1</always_on>
      <update_rate>100</update_rate>
      <pose>0.0894115 0.0175 0.09929950000000001 -1.5707963267948968 -0.0 -1.5707963267948968</pose>
      <plugin name="iCub_yarp_gazebo_plugin_IMU" filename="libgazebo_yarp_imu.so">
        <yarpConfigurationString>(yarpDeviceName chest-inertial) (disableImplicitNetworkWrapper)</yarpConfigurationString>
      </plugin>
    </sensor>
  </gazebo>
  <sensor name="chest_imu_acc_1x1" type="accelerometer">
    <parent link="chest"/>
    <origin rpy="-1.5707963267948968 -0.0 -1.5707963267948968" xyz="0.0894115 0.0175 0.09929950000000001"/>
  </sensor>
  <gazebo>
      <plugin name="robotinterface" filename="libgazebo_yarp_robotinterface.so">
      <yarpRobotInterfaceConfigurationFile>model://iCub/conf_stickBot/icub.xml</yarpRobotInterfaceConfigurationFile>
    </plugin>
  </gazebo>
  <gazebo>
    <pose>0.0 0.0 0.64 0.0 0.0 0.0</pose>
  </gazebo>
</robot>
