<robot name="iCub">
    <link name="base_link" />
    <link name="chest">
        <inertial>
            <mass value="4.81" />
            <origin xyz="0.00222898 0.075 -1.18362e-17" rpy="0 -0 0" />
            <inertia ixx="0.07472" ixy="-3.6e-06" ixz="-4.705e-05" iyy="0.08145" iyz="0.004567" izz="0.01306" />
        </inertial>
        <visual>
            <origin xyz="-0.0055 -6.93889e-18 -3.55271e-18" rpy="2.03442 -1.5708 -0.463617" />
            <geometry>
                <mesh filename="meshes/visual/icub_chest.dae" scale="1 1 1" />
            </geometry>
            <material name="material.metal">
                <texture />
                <color rgba="0 0 0 1" />
            </material>
        </visual>
        <collision>
            <origin xyz="-0.0055 -6.93889e-18 -3.55271e-18" rpy="2.03442 -1.5708 -0.463617" />
            <geometry>
                <mesh filename="meshes/collision/icub_simple_collision_chest.dae" scale="1 1 1" />
            </geometry>
        </collision>
    </link>
    <link name="chest_skin_frame">
        <inertial>
            <mass value="0" />
            <origin xyz="0 0 0" rpy="0 -0 0" />
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
        </inertial>
    </link>
    <link name="codyco_balancing_world">
        <inertial>
            <mass value="0" />
            <origin xyz="0 0 0" rpy="0 -0 0" />
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
        </inertial>
    </link>
    <link name="head">
        <inertial>
            <mass value="3" />
            <origin xyz="0.0185 -0.1108 6.78454e-18" rpy="0 -0 0" />
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
        </inertial>
        <visual>
            <origin xyz="0.0045 -0.0235 9.42472e-13" rpy="1.81577 1.5708 0.244969" />
            <geometry>
                <mesh filename="meshes/visual/icub_head.dae" scale="1 1 1" />
            </geometry>
            <material name="material.metal">
                <texture />
                <color rgba="0 0 0 1" />
            </material>
        </visual>
        <collision>
            <origin xyz="0.0045 -0.0235 9.42472e-13" rpy="1.81577 1.5708 0.244969" />
            <geometry>
                <mesh filename="meshes/collision/icub_simple_collision_head.dae" scale="1 1 1" />
            </geometry>
        </collision>
    </link>
    <link name="imu_frame">
        <inertial>
            <mass value="0" />
            <origin xyz="0 0 0" rpy="0 -0 0" />
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
        </inertial>
    </link>
    <link name="l_ankle_1">
        <inertial>
            <mass value="0.643" />
            <origin xyz="2.65892e-05 -0.00082253 0.0102574" rpy="0 -0 0" />
            <inertia ixx="4.76593" ixy="9.7426e-06" ixz="2.68085e-05" iyy="0.0040935" iyz="-0.000353796" izz="0.0036688" />
        </inertial>
        <visual>
            <origin xyz="-0.001555 -5.54343e-12 0.0009175" rpy="1.5708 1.53106e-11 1.53105e-11" />
            <geometry>
                <mesh filename="meshes/visual/icub_l_ankle_1.dae" scale="1 1 1" />
            </geometry>
            <material name="material.metal">
                <texture />
                <color rgba="0 0 0 1" />
            </material>
        </visual>
        <collision>
            <origin xyz="-0.001555 -5.54343e-12 0.0009175" rpy="1.5708 1.53106e-11 1.53105e-11" />
            <geometry>
                <mesh filename="meshes/collision/icub_simple_collision_l_ankle_1.dae" scale="1 1 1" />
            </geometry>
        </collision>
    </link>
    <link name="l_ankle_2">
        <inertial>
            <mass value="0.2675" />
            <origin xyz="-0.0035 0 0.0645" rpy="0 -0 0" />
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
        </inertial>
    </link>
    <link name="l_elbow_1">
        <inertial>
            <mass value="0.074" />
            <origin xyz="-0.0013 0.00371 -0.00105" rpy="0 -0 0" />
            <inertia ixx="2.8389e-05" ixy="-5.15e-07" ixz="-4.08e-07" iyy="9.244e-06" iyz="-3.71e-07" izz="2.9968e-05" />
        </inertial>
    </link>
    <link name="l_foot">
        <inertial>
            <mass value="0.5935" />
            <origin xyz="0.024069 -0.000613931 -0.0284154" rpy="0 -0 0" />
            <inertia ixx="0.00167275" ixy="-4.51893e-06" ixz="0.000567785" iyy="0.00320869" iyz="-6.62361e-07" izz="0.00208378" />
        </inertial>
        <visual>
            <origin xyz="0.0044175 -5.54341e-12 -0.062945" rpy="2.35619 1.5708 2.35619" />
            <geometry>
                <mesh filename="meshes/visual/icub_l_foot.dae" scale="1 1 1" />
            </geometry>
            <material name="material.metal">
                <texture />
                <color rgba="0 0 0 1" />
            </material>
        </visual>
        <collision>
            <origin xyz="0.0044175 -5.54341e-12 -0.062945" rpy="2.35619 1.5708 2.35619" />
            <geometry>
                <mesh filename="meshes/collision/icub_simple_collision_l_foot.dae" scale="1 1 1" />
            </geometry>
        </collision>
    </link>
    <link name="l_foot_dh_frame">
        <inertial>
            <mass value="0" />
            <origin xyz="0 0 0" rpy="0 -0 0" />
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
        </inertial>
    </link>
    <link name="l_forearm">
        <inertial>
            <mass value="0.525" />
            <origin xyz="0.000347 0.066 0.00476" rpy="0 -0 0" />
            <inertia ixx="0.000765393" ixy="4.337e-06" ixz="2.39e-07" iyy="0.000164578" iyz="1.9381e-05" izz="0.00069806" />
        </inertial>
        <visual>
            <origin xyz="-0.000143922 -1.25974e-12 0.0204334" rpy="0.477647 1.5708 2.04844" />
            <geometry>
                <mesh filename="meshes/visual/icub_l_forearm.dae" scale="1 1 1" />
            </geometry>
            <material name="material.metal">
                <texture />
                <color rgba="0 0 0 1" />
            </material>
        </visual>
        <collision>
            <origin xyz="-0.000143922 -1.25974e-12 0.0204334" rpy="0.477647 1.5708 2.04844" />
            <geometry>
                <mesh filename="meshes/collision/icub_simple_collision_l_forearm.dae" scale="1 1 1" />
            </geometry>
        </collision>
    </link>
    <link name="l_forearm_dh_frame">
        <inertial>
            <mass value="0" />
            <origin xyz="0 0 0" rpy="0 -0 0" />
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
        </inertial>
    </link>
    <link name="l_gripper">
        <inertial>
            <mass value="0" />
            <origin xyz="0 0 0" rpy="0 -0 0" />
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
        </inertial>
    </link>
    <link name="l_hand">
        <inertial>
            <mass value="0.213" />
            <origin xyz="0.07023 -0.00805 -0.007" rpy="0 -0 0" />
            <inertia ixx="0.000157143" ixy="1.278e-05" ixz="4.823e-06" iyy="0.000247995" iyz="-1.8188e-05" izz="0.000380535" />
        </inertial>
        <visual>
            <origin xyz="-1.25973e-12 0.0204334 -0.000143922" rpy="5.10271e-12 -2.64186e-12 1.49622e-11" />
            <geometry>
                <mesh filename="meshes/visual/icub_l_hand.dae" scale="1 1 1" />
            </geometry>
            <material name="material.metal">
                <texture />
                <color rgba="0 0 0 1" />
            </material>
        </visual>
        <collision>
            <origin xyz="-1.25973e-12 0.0204334 -0.000143922" rpy="5.10271e-12 -2.64186e-12 1.49622e-11" />
            <geometry>
                <mesh filename="meshes/collision/icub_simple_collision_l_hand.dae" scale="1 1 1" />
            </geometry>
        </collision>
    </link>
    <link name="l_hand_dh_frame">
        <inertial>
            <mass value="0" />
            <origin xyz="0 0 0" rpy="0 -0 0" />
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
        </inertial>
    </link>
    <link name="l_hip_1">
        <inertial>
            <mass value="0.695" />
            <origin xyz="8.0431e-05 -0.00691643 -0.0023047" rpy="0 -0 0" />
            <inertia ixx="0.00447053" ixy="-2.2209e-05" ixz="1.39712e-05" iyy="0.00325085" iyz="4.77629e-05" izz="0.00490487" />
        </inertial>
        <visual>
            <origin xyz="0 0 0" rpy="2.35619 1.5708 0.785398" />
            <geometry>
                <mesh filename="meshes/visual/icub_l_hip_1.dae" scale="1 1 1" />
            </geometry>
            <material name="material.metal">
                <texture />
                <color rgba="0 0 0 1" />
            </material>
        </visual>
        <collision>
            <origin xyz="0 0 0" rpy="2.35619 1.5708 0.785398" />
            <geometry>
                <mesh filename="meshes/collision/icub_simple_collision_l_hip_1.dae" scale="1 1 1" />
            </geometry>
        </collision>
    </link>
    <link name="l_hip_2">
        <inertial>
            <mass value="0.982" />
            <origin xyz="0.0010964 -3.13229e-05 0.0598624" rpy="0 -0 0" />
            <inertia ixx="0.0253646" ixy="1.2116e-06" ixz="0.000491081" iyy="0.0272714" iyz="1.15713e-05" izz="0.0041739" />
        </inertial>
        <visual>
            <origin xyz="0 0 0" rpy="-2.35623 -1.5708 -0.785361" />
            <geometry>
                <mesh filename="meshes/visual/icub_l_hip_2.dae" scale="1 1 1" />
            </geometry>
            <material name="material.metal">
                <texture />
                <color rgba="0 0 0 1" />
            </material>
        </visual>
        <collision>
            <origin xyz="0 0 0" rpy="-2.35623 -1.5708 -0.785361" />
            <geometry>
                <mesh filename="meshes/collision/icub_simple_collision_l_hip_2.dae" scale="1 1 1" />
            </geometry>
        </collision>
    </link>
    <link name="l_hip_3">
        <inertial>
            <mass value="0" />
            <origin xyz="0 0 0" rpy="0 -0 0" />
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
        </inertial>
    </link>
    <link name="l_lower_leg">
        <inertial>
            <mass value="2.032" />
            <origin xyz="-0.0761044 -4.50091e-05 0.0062408" rpy="0 -0 0" />
            <inertia ixx="0.0154559" ixy="-0.0013903" ixz="-0.0039454" iyy="0.070277" iyz="0.00071995" izz="0.0659645" />
        </inertial>
        <visual>
            <origin xyz="0.010945 0.0009175 2.2823e-12" rpy="-3.14159 -2.33487e-16 1.5708" />
            <geometry>
                <mesh filename="meshes/visual/icub_l_shank.dae" scale="1 1 1" />
            </geometry>
            <material name="material.metal">
                <texture />
                <color rgba="0 0 0 1" />
            </material>
        </visual>
        <collision>
            <origin xyz="0.010945 0.0009175 2.2823e-12" rpy="-3.14159 -2.33487e-16 1.5708" />
            <geometry>
                <mesh filename="meshes/collision/icub_simple_collision_l_shank.dae" scale="1 1 1" />
            </geometry>
        </collision>
    </link>
    <link name="l_shoulder_1">
        <inertial>
            <mass value="0.189" />
            <origin xyz="-5e-06 0.0187 -0.00119" rpy="0 -0 0" />
            <inertia ixx="5.4421e-05" ixy="9e-09" ixz="0" iyy="9.331e-06" iyz="-1.7e-08" izz="5.4862e-05" />
        </inertial>
        <visual>
            <origin xyz="3.08642e-14 0.109285 -0.00521101" rpy="2.84053 1.5708 1.26973" />
            <geometry>
                <mesh filename="meshes/visual/icub_l_shoulder_1.dae" scale="1 1 1" />
            </geometry>
            <material name="material.metal">
                <texture />
                <color rgba="0 0 0 1" />
            </material>
        </visual>
        <collision>
            <origin xyz="3.08642e-14 0.109285 -0.00521101" rpy="2.84053 1.5708 1.26973" />
            <geometry>
                <mesh filename="meshes/collision/icub_simple_collision_l_shoulder_1.dae" scale="1 1 1" />
            </geometry>
        </collision>
    </link>
    <link name="l_shoulder_2">
        <inertial>
            <mass value="0.179" />
            <origin xyz="9.4e-05 -0.00627 0.0166" rpy="0 -0 0" />
            <inertia ixx="0.0001372" ixy="4.66e-07" ixz="3.65e-07" iyy="8.2927e-05" iyz="-2.0524e-05" izz="9.9274e-05" />
        </inertial>
        <visual>
            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="1.00126 1.5708 2.57206" />
            <geometry>
                <mesh filename="meshes/visual/icub_l_shoulder_2.dae" scale="1 1 1" />
            </geometry>
            <material name="material.metal">
                <texture />
                <color rgba="0 0 0 1" />
            </material>
        </visual>
        <collision>
            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="1.00126 1.5708 2.57206" />
            <geometry>
                <mesh filename="meshes/collision/icub_simple_collision_l_shoulder_2.dae" scale="1 1 1" />
            </geometry>
        </collision>
    </link>
    <link name="l_shoulder_3">
        <inertial>
            <mass value="0.156157" />
            <origin xyz="0.00559398 9.77357e-05 0.0501346" rpy="0 -0 0" />
            <inertia ixx="4.84118e-05" ixy="-5.662e-08" ixz="-4.3618e-06" iyy="5.79067e-05" iyz="-7.42844e-07" izz="6.7103e-05" />
        </inertial>
    </link>
    <link name="l_sole">
        <inertial>
            <mass value="0" />
            <origin xyz="0 0 0" rpy="0 -0 0" />
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
        </inertial>
    </link>
    <link name="l_upper_arm">
        <inertial>
            <mass value="0.727843" />
            <origin xyz="-0.000156 -9.87e-05 0.0298" rpy="0 -0 0" />
            <inertia ixx="0.000408" ixy="-1.08e-06" ixz="-2.29e-06" iyy="0.00038" iyz="3.57e-06" izz="0.00026" />
        </inertial>
        <visual>
            <origin xyz="-0.0204334 0.000143922 -0.068" rpy="1.15274e-11 4.92958e-12 -1.309" />
            <geometry>
                <mesh filename="meshes/visual/icub_l_arm.dae" scale="1 1 1" />
            </geometry>
            <material name="material.metal">
                <texture />
                <color rgba="0 0 0 1" />
            </material>
        </visual>
        <collision>
            <origin xyz="-0.0204334 0.000143922 -0.068" rpy="1.15274e-11 4.92958e-12 -1.309" />
            <geometry>
                <mesh filename="meshes/collision/icub_simple_collision_l_arm.dae" scale="1 1 1" />
            </geometry>
        </collision>
    </link>
    <link name="l_upper_arm_dh_frame">
        <inertial>
            <mass value="0" />
            <origin xyz="0 0 0" rpy="0 -0 0" />
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
        </inertial>
    </link>
    <link name="l_upper_leg">
        <inertial>
            <mass value="1.522" />
            <origin xyz="0.0021968 -0.174502 0.0013437" rpy="0 -0 0" />
            <inertia ixx="0.0454917" ixy="-0.000940971" ixz="5.37405e-05" iyy="0.01004" iyz="0.001203" izz="0.0458255" />
        </inertial>
        <visual>
            <origin xyz="0 0 0" rpy="0.463626 1.5708 2.03443" />
            <geometry>
                <mesh filename="meshes/visual/icub_l_thigh.dae" scale="1 1 1" />
            </geometry>
            <material name="material.metal">
                <texture />
                <color rgba="0 0 0 1" />
            </material>
        </visual>
        <collision>
            <origin xyz="0 0 0" rpy="0.463626 1.5708 2.03443" />
            <geometry>
                <mesh filename="meshes/collision/icub_simple_collision_l_thigh.dae" scale="1 1 1" />
            </geometry>
        </collision>
    </link>
    <link name="l_wrist_1">
        <inertial>
            <mass value="0" />
            <origin xyz="0 0 0" rpy="0 -0 0" />
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
        </inertial>
    </link>
    <link name="neck_1">
        <inertial>
            <mass value="0" />
            <origin xyz="0 0 0" rpy="0 -0 0" />
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
        </inertial>
        <visual>
            <origin xyz="-5.55112e-17 9.62889e-13 -0.0055" rpy="-7.25153e-06 -1.5708 -1.57079" />
            <geometry>
                <mesh filename="meshes/visual/icub_neck_1.dae" scale="1 1 1" />
            </geometry>
            <material name="material.metal">
                <texture />
                <color rgba="0 0 0 1" />
            </material>
        </visual>
        <collision>
            <origin xyz="-5.55112e-17 9.62889e-13 -0.0055" rpy="-7.25153e-06 -1.5708 -1.57079" />
            <geometry>
                <mesh filename="meshes/collision/icub_simple_collision_neck_1.dae" scale="1 1 1" />
            </geometry>
        </collision>
    </link>
    <link name="neck_2">
        <inertial>
            <mass value="0" />
            <origin xyz="0 0 0" rpy="0 -0 0" />
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
        </inertial>
        <visual>
            <origin xyz="-9.62885e-13 0.0055 0.0235" rpy="2.17538e-05 -1.5708 1.57077" />
            <geometry>
                <mesh filename="meshes/visual/icub_neck_2.dae" scale="1 1 1" />
            </geometry>
            <material name="material.metal">
                <texture />
                <color rgba="0 0 0 1" />
            </material>
        </visual>
        <collision>
            <origin xyz="-9.62885e-13 0.0055 0.0235" rpy="2.17538e-05 -1.5708 1.57077" />
            <geometry>
                <mesh filename="meshes/collision/icub_simple_collision_neck_2.dae" scale="1 1 1" />
            </geometry>
        </collision>
    </link>
    <link name="r_ankle_1">
        <inertial>
            <mass value="0.643" />
            <origin xyz="-2.65892e-05 -0.00082253 -0.0102574" rpy="0 -0 0" />
            <inertia ixx="4.76593" ixy="9.7426e-06" ixz="-2.68085e-05" iyy="0.0040935" iyz="0.000353796" izz="0.0036688" />
        </inertial>
        <visual>
            <origin xyz="-0.001555 -1.08717e-12 -0.0009175" rpy="-1.5708 -1.53106e-11 5.10376e-12" />
            <geometry>
                <mesh filename="meshes/visual/icub_r_ankle_1.dae" scale="1 1 1" />
            </geometry>
            <material name="material.metal">
                <texture />
                <color rgba="0 0 0 1" />
            </material>
        </visual>
        <collision>
            <origin xyz="-0.001555 -1.08717e-12 -0.0009175" rpy="-1.5708 -1.53106e-11 5.10376e-12" />
            <geometry>
                <mesh filename="meshes/collision/icub_simple_collision_r_ankle_1.dae" scale="1 1 1" />
            </geometry>
        </collision>
    </link>
    <link name="r_ankle_2">
        <inertial>
            <mass value="0.2675" />
            <origin xyz="-0.0035 4.28626e-19 0.0645" rpy="0 -0 0" />
            <inertia ixx="0" ixy="4.70198e-38" ixz="0" iyy="0" iyz="0" izz="0" />
        </inertial>
    </link>
    <link name="r_elbow_1">
        <inertial>
            <mass value="0.074" />
            <origin xyz="0.0013 -0.00371 0.00105" rpy="0 -0 0" />
            <inertia ixx="2.84e-05" ixy="-5.02e-07" ixz="-3.99e-07" iyy="9.24e-06" iyz="-3.71e-07" izz="2.99e-05" />
        </inertial>
    </link>
    <link name="r_foot">
        <inertial>
            <mass value="0.5935" />
            <origin xyz="0.024069 0.000613931 -0.0284154" rpy="0 -0 0" />
            <inertia ixx="0.00167275" ixy="4.51893e-06" ixz="0.000567784" iyy="0.00320869" iyz="6.62535e-07" izz="0.00208378" />
        </inertial>
        <visual>
            <origin xyz="0.0044175 1.08716e-12 -0.062945" rpy="0.321762 1.5708 -2.81983" />
            <geometry>
                <mesh filename="meshes/visual/icub_r_foot.dae" scale="1 1 1" />
            </geometry>
            <material name="material.metal">
                <texture />
                <color rgba="0 0 0 1" />
            </material>
        </visual>
        <collision>
            <origin xyz="0.0044175 1.08716e-12 -0.062945" rpy="0.321762 1.5708 -2.81983" />
            <geometry>
                <mesh filename="meshes/collision/icub_simple_collision_r_foot.dae" scale="1 1 1" />
            </geometry>
        </collision>
    </link>
    <link name="r_foot_dh_frame">
        <inertial>
            <mass value="0" />
            <origin xyz="0 0 0" rpy="0 -0 0" />
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
        </inertial>
    </link>
    <link name="r_forearm">
        <inertial>
            <mass value="0.525" />
            <origin xyz="-0.000347 -0.066 -0.00476" rpy="0 -0 0" />
            <inertia ixx="0.000766" ixy="5.66e-06" ixz="1.4e-06" iyy="0.000164" iyz="1.82e-05" izz="0.000699" />
        </inertial>
        <visual>
            <origin xyz="0.000143922 -1.07214e-12 -0.0204334" rpy="1.39994 1.5708 2.97073" />
            <geometry>
                <mesh filename="meshes/visual/icub_r_forearm.dae" scale="1 1 1" />
            </geometry>
            <material name="material.metal">
                <texture />
                <color rgba="0 0 0 1" />
            </material>
        </visual>
        <collision>
            <origin xyz="0.000143922 -1.07214e-12 -0.0204334" rpy="1.39994 1.5708 2.97073" />
            <geometry>
                <mesh filename="meshes/collision/icub_simple_collision_r_forearm.dae" scale="1 1 1" />
            </geometry>
        </collision>
    </link>
    <link name="r_forearm_dh_frame">
        <inertial>
            <mass value="0" />
            <origin xyz="0 0 0" rpy="0 -0 0" />
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
        </inertial>
    </link>
    <link name="r_gripper">
        <inertial>
            <mass value="0" />
            <origin xyz="0 0 0" rpy="0 -0 0" />
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
        </inertial>
    </link>
    <link name="r_hand">
        <inertial>
            <mass value="0.213" />
            <origin xyz="0.07023 -0.00805 0.007" rpy="0 -0 0" />
            <inertia ixx="0.000154" ixy="1.26e-05" ixz="-6.08e-06" iyy="0.00025" iyz="1.76e-05" izz="0.000378" />
        </inertial>
        <visual>
            <origin xyz="1.0721e-12 0.0204334 0.000143922" rpy="1.53098e-11 2.00656e-11 -3.14159" />
            <geometry>
                <mesh filename="meshes/visual/icub_r_hand.dae" scale="1 1 1" />
            </geometry>
            <material name="material.metal">
                <texture />
                <color rgba="0 0 0 1" />
            </material>
        </visual>
        <collision>
            <origin xyz="1.0721e-12 0.0204334 0.000143922" rpy="1.53098e-11 2.00656e-11 -3.14159" />
            <geometry>
                <mesh filename="meshes/collision/icub_simple_collision_r_hand.dae" scale="1 1 1" />
            </geometry>
        </collision>
    </link>
    <link name="r_hand_dh_frame">
        <inertial>
            <mass value="0" />
            <origin xyz="0 0 0" rpy="0 -0 0" />
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
        </inertial>
    </link>
    <link name="r_hip_1">
        <inertial>
            <mass value="0.695" />
            <origin xyz="8.0431e-05 -0.00691643 0.0023047" rpy="0 -0 0" />
            <inertia ixx="0.00447053" ixy="-2.2209e-05" ixz="-1.39712e-05" iyy="0.00325085" iyz="-4.77629e-05" izz="0.00490487" />
        </inertial>
        <visual>
            <origin xyz="0 0 0" rpy="-1.3734 -1.5708 -0.380506" />
            <geometry>
                <mesh filename="meshes/visual/icub_r_hip_1.dae" scale="1 1 1" />
            </geometry>
            <material name="material.metal">
                <texture />
                <color rgba="0 0 0 1" />
            </material>
        </visual>
        <collision>
            <origin xyz="0 0 0" rpy="-1.3734 -1.5708 -0.380506" />
            <geometry>
                <mesh filename="meshes/collision/icub_simple_collision_r_hip_1.dae" scale="1 1 1" />
            </geometry>
        </collision>
    </link>
    <link name="r_hip_2">
        <inertial>
            <mass value="0.982" />
            <origin xyz="0.0010964 -3.13229e-05 0.0598624" rpy="0 -0 0" />
            <inertia ixx="0.0253646" ixy="1.2116e-06" ixz="0.000491081" iyy="0.0272714" iyz="1.15713e-05" izz="0.0041739" />
        </inertial>
        <visual>
            <origin xyz="0 0 0" rpy="0.785327 -1.5708 -0.785327" />
            <geometry>
                <mesh filename="meshes/visual/icub_r_hip_2.dae" scale="1 1 1" />
            </geometry>
            <material name="material.metal">
                <texture />
                <color rgba="0 0 0 1" />
            </material>
        </visual>
        <collision>
            <origin xyz="0 0 0" rpy="0.785327 -1.5708 -0.785327" />
            <geometry>
                <mesh filename="meshes/collision/icub_simple_collision_r_hip_2.dae" scale="1 1 1" />
            </geometry>
        </collision>
    </link>
    <link name="r_hip_3">
        <inertial>
            <mass value="0" />
            <origin xyz="0 0 0" rpy="0 -0 0" />
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
        </inertial>
    </link>
    <link name="r_lower_leg">
        <inertial>
            <mass value="2.032" />
            <origin xyz="-0.0761044 -4.50091e-05 -0.0062408" rpy="0 -0 0" />
            <inertia ixx="0.0154559" ixy="-0.0013903" ixz="0.0039454" iyy="0.070277" iyz="-0.00071995" izz="0.0659645" />
        </inertial>
        <visual>
            <origin xyz="0.010945 0.0009175 -8.32667e-17" rpy="3.14159 -1.14424e-17 1.5708" />
            <geometry>
                <mesh filename="meshes/visual/icub_r_shank.dae" scale="1 1 1" />
            </geometry>
            <material name="material.metal">
                <texture />
                <color rgba="0 0 0 1" />
            </material>
        </visual>
        <collision>
            <origin xyz="0.010945 0.0009175 -8.32667e-17" rpy="3.14159 -1.14424e-17 1.5708" />
            <geometry>
                <mesh filename="meshes/collision/icub_simple_collision_r_shank.dae" scale="1 1 1" />
            </geometry>
        </collision>
    </link>
    <link name="r_shoulder_1">
        <inertial>
            <mass value="0.189" />
            <origin xyz="5e-06 0.0187 0.00119" rpy="0 -0 0" />
            <inertia ixx="0.000123" ixy="2.1e-08" ixz="-1e-09" iyy="2.44e-05" iyz="4.22e-06" izz="0.000113" />
        </inertial>
        <visual>
            <origin xyz="-3.08642e-14 0.109285 0.00521101" rpy="-0.0815582 1.5708 1.48924" />
            <geometry>
                <mesh filename="meshes/visual/icub_r_shoulder_1.dae" scale="1 1 1" />
            </geometry>
            <material name="material.metal">
                <texture />
                <color rgba="0 0 0 1" />
            </material>
        </visual>
        <collision>
            <origin xyz="-3.08642e-14 0.109285 0.00521101" rpy="-0.0815582 1.5708 1.48924" />
            <geometry>
                <mesh filename="meshes/collision/icub_simple_collision_r_shoulder_1.dae" scale="1 1 1" />
            </geometry>
        </collision>
    </link>
    <link name="r_shoulder_2">
        <inertial>
            <mass value="0.179" />
            <origin xyz="-9.4e-05 -0.00627 -0.0166" rpy="0 -0 0" />
            <inertia ixx="0.000137" ixy="-4.53e-07" ixz="2.03e-07" iyy="8.3e-05" iyz="2.07e-05" izz="9.93e-05" />
        </inertial>
        <visual>
            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="2.14034 -1.5708 -0.56954" />
            <geometry>
                <mesh filename="meshes/visual/icub_r_shoulder_2.dae" scale="1 1 1" />
            </geometry>
            <material name="material.metal">
                <texture />
                <color rgba="0 0 0 1" />
            </material>
        </visual>
        <collision>
            <origin xyz="-0.00154529 -0.00521101 -1.11181e-12" rpy="2.14034 -1.5708 -0.56954" />
            <geometry>
                <mesh filename="meshes/collision/icub_simple_collision_r_shoulder_2.dae" scale="1 1 1" />
            </geometry>
        </collision>
    </link>
    <link name="r_shoulder_3">
        <inertial>
            <mass value="0.155" />
            <origin xyz="0.00553932 -2.47652e-05 0.0493895" rpy="0 -0 0" />
            <inertia ixx="3.44122e-05" ixy="3.56977e-07" ixz="-3.95372e-06" iyy="4.33544e-05" iyz="-6.5128e-06" izz="6.49393e-05" />
        </inertial>
    </link>
    <link name="r_sole">
        <inertial>
            <mass value="0" />
            <origin xyz="0 0 0" rpy="0 -0 0" />
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
        </inertial>
    </link>
    <link name="r_upper_arm">
        <inertial>
            <mass value="0.729" />
            <origin xyz="-0.00015906 8.28733e-05 0.0298828" rpy="0 -0 0" />
            <inertia ixx="0.000408" ixy="-1.08e-06" ixz="-2.29e-06" iyy="0.00038" iyz="3.57e-06" izz="0.00026" />
        </inertial>
        <visual>
            <origin xyz="-0.0204334 -0.000143922 -0.068" rpy="-3.14159 -1.51363e-11 1.309" />
            <geometry>
                <mesh filename="meshes/visual/icub_r_arm.dae" scale="1 1 1" />
            </geometry>
            <material name="material.metal">
                <texture />
                <color rgba="0 0 0 1" />
            </material>
        </visual>
        <collision>
            <origin xyz="-0.0204334 -0.000143922 -0.068" rpy="-3.14159 -1.51363e-11 1.309" />
            <geometry>
                <mesh filename="meshes/collision/icub_simple_collision_r_arm.dae" scale="1 1 1" />
            </geometry>
        </collision>
    </link>
    <link name="r_upper_arm_dh_frame">
        <inertial>
            <mass value="0" />
            <origin xyz="0 0 0" rpy="0 -0 0" />
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
        </inertial>
    </link>
    <link name="r_upper_leg">
        <inertial>
            <mass value="1.522" />
            <origin xyz="0.0021968 -0.174502 -0.0013437" rpy="0 -0 0" />
            <inertia ixx="0.0454917" ixy="-0.000940971" ixz="-5.37405e-05" iyy="0.01004" iyz="-0.001203" izz="0.0458255" />
        </inertial>
        <visual>
            <origin xyz="0 0 0" rpy="-1.57072 -1.5708 3.14152" />
            <geometry>
                <mesh filename="meshes/visual/icub_r_thigh.dae" scale="1 1 1" />
            </geometry>
            <material name="material.metal">
                <texture />
                <color rgba="0 0 0 1" />
            </material>
        </visual>
        <collision>
            <origin xyz="0 0 0" rpy="-1.57072 -1.5708 3.14152" />
            <geometry>
                <mesh filename="meshes/collision/icub_simple_collision_r_thigh.dae" scale="1 1 1" />
            </geometry>
        </collision>
    </link>
    <link name="r_wrist_1">
        <inertial>
            <mass value="0" />
            <origin xyz="0 0 0" rpy="0 -0 0" />
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
        </inertial>
    </link>
    <link name="root_link">
        <inertial>
            <mass value="4.72" />
            <origin xyz="0 0 0" rpy="0 -0 0" />
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
        </inertial>
        <visual>
            <origin xyz="0 0 0" rpy="0 -0 0" />
            <geometry>
                <mesh filename="meshes/visual/icub_root_link.dae" scale="1 1 1" />
            </geometry>
            <material name="material.metal">
                <texture />
                <color rgba="0 0 0 1" />
            </material>
        </visual>
        <collision>
            <origin xyz="0 0 0" rpy="0 -0 0" />
            <geometry>
                <mesh filename="meshes/collision/icub_simple_collision_root_link.dae" scale="1 1 1" />
            </geometry>
        </collision>
    </link>
    <link name="torso">
        <inertial>
            <mass value="0" />
            <origin xyz="0 0 0" rpy="0 -0 0" />
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
        </inertial>
    </link>
    <link name="torso_1">
        <inertial>
            <mass value="0" />
            <origin xyz="0 0 0" rpy="0 -0 0" />
            <inertia ixx="0.0004544" ixy="-4.263e-05" ixz="-3.889e-08" iyy="0.001141" iyz="0" izz="0.001236" />
        </inertial>
        <visual>
            <origin xyz="0 0 0" rpy="-1.5708 5.10314e-12 1.11022e-16" />
            <geometry>
                <mesh filename="meshes/visual/icub_lap_belt_1.dae" scale="1 1 1" />
            </geometry>
            <material name="material.metal">
                <texture />
                <color rgba="0 0 0 1" />
            </material>
        </visual>
        <collision>
            <origin xyz="0 0 0" rpy="-1.5708 5.10314e-12 1.11022e-16" />
            <geometry>
                <mesh filename="meshes/collision/icub_simple_collision_lap_belt_1.dae" scale="1 1 1" />
            </geometry>
        </collision>
    </link>
    <link name="torso_2">
        <inertial>
            <mass value="0" />
            <origin xyz="0 0 0" rpy="0 -0 0" />
            <inertia ixx="0.0005308" ixy="-1.923e-06" ixz="5.095e-05" iyy="0.002031" iyz="-3.849e-07" izz="0.001803" />
        </inertial>
    </link>
    <joint name="torso_yaw" type="revolute">
        <origin xyz="-7.46298e-34 -0.0055 0" rpy="-1.5708 -0 -1.5708" />
        <axis xyz="0 -1 6.12323e-17" />
        <parent link="torso_2" />
        <child link="chest" />
        <dynamics damping="1" friction="0" />
        <limit effort="80" velocity="100" lower="-1.02974" upper="1.02974" />
    </joint>
    <joint name="chest_skin_frame_fixed_joint" type="fixed">
        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="-1.8747e-33 5.77686e-66 6.16298e-33" />
        <axis xyz="0 0 0" />
        <parent link="chest" />
        <child link="chest_skin_frame" />
    </joint>
    <joint name="codyco_balancing_world_fixed_joint" type="fixed">
        <origin xyz="-8.67362e-19 -1.38778e-17 0.004" rpy="3.14159 -0 0" />
        <axis xyz="0 0 0" />
        <parent link="l_foot" />
        <child link="codyco_balancing_world" />
    </joint>
    <joint name="neck_yaw" type="revolute">
        <origin xyz="-5.77779e-34 0 0" rpy="-1.5708 -0 1.5708" />
        <axis xyz="0 -1 6.12323e-17" />
        <parent link="neck_2" />
        <child link="head" />
        <dynamics damping="1" friction="0" />
        <limit effort="20" velocity="100" lower="-0.959931" upper="0.959931" />
    </joint>
    <joint name="imu_frame_fixed_joint" type="fixed">
        <origin xyz="0.0185 -0.1108 0.0066" rpy="1.5708 -0 0" />
        <axis xyz="0 0 0" />
        <parent link="head" />
        <child link="imu_frame" />
    </joint>
    <joint name="l_ankle_pitch" type="revolute">
        <origin xyz="-0.2005 0 0" rpy="-1.5708 0 0" />
        <axis xyz="0 -1 6.12323e-17" />
        <parent link="l_lower_leg" />
        <child link="l_ankle_1" />
        <dynamics damping="1" friction="0" />
        <limit effort="24" velocity="100" lower="-0.733038" upper="0.366519" />
    </joint>
    <joint name="l_ankle_roll" type="revolute">
        <origin xyz="0 0 0" rpy="0 -1.5708 0" />
        <axis xyz="1 0 0" />
        <parent link="l_ankle_1" />
        <child link="l_ankle_2" />
        <dynamics damping="1" friction="0" />
        <limit effort="11" velocity="100" lower="-0.418879" upper="0.418879" />
    </joint>
    <joint name="l_elbow" type="revolute">
        <origin xyz="0 0 0.08428" rpy="-6.12323e-17 0 0" />
        <axis xyz="0 1 6.12323e-17" />
        <parent link="l_upper_arm" />
        <child link="l_elbow_1" />
        <dynamics damping="1" friction="0" />
        <limit effort="20" velocity="100" lower="0.0959931" upper="1.85005" />
    </joint>
    <joint name="l_foot_ft_sensor" type="fixed">
        <origin xyz="-0.0035 0 0.0645" rpy="0 -0 0" />
        <axis xyz="0 0 0" />
        <parent link="l_ankle_2" />
        <child link="l_foot" />
    </joint>
    <joint name="l_foot_dh_frame_fixed_joint" type="fixed">
        <origin xyz="0 0 0.004" rpy="0 1.5708 0" />
        <axis xyz="0 0 0" />
        <parent link="l_foot" />
        <child link="l_foot_dh_frame" />
    </joint>
    <joint name="l_wrist_prosup" type="revolute">
        <origin xyz="-0.015 0 0" rpy="1.5708 -0 -1.5708" />
        <axis xyz="0 1 6.12323e-17" />
        <parent link="l_elbow_1" />
        <child link="l_forearm" />
        <dynamics damping="1" friction="0" />
        <limit effort="0.45" velocity="100" lower="-0.872665" upper="0.872665" />
    </joint>
    <joint name="l_forearm_dh_frame_fixed_joint" type="fixed">
        <origin xyz="0 0.1373 8.4072e-18" rpy="0 -0 0" />
        <axis xyz="0 0 0" />
        <parent link="l_forearm" />
        <child link="l_forearm_dh_frame" />
    </joint>
    <joint name="l_gripper_joint" type="fixed">
        <origin xyz="0.0625 0 -0.016" rpy="0 -0 0" />
        <axis xyz="0 0 0" />
        <parent link="l_hand" />
        <child link="l_gripper" />
    </joint>
    <joint name="l_wrist_yaw" type="revolute">
        <origin xyz="0 0 0" rpy="0 -0 0" />
        <axis xyz="0 0 1" />
        <parent link="l_wrist_1" />
        <child link="l_hand" />
        <dynamics damping="1" friction="0" />
        <limit effort="0.65" velocity="100" lower="-0.436332" upper="0.436332" />
    </joint>
    <joint name="l_hand_dh_frame_fixed_joint" type="fixed">
        <origin xyz="0.0625 0 -0.016" rpy="0 -0 0" />
        <axis xyz="0 0 0" />
        <parent link="l_hand" />
        <child link="l_hand_dh_frame" />
    </joint>
    <joint name="l_hip_pitch" type="revolute">
        <origin xyz="0 -0.0681 -0.1199" rpy="-3.14159 1.5708 -2.77556e-17" />
        <axis xyz="0 -1 6.12323e-17" />
        <parent link="root_link" />
        <child link="l_hip_1" />
        <dynamics damping="1" friction="0" />
        <limit effort="84" velocity="100" lower="-0.767945" upper="2.30383" />
    </joint>
    <joint name="l_hip_roll" type="revolute">
        <origin xyz="0 0 0" rpy="3.14159 -1.5708 -2.77556e-17" />
        <axis xyz="1 0 -6.12323e-17" />
        <parent link="l_hip_1" />
        <child link="l_hip_2" />
        <dynamics damping="1" friction="0" />
        <limit effort="84" velocity="100" lower="-2.07694" upper="0.296706" />
    </joint>
    <joint name="l_leg_ft_sensor" type="fixed">
        <origin xyz="0 0 0.0665" rpy="0 -0 0" />
        <axis xyz="0 0 0" />
        <parent link="l_hip_2" />
        <child link="l_hip_3" />
    </joint>
    <joint name="l_knee" type="revolute">
        <origin xyz="-0.0009175 -0.234545 -1.43617e-17" rpy="3.14159 -1.2326e-32 1.5708" />
        <axis xyz="0 1.22465e-16 -1" />
        <parent link="l_upper_leg" />
        <child link="l_lower_leg" />
        <dynamics damping="1" friction="0" />
        <limit effort="30" velocity="100" lower="-2.18166" upper="0.401426" />
    </joint>
    <joint name="l_shoulder_pitch" type="revolute">
        <origin xyz="0.00525 0.1433 -0.11026" rpy="1.309 5.55112e-17 1.5708" />
        <axis xyz="0 -1 5.55112e-17" />
        <parent link="chest" />
        <child link="l_shoulder_1" />
        <dynamics damping="1" friction="0" />
        <limit effort="84" velocity="100" lower="-1.65806" upper="0.0872665" />
    </joint>
    <joint name="l_shoulder_roll" type="revolute">
        <origin xyz="0 0 0" rpy="1.5708 -0 -1.5708" />
        <axis xyz="0 1 6.12323e-17" />
        <parent link="l_shoulder_1" />
        <child link="l_shoulder_2" />
        <dynamics damping="1" friction="0" />
        <limit effort="84" velocity="100" lower="0" upper="2.80649" />
    </joint>
    <joint name="l_shoulder_yaw" type="revolute">
        <origin xyz="4.33681e-19 1.73472e-18 0" rpy="6.12323e-17 -0 1.309" />
        <axis xyz="0 6.12323e-17 1" />
        <parent link="l_shoulder_2" />
        <child link="l_shoulder_3" />
        <dynamics damping="1" friction="0" />
        <limit effort="34" velocity="100" lower="-0.645772" upper="1.74533" />
    </joint>
    <joint name="l_sole_fixed_joint" type="fixed">
        <origin xyz="-8.67362e-19 -1.38778e-17 0.004" rpy="3.14159 -0 0" />
        <axis xyz="0 0 0" />
        <parent link="l_foot" />
        <child link="l_sole" />
    </joint>
    <joint name="l_arm_ft_sensor" type="fixed">
        <origin xyz="0.015 9.36797e-18 0.068" rpy="0 -0 0" />
        <axis xyz="0 0 0" />
        <parent link="l_shoulder_3" />
        <child link="l_upper_arm" />
    </joint>
    <joint name="l_upper_arm_dh_frame_fixed_joint" type="fixed">
        <origin xyz="0 0 0.08428" rpy="-1.5708 0 0" />
        <axis xyz="0 0 0" />
        <parent link="l_upper_arm" />
        <child link="l_upper_arm_dh_frame" />
    </joint>
    <joint name="l_hip_yaw" type="revolute">
        <origin xyz="0 -1.68519e-34 -0.0665" rpy="-1.5708 -6.16298e-33 -6.12323e-17" />
        <axis xyz="0 1 6.12323e-17" />
        <parent link="l_hip_3" />
        <child link="l_upper_leg" />
        <dynamics damping="1" friction="0" />
        <limit effort="40" velocity="100" lower="-1.37881" upper="1.37881" />
    </joint>
    <joint name="l_wrist_pitch" type="revolute">
        <origin xyz="0 0.1373 8.4072e-18" rpy="1.5708 -0 1.5708" />
        <axis xyz="0 1 6.12323e-17" />
        <parent link="l_forearm" />
        <child link="l_wrist_1" />
        <dynamics damping="1" friction="0" />
        <limit effort="0.65" velocity="100" lower="-1.13446" upper="0.174533" />
    </joint>
    <joint name="neck_pitch" type="revolute">
        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="1.5708 -0 1.5708" />
        <axis xyz="0 1 6.12323e-17" />
        <parent link="chest" />
        <child link="neck_1" />
        <dynamics damping="1" friction="0" />
        <limit effort="20" velocity="100" lower="-0.698132" upper="0.523599" />
    </joint>
    <joint name="neck_roll" type="revolute">
        <origin xyz="0.0095 0 0" rpy="-1.5708 -0 -1.5708" />
        <axis xyz="0 -1 6.12323e-17" />
        <parent link="neck_1" />
        <child link="neck_2" />
        <dynamics damping="1" friction="0" />
        <limit effort="20" velocity="100" lower="-1.22173" upper="1.0472" />
    </joint>
    <joint name="r_ankle_pitch" type="revolute">
        <origin xyz="-0.2005 0 0" rpy="1.5708 -0 0" />
        <axis xyz="0 1 6.12323e-17" />
        <parent link="r_lower_leg" />
        <child link="r_ankle_1" />
        <dynamics damping="1" friction="0" />
        <limit effort="24" velocity="100" lower="-0.733038" upper="0.366519" />
    </joint>
    <joint name="r_ankle_roll" type="revolute">
        <origin xyz="0 0 0" rpy="0.504019 1.5708 -2.63757" />
        <axis xyz="-1 1.22465e-16 0" />
        <parent link="r_ankle_1" />
        <child link="r_ankle_2" />
        <dynamics damping="1" friction="0" />
        <limit effort="11" velocity="100" lower="-0.418879" upper="0.418879" />
    </joint>
    <joint name="r_elbow" type="revolute">
        <origin xyz="0 0 0.08428" rpy="3.14159 -0 3.14159" />
        <axis xyz="0 1 6.12323e-17" />
        <parent link="r_upper_arm" />
        <child link="r_elbow_1" />
        <dynamics damping="1" friction="0" />
        <limit effort="20" velocity="100" lower="0.0959931" upper="1.85005" />
    </joint>
    <joint name="r_foot_ft_sensor" type="fixed">
        <origin xyz="-0.0035 4.28626e-19 0.0645" rpy="0 -0 0" />
        <axis xyz="0 0 0" />
        <parent link="r_ankle_2" />
        <child link="r_foot" />
    </joint>
    <joint name="r_foot_dh_frame_fixed_joint" type="fixed">
        <origin xyz="0 0 0.004" rpy="0 1.5708 0" />
        <axis xyz="0 0 0" />
        <parent link="r_foot" />
        <child link="r_foot_dh_frame" />
    </joint>
    <joint name="r_wrist_prosup" type="revolute">
        <origin xyz="0.015 0 0" rpy="1.5708 -0 -1.5708" />
        <axis xyz="0 1 6.12323e-17" />
        <parent link="r_elbow_1" />
        <child link="r_forearm" />
        <dynamics damping="1" friction="0" />
        <limit effort="0.45" velocity="100" lower="-0.872665" upper="0.872665" />
    </joint>
    <joint name="r_forearm_dh_frame_fixed_joint" type="fixed">
        <origin xyz="0 -0.1373 -8.4072e-18" rpy="0 -0 0" />
        <axis xyz="0 0 0" />
        <parent link="r_forearm" />
        <child link="r_forearm_dh_frame" />
    </joint>
    <joint name="r_gripper_joint" type="fixed">
        <origin xyz="0.0625 0 0.016" rpy="0 -0 0" />
        <axis xyz="0 0 0" />
        <parent link="r_hand" />
        <child link="r_gripper" />
    </joint>
    <joint name="r_wrist_yaw" type="revolute">
        <origin xyz="0 0 0" rpy="0 -0 3.14159" />
        <axis xyz="0 0 1" />
        <parent link="r_wrist_1" />
        <child link="r_hand" />
        <dynamics damping="1" friction="0" />
        <limit effort="0.65" velocity="100" lower="-0.436332" upper="0.436332" />
    </joint>
    <joint name="r_hand_dh_frame_fixed_joint" type="fixed">
        <origin xyz="0.0625 0 0.016" rpy="0 -0 0" />
        <axis xyz="0 0 0" />
        <parent link="r_hand" />
        <child link="r_hand_dh_frame" />
    </joint>
    <joint name="r_hip_pitch" type="revolute">
        <origin xyz="0 0.0681 -0.1199" rpy="-2.77556e-17 1.5708 2.77556e-17" />
        <axis xyz="0 1 6.12323e-17" />
        <parent link="root_link" />
        <child link="r_hip_1" />
        <dynamics damping="1" friction="0" />
        <limit effort="84" velocity="100" lower="-0.767945" upper="2.30383" />
    </joint>
    <joint name="r_hip_roll" type="revolute">
        <origin xyz="0 0 0" rpy="2.77556e-17 1.5708 -2.77556e-17" />
        <axis xyz="-1 0 6.12323e-17" />
        <parent link="r_hip_1" />
        <child link="r_hip_2" />
        <dynamics damping="1" friction="0" />
        <limit effort="84" velocity="100" lower="-2.07694" upper="0.296706" />
    </joint>
    <joint name="r_leg_ft_sensor" type="fixed">
        <origin xyz="0 0 0.0665" rpy="0 -0 0" />
        <axis xyz="0 0 0" />
        <parent link="r_hip_2" />
        <child link="r_hip_3" />
    </joint>
    <joint name="r_knee" type="revolute">
        <origin xyz="-0.0009175 -0.234545 1.43617e-17" rpy="3.14159 -1.2326e-32 1.5708" />
        <axis xyz="0 1.22465e-16 -1" />
        <parent link="r_upper_leg" />
        <child link="r_lower_leg" />
        <dynamics damping="1" friction="0" />
        <limit effort="30" velocity="100" lower="-2.18166" upper="0.401426" />
    </joint>
    <joint name="r_shoulder_pitch" type="revolute">
        <origin xyz="0.00525 0.1433 0.11026" rpy="-1.309 -0 1.5708" />
        <axis xyz="0 1 5.55112e-17" />
        <parent link="chest" />
        <child link="r_shoulder_1" />
        <dynamics damping="1" friction="0" />
        <limit effort="84" velocity="100" lower="-1.65806" upper="0.0872665" />
    </joint>
    <joint name="r_shoulder_roll" type="revolute">
        <origin xyz="0 0 0" rpy="-1.5708 -0 -1.5708" />
        <axis xyz="0 -1 6.12323e-17" />
        <parent link="r_shoulder_1" />
        <child link="r_shoulder_2" />
        <dynamics damping="1" friction="0" />
        <limit effort="84" velocity="100" lower="0" upper="2.80649" />
    </joint>
    <joint name="r_shoulder_yaw" type="revolute">
        <origin xyz="-4.33681e-19 0 0" rpy="3.14159 -0 1.309" />
        <axis xyz="0 6.12323e-17 -1" />
        <parent link="r_shoulder_2" />
        <child link="r_shoulder_3" />
        <dynamics damping="1" friction="0" />
        <limit effort="34" velocity="100" lower="-0.645772" upper="1.74533" />
    </joint>
    <joint name="r_sole_fixed_joint" type="fixed">
        <origin xyz="-8.67362e-19 1.38778e-17 0.004" rpy="3.14159 -0 0" />
        <axis xyz="0 0 0" />
        <parent link="r_foot" />
        <child link="r_sole" />
    </joint>
    <joint name="r_arm_ft_sensor" type="fixed">
        <origin xyz="0.015 -8.93429e-18 0.068" rpy="0 -0 0" />
        <axis xyz="0 0 0" />
        <parent link="r_shoulder_3" />
        <child link="r_upper_arm" />
    </joint>
    <joint name="r_upper_arm_dh_frame_fixed_joint" type="fixed">
        <origin xyz="0 0 0.08428" rpy="1.5708 -0 3.14159" />
        <axis xyz="0 0 0" />
        <parent link="r_upper_arm" />
        <child link="r_upper_arm_dh_frame" />
    </joint>
    <joint name="r_hip_yaw" type="revolute">
        <origin xyz="0 1.68519e-34 -0.0665" rpy="-1.5708 -6.16298e-33 6.12323e-17" />
        <axis xyz="0 -1 6.12323e-17" />
        <parent link="r_hip_3" />
        <child link="r_upper_leg" />
        <dynamics damping="1" friction="0" />
        <limit effort="40" velocity="100" lower="-1.37881" upper="1.37881" />
    </joint>
    <joint name="r_wrist_pitch" type="revolute">
        <origin xyz="0 -0.1373 -8.4072e-18" rpy="1.5708 -0 1.5708" />
        <axis xyz="0 1 6.12323e-17" />
        <parent link="r_forearm" />
        <child link="r_wrist_1" />
        <dynamics damping="1" friction="0" />
        <limit effort="0.65" velocity="100" lower="-1.13446" upper="0.174533" />
    </joint>
    <joint name="base_fixed_joint" type="fixed">
        <origin xyz="0 0 0" rpy="0 -0 0" />
        <axis xyz="0 0 0" />
        <parent link="base_link" />
        <child link="root_link" />
    </joint>
    <joint name="torso_joint" type="fixed">
        <origin xyz="0.00231 0.1933 -1.18362e-17" rpy="0 -0 0" />
        <axis xyz="0 0 0" />
        <parent link="chest" />
        <child link="torso" />
    </joint>
    <joint name="torso_pitch" type="revolute">
        <origin xyz="0 0 0" rpy="-2.77556e-17 -1.5708 3.14159" />
        <axis xyz="0 1 6.12323e-17" />
        <parent link="root_link" />
        <child link="torso_1" />
        <dynamics damping="1" friction="0" />
        <limit effort="36" velocity="100" lower="-0.383972" upper="1.46608" />
    </joint>
    <joint name="torso_roll" type="revolute">
        <origin xyz="0.032 -2.67276e-51 0" rpy="1.5708 -0 -1.5708" />
        <axis xyz="0 1 6.12323e-17" />
        <parent link="torso_1" />
        <child link="torso_2" />
        <dynamics damping="1" friction="0" />
        <limit effort="80" velocity="100" lower="-0.680678" upper="0.680678" />
    </joint>
    <gazebo reference="l_arm_ft_sensor">
        <sensor name="left_arm_ft" type="force_torque">
            <force_torque>
                <frame>child</frame>
                <measure_direction>child_to_parent</measure_direction>
            </force_torque>
            <update_rate>100</update_rate>
            <always_on>1</always_on>
        </sensor>
    </gazebo>
    <gazebo reference="r_arm_ft_sensor">
        <sensor name="right_arm_ft" type="force_torque">
            <force_torque>
                <frame>child</frame>
                <measure_direction>child_to_parent</measure_direction>
            </force_torque>
            <update_rate>100</update_rate>
            <always_on>1</always_on>
        </sensor>
    </gazebo>
    <gazebo reference="l_leg_ft_sensor">
        <sensor name="left_leg_ft" type="force_torque">
            <force_torque>
                <frame>child</frame>
                <measure_direction>child_to_parent</measure_direction>
            </force_torque>
            <update_rate>100</update_rate>
            <always_on>1</always_on>
        </sensor>
    </gazebo>
    <gazebo reference="r_leg_ft_sensor">
        <sensor name="right_leg_ft" type="force_torque">
            <force_torque>
                <frame>child</frame>
                <measure_direction>child_to_parent</measure_direction>
            </force_torque>
            <update_rate>100</update_rate>
            <always_on>1</always_on>
        </sensor>
    </gazebo>
    <gazebo reference="l_foot_ft_sensor">
        <sensor name="left_foot_ft" type="force_torque">
            <force_torque>
                <frame>child</frame>
                <measure_direction>child_to_parent</measure_direction>
            </force_torque>
            <update_rate>100</update_rate>
            <always_on>1</always_on>
        </sensor>
    </gazebo>
    <gazebo reference="r_foot_ft_sensor">
        <sensor name="right_foot_ft" type="force_torque">
            <force_torque>
                <frame>child</frame>
                <measure_direction>child_to_parent</measure_direction>
            </force_torque>
            <update_rate>100</update_rate>
            <always_on>1</always_on>
        </sensor>
    </gazebo>
</robot>
