1340 lines
48 KiB
XML
1340 lines
48 KiB
XML
<?xml version="1.0"?>
|
|
<!--URDF MODEL 48 DoFs-->
|
|
<!-- https://github.com/robotology/human-gazebo/tree/import_urdf/humanSubject06 -->
|
|
<robot name="XSensStyleModel_template">
|
|
|
|
<material name="color">
|
|
<color rgba="0.1 0.1 0.1 1"/>
|
|
</material>
|
|
|
|
<!--To open this model with Gazebo replace the null masses, inertias and dimensions of the 'fake links f1 and f2'-->
|
|
<!--LINKS-->
|
|
<!--Link base (1)-->
|
|
<link name="Pelvis">
|
|
<inertial>
|
|
<mass value="5.696"/>
|
|
<!--COM origin wrt pHipOrigin-->
|
|
<origin xyz="0 0 0.059167" rpy="0 0 0" />
|
|
<inertia ixx="0.050919" iyy="0.019195" izz="0.058056" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
|
|
<visual>
|
|
<!--box origin wrt pHipOrigin-->
|
|
<origin xyz="0 0 0.059167" rpy="0 0 0" />
|
|
<geometry>
|
|
<box size="0.16655 0.30753 0.1127"/>
|
|
</geometry>
|
|
<material name="color"/>
|
|
</visual>
|
|
|
|
<collision>
|
|
<!--box origin wrt pHipOrigin-->
|
|
<origin xyz="0 0 0.059167" rpy="0 0 0" />
|
|
<geometry>
|
|
<box size="0.16655 0.30753 0.1127"/>
|
|
</geometry>
|
|
</collision>
|
|
|
|
</link>
|
|
<!--Chain from (2) to (7)-->
|
|
<link name="L5_f1">
|
|
<inertial>
|
|
<mass value="0"/>
|
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
<inertia ixx="0" iyy="0" izz="0" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
</link>
|
|
<link name="L5">
|
|
<inertial>
|
|
<mass value="7.2624"/>
|
|
<!--COM origin wrt jL5S1-->
|
|
<origin xyz="0 0 0.044881" rpy="0 0 0" />
|
|
<inertia ixx="0.02831" iyy="0.021663" izz="0.040221" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
|
|
<visual>
|
|
<!--box origin origin wrt jL5S1-->
|
|
<origin xyz="0 0 0.044881" rpy="0 0 0" />
|
|
<geometry>
|
|
<box size="0.16655 0.19678 0.089762"/>
|
|
</geometry>
|
|
<material name="color"/>
|
|
</visual>
|
|
|
|
<collision>
|
|
<!--box origin origin wrt jL5S1-->
|
|
<origin xyz="0 0 0.044881" rpy="0 0 0" />
|
|
<geometry>
|
|
<box size="0.16655 0.19678 0.089762"/>
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<link name="L3_f1">
|
|
<inertial>
|
|
<mass value="0"/>
|
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
<inertia ixx="0" iyy="0" izz="0" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
</link>
|
|
<link name="L3">
|
|
<inertial>
|
|
<mass value="7.2624"/>
|
|
<!--COM origin wrt jL4L3-->
|
|
<origin xyz="0 0 0.040536" rpy="0 0 0" />
|
|
<inertia ixx="0.027412" iyy="0.020764" izz="0.040221" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
|
|
<visual>
|
|
<!--box origin origin wrt jL4L3-->
|
|
<origin xyz="0 0 0.040536" rpy="0 0 0" />
|
|
<geometry>
|
|
<box size="0.16655 0.19678 0.081071"/>
|
|
</geometry>
|
|
<material name="color"/>
|
|
</visual>
|
|
|
|
<collision>
|
|
<!--box origin origin wrt jL4L3-->
|
|
<origin xyz="0 0 0.040536" rpy="0 0 0" />
|
|
<geometry>
|
|
<box size="0.16655 0.19678 0.081071"/>
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<link name="T12_f1">
|
|
<inertial>
|
|
<mass value="0"/>
|
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
<inertia ixx="0" iyy="0" izz="0" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
</link>
|
|
<link name="T12">
|
|
<inertial>
|
|
<mass value="7.2624"/>
|
|
<!--COM origin wrt jL1T12-->
|
|
<origin xyz="0 0 0.040536" rpy="0 0 0" />
|
|
<inertia ixx="0.027412" iyy="0.020764" izz="0.040221" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
|
|
<visual>
|
|
<!--box origin origin wrt jL1T12-->
|
|
<origin xyz="0 0 0.040536" rpy="0 0 0" />
|
|
<geometry>
|
|
<box size="0.16655 0.19678 0.081071"/>
|
|
</geometry>
|
|
<material name="color"/>
|
|
</visual>
|
|
|
|
<collision>
|
|
<!--box origin origin wrt jL1T12-->
|
|
<origin xyz="0 0 0.040536" rpy="0 0 0" />
|
|
<geometry>
|
|
<box size="0.16655 0.19678 0.081071"/>
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<link name="T8_f1">
|
|
<inertial>
|
|
<mass value="0"/>
|
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
<inertia ixx="0" iyy="0" izz="0" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
</link>
|
|
<link name="T8_f2">
|
|
<inertial>
|
|
<mass value="0"/>
|
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
<inertia ixx="0" iyy="0" izz="0" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
</link>
|
|
<link name="T8">
|
|
<inertial>
|
|
<mass value="2.848"/>
|
|
<!--COM origin wrt jT9T8-->
|
|
<origin xyz="0 0 0.058058" rpy="0 0 0" />
|
|
<inertia ixx="0.0040381" iyy="0.0077541" izz="0.0053923" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
|
|
<visual>
|
|
<!--box origin wrt jT9T8-->
|
|
<origin xyz="0 0 0.058058" rpy="0 0 0" />
|
|
<geometry>
|
|
<box size="0.13852 0.059426 0.11612"/>
|
|
</geometry>
|
|
<material name="color"/>
|
|
</visual>
|
|
|
|
<collision>
|
|
<!--box origin wrt jT9T8-->
|
|
<origin xyz="0 0 0.058058" rpy="0 0 0" />
|
|
<geometry>
|
|
<box size="0.13852 0.059426 0.11612"/>
|
|
</geometry>
|
|
</collision>
|
|
|
|
</link>
|
|
<link name="Neck_f1">
|
|
<inertial>
|
|
<mass value="0"/>
|
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
<inertia ixx="0" iyy="0" izz="0" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
</link>
|
|
<link name="Neck_f2">
|
|
<inertial>
|
|
<mass value="0"/>
|
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
<inertia ixx="0" iyy="0" izz="0" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
</link>
|
|
<link name="Neck">
|
|
<inertial>
|
|
<mass value="0.8544"/>
|
|
<!--COM origin wrt jT1C7-->
|
|
<origin xyz="0 0 0.048176" rpy="0 0 0" />
|
|
<inertia ixx="0.00086442" iyy="0.00086442" izz="0.00040681" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
|
|
<visual>
|
|
<!--box origin wrt jT1C7-->
|
|
<origin xyz="0 0 0.048176" rpy="0 0 0" />
|
|
<geometry>
|
|
<cylinder length="0.096353" radius="0.01543"/>
|
|
</geometry>
|
|
<material name="color"/>
|
|
</visual>
|
|
|
|
<collision>
|
|
<!--box origin wrt jT1C7-->
|
|
<origin xyz="0 0 0.048176" rpy="0 0 0" />
|
|
<geometry>
|
|
<cylinder length="0.096353" radius="0.01543"/>
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<link name="Head_f1">
|
|
<inertial>
|
|
<mass value="0"/>
|
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
<inertia ixx="0" iyy="0" izz="0" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
</link>
|
|
<link name="Head">
|
|
<inertial>
|
|
<mass value="2.5632"/>
|
|
<!--COM origin wrt jC1Head-->
|
|
<origin xyz="0 0 0.099452" rpy="0 0 0" />
|
|
<inertia ixx="0.010141" iyy="0.010141" izz="0.010141" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
|
|
<visual>
|
|
<!--boc origin wrt jC1Head-->
|
|
<origin xyz="0 0 0.099452" rpy="0 0 0" />
|
|
<geometry>
|
|
<sphere radius="0.099452"/>
|
|
</geometry>
|
|
<material name="color"/>
|
|
</visual>
|
|
|
|
<collision>
|
|
<!--box origin wrt jC1Head-->
|
|
<origin xyz="0 0 0.099452" rpy="0 0 0" />
|
|
<geometry>
|
|
<sphere radius="0.099452"/>
|
|
</geometry>
|
|
</collision>
|
|
|
|
</link>
|
|
<!--Chain from (8) to (11)-->
|
|
<link name="RightShoulder">
|
|
<inertial>
|
|
<mass value="2.2072"/>
|
|
<!--COM origin wrt jRightC7Shoulder-->
|
|
<origin xyz="0 -0.078854 0" rpy="0 0 0" />
|
|
<inertia ixx="0.0050082" iyy="0.00086686" izz="0.0050082" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
|
|
<visual>
|
|
<!--box origin wrt jRightC7Shoulder. RPY rotated of pi/2.-->
|
|
<origin xyz="0 -0.078854 0" rpy="1.5708 0 0" />
|
|
<geometry>
|
|
<cylinder length="0.15771" radius="0.028026"/>
|
|
</geometry>
|
|
<material name="color"/>
|
|
</visual>
|
|
|
|
<collision>
|
|
<!--box origin wrt jRightC7Shoulder. RPY rotated of pi/2.-->
|
|
<origin xyz="0 -0.078854 0" rpy="1.5708 0 0" />
|
|
<geometry>
|
|
<cylinder length="0.15771" radius="0.028026"/>
|
|
</geometry>
|
|
</collision>
|
|
|
|
</link>
|
|
<link name="RightUpperArm_f1">
|
|
<inertial>
|
|
<mass value="0"/>
|
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
<inertia ixx="0" iyy="0" izz="0" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
</link>
|
|
<link name="RightUpperArm_f2">
|
|
<inertial>
|
|
<mass value="0"/>
|
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
<inertia ixx="0" iyy="0" izz="0" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
</link>
|
|
<link name="RightUpperArm">
|
|
<inertial>
|
|
<mass value="2.136"/>
|
|
<!--COM origin wrt jRightShoulder-->
|
|
<origin xyz="0 -0.14573 0" rpy="0 0 0" />
|
|
<inertia ixx="0.015663" iyy="0.0010831" izz="0.015663" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
|
|
<visual>
|
|
<!--box origin wrt jRightShoulder. RPY rotated of pi/2.-->
|
|
<origin xyz="0 -0.14573 0" rpy="1.5708 0 0" />
|
|
<geometry>
|
|
<cylinder length="0.29147" radius="0.031846"/>
|
|
</geometry>
|
|
<material name="color"/>
|
|
</visual>
|
|
|
|
<collision>
|
|
<!--box origin wrt jRightShoulder. RPY rotated of pi/2.-->
|
|
<origin xyz="0 -0.14573 0" rpy="1.5708 0 0" />
|
|
<geometry>
|
|
<cylinder length="0.29147" radius="0.031846"/>
|
|
</geometry>
|
|
</collision>
|
|
|
|
</link>
|
|
<link name="RightForeArm_f1">
|
|
<inertial>
|
|
<mass value="0"/>
|
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
<inertia ixx="0" iyy="0" izz="0" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
</link>
|
|
<link name="RightForeArm">
|
|
<inertial>
|
|
<mass value="1.424"/>
|
|
<!--COM origin wrt jRightElbow-->
|
|
<origin xyz="0 -0.12047 0" rpy="0 0 0" />
|
|
<inertia ixx="0.0070493" iyy="0.00032093" izz="0.0070493" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
|
|
<visual>
|
|
<!--box origin wrt jRightElbow. RPY rotated of pi/2.-->
|
|
<origin xyz="0 -0.12047 0" rpy="1.5708 0 0" />
|
|
<geometry>
|
|
<cylinder length="0.24094" radius="0.021231"/>
|
|
</geometry>
|
|
<material name="color"/>
|
|
</visual>
|
|
|
|
<collision>
|
|
<!--box origin wrt jRightElbow. RPY rotated of pi/2.-->
|
|
<origin xyz="0 -0.12047 0" rpy="1.5708 0 0" />
|
|
<geometry>
|
|
<cylinder length="0.24094" radius="0.021231"/>
|
|
</geometry>
|
|
</collision>
|
|
|
|
</link>
|
|
<link name="RightHand_f1">
|
|
<inertial>
|
|
<mass value="0"/>
|
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
<inertia ixx="0" iyy="0" izz="0" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
</link>
|
|
<link name="RightHand">
|
|
<inertial>
|
|
<mass value="0.4272"/>
|
|
<!--COM origin wrt jRightWrist-->
|
|
<origin xyz="0 -0.090968 0" rpy="0 0 0" />
|
|
<inertia ixx="0.0012426" iyy="0.00058792" izz="0.0017021" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
|
|
<visual>
|
|
<!--box origin wrt jRightWrist-->
|
|
<origin xyz="0 -0.090968 0" rpy="0 0 0" />
|
|
<geometry>
|
|
<box size="0.12129 0.18194 0.042461"/>
|
|
</geometry>
|
|
<material name="color"/>
|
|
</visual>
|
|
|
|
<collision>
|
|
<!--box origin wrt jRightWrist-->
|
|
<origin xyz="0 -0.090968 0" rpy="0 0 0" />
|
|
<geometry>
|
|
<box size="0.12129 0.18194 0.042461"/>
|
|
</geometry>
|
|
</collision>
|
|
|
|
</link>
|
|
<!--Chain from (12) to (15)-->
|
|
<link name="LeftShoulder">
|
|
<inertial>
|
|
<mass value="2.2072"/>
|
|
<!--COM origin wrt jLeftC7Shoulder-->
|
|
<origin xyz="0 0.078854 0" rpy="0 0 0" />
|
|
<inertia ixx="0.0050082" iyy="0.00086686" izz="0.0050082" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
|
|
<visual>
|
|
<!--box origin wrt jLeftC7Shoulder. RPY rotated of pi/2.-->
|
|
<origin xyz="0 0.078854 0" rpy="1.5708 0 0" />
|
|
<geometry>
|
|
<cylinder length="0.15771" radius="0.028026"/>
|
|
</geometry>
|
|
<material name="color"/>
|
|
</visual>
|
|
|
|
<collision>
|
|
<!--box origin wrt jLeftC7Shoulder. RPY rotated of pi/2.-->
|
|
<origin xyz="0 0.078854 0" rpy="1.5708 0 0" />
|
|
<geometry>
|
|
<cylinder length="0.15771" radius="0.028026"/>
|
|
</geometry>
|
|
</collision>
|
|
|
|
</link>
|
|
<link name="LeftUpperArm_f1">
|
|
<inertial>
|
|
<mass value="0"/>
|
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
<inertia ixx="0" iyy="0" izz="0" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
</link>
|
|
<link name="LeftUpperArm_f2">
|
|
<inertial>
|
|
<mass value="0"/>
|
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
<inertia ixx="0" iyy="0" izz="0" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
</link>
|
|
<link name="LeftUpperArm">
|
|
<inertial>
|
|
<mass value="2.136"/>
|
|
<!--COM origin wrt jLeftShoulder-->
|
|
<origin xyz="0 0.14573 0" rpy="0 0 0" />
|
|
<inertia ixx="0.015663" iyy="0.0010831" izz="0.015663" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
|
|
<visual>
|
|
<!--box origin wrt jLeftShoulder. RPY rotated of pi/2.-->
|
|
<origin xyz="0 0.14573 0" rpy="1.5708 0 0" />
|
|
<geometry>
|
|
<cylinder length="0.29147" radius="0.031846"/>
|
|
</geometry>
|
|
<material name="color"/>
|
|
</visual>
|
|
|
|
<collision>
|
|
<!--box origin wrt jLeftShoulder. RPY rotated of pi/2.-->
|
|
<origin xyz="0 0.14573 0" rpy="1.5708 0 0" />
|
|
<geometry>
|
|
<cylinder length="0.29147" radius="0.031846"/>
|
|
</geometry>
|
|
</collision>
|
|
|
|
</link>
|
|
<link name="LeftForeArm_f1">
|
|
<inertial>
|
|
<mass value="0"/>
|
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
<inertia ixx="0" iyy="0" izz="0" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
</link>
|
|
<link name="LeftForeArm">
|
|
<inertial>
|
|
<mass value="1.424"/>
|
|
<!--COM origin wrt jLeftElbow-->
|
|
<origin xyz="0 0.12047 0" rpy="0 0 0" />
|
|
<inertia ixx="0.0070493" iyy="0.00032093" izz="0.0070493" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
|
|
<visual>
|
|
<!--box origin wrt jLeftElbow. RPY rotated of pi/2.-->
|
|
<origin xyz="0 0.12047 0" rpy="1.5708 0 0" />
|
|
<geometry>
|
|
<cylinder length="0.24094" radius="0.021231"/>
|
|
</geometry>
|
|
<material name="color"/>
|
|
</visual>
|
|
|
|
<collision>
|
|
<!--box origin wrt jLeftElbow. RPY rotated of pi/2.-->
|
|
<origin xyz="0 0.12047 0" rpy="1.5708 0 0" />
|
|
<geometry>
|
|
<cylinder length="0.24094" radius="0.021231"/>
|
|
</geometry>
|
|
</collision>
|
|
|
|
</link>
|
|
<link name="LeftHand_f1">
|
|
<inertial>
|
|
<mass value="0"/>
|
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
<inertia ixx="0" iyy="0" izz="0" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
</link>
|
|
<link name="LeftHand">
|
|
<inertial>
|
|
<mass value="0.4272"/>
|
|
<!--COM origin wrt jLeftWrist-->
|
|
<origin xyz="0 0.090968 0" rpy="0 0 0" />
|
|
<inertia ixx="0.0012426" iyy="0.00058792" izz="0.0017021" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
|
|
<visual>
|
|
<!--COM origin wrt jLeftWrist-->
|
|
<origin xyz="0 0.090968 0" rpy="0 0 0" />
|
|
<geometry>
|
|
<box size="0.12129 0.18194 0.042461"/>
|
|
</geometry>
|
|
<material name="color"/>
|
|
</visual>
|
|
|
|
<collision>
|
|
<!--COM origin wrt jLeftWrist-->
|
|
<origin xyz="0 0.090968 0" rpy="0 0 0" />
|
|
<geometry>
|
|
<box size="0.12129 0.18194 0.042461"/>
|
|
</geometry>
|
|
</collision>
|
|
|
|
</link>
|
|
<!--Chain from (16) to (19)-->
|
|
<link name="RightUpperLeg_f1">
|
|
<inertial>
|
|
<mass value="0"/>
|
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
<inertia ixx="0" iyy="0" izz="0" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
</link>
|
|
<link name="RightUpperLeg_f2">
|
|
<inertial>
|
|
<mass value="0"/>
|
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
<inertia ixx="0" iyy="0" izz="0" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
</link>
|
|
<link name="RightUpperLeg">
|
|
<inertial>
|
|
<mass value="8.9"/>
|
|
<!--COM origin wrt jRightHip-->
|
|
<origin xyz="0 0 -0.26304" rpy="0 0 0" />
|
|
<inertia ixx="0.21503" iyy="0.21503" izz="0.019525" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
|
|
<visual>
|
|
<!--box origin wrt jRightHip-->
|
|
<origin xyz="0 0 -0.26304" rpy="0 0 0" />
|
|
<geometry>
|
|
<cylinder length="0.52609" radius="0.066238"/>
|
|
</geometry>
|
|
<material name="color"/>
|
|
</visual>
|
|
|
|
<collision>
|
|
<!--box origin wrt jRightHip-->
|
|
<origin xyz="0 0 -0.26304" rpy="0 0 0" />
|
|
<geometry>
|
|
<cylinder length="0.52609" radius="0.066238"/>
|
|
</geometry>
|
|
</collision>
|
|
|
|
</link>
|
|
<link name="RightLowerLeg_f1">
|
|
<inertial>
|
|
<mass value="0"/>
|
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
<inertia ixx="0" iyy="0" izz="0" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
</link>
|
|
<link name="RightLowerLeg">
|
|
<inertial>
|
|
<mass value="2.5988"/>
|
|
<!--COM origin wrt jRightKnee-->
|
|
<origin xyz="0 0 -0.20614" rpy="0 0 0" />
|
|
<inertia ixx="0.037755" iyy="0.037755" izz="0.0018892" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
|
|
<visual>
|
|
<!--box origin wrt jRightKnee-->
|
|
<origin xyz="0 0 -0.20614" rpy="0 0 0" />
|
|
<geometry>
|
|
<cylinder length="0.41228" radius="0.03813"/>
|
|
</geometry>
|
|
<material name="color"/>
|
|
</visual>
|
|
|
|
<collision>
|
|
<!--box origin wrt jRightKnee-->
|
|
<origin xyz="0 0 -0.20614" rpy="0 0 0" />
|
|
<geometry>
|
|
<cylinder length="0.41228" radius="0.03813"/>
|
|
</geometry>
|
|
</collision>
|
|
|
|
</link>
|
|
<link name="RightFoot_f1">
|
|
<inertial>
|
|
<mass value="0"/>
|
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
<inertia ixx="0" iyy="0" izz="0" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
</link>
|
|
<link name="RightFoot_f2">
|
|
<inertial>
|
|
<mass value="0"/>
|
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
<inertia ixx="0" iyy="0" izz="0" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
</link>
|
|
<link name="RightFoot">
|
|
<inertial>
|
|
<mass value="0.9256"/>
|
|
<!--COM origin wrt jRightAnkle-->
|
|
<origin xyz="0.061581 0 -0.040628" rpy="0 0 0" />
|
|
<inertia ixx="0.00095786" iyy="0.0038434" izz="0.0037827" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
|
|
<visual>
|
|
<!--box origin wrt jRightAnkle-->
|
|
<origin xyz="0.061581 0 -0.040628" rpy="0 0 0" />
|
|
<geometry>
|
|
<box size="0.20791 0.07626 0.081257"/>
|
|
</geometry>
|
|
<material name="color"/>
|
|
</visual>
|
|
|
|
<collision>
|
|
<!--box origin wrt jRightAnkle-->
|
|
<origin xyz="0.061581 0 -0.040628" rpy="0 0 0" />
|
|
<geometry>
|
|
<box size="0.20791 0.07626 0.081257"/>
|
|
</geometry>
|
|
</collision>
|
|
|
|
</link>
|
|
<link name="RightToe">
|
|
<inertial>
|
|
<mass value="0.1068"/>
|
|
<!--COM origin wrt jRightBallFoot-->
|
|
<origin xyz="0.041044 0 0" rpy="0 0 0" />
|
|
<inertia ixx="5.4675e-05" iyy="6.289e-05" izz="0.00011173" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
|
|
<visual>
|
|
<!--box origin wrt jRightBallFoot-->
|
|
<origin xyz="0.041044 0 0" rpy="0 0 0" />
|
|
<geometry>
|
|
<box size="0.082089 0.07626 0.018102"/>
|
|
</geometry>
|
|
<material name="color"/>
|
|
</visual>
|
|
|
|
<collision>
|
|
<!--box origin wrt jRightBallFoot-->
|
|
<origin xyz="0.041044 0 0" rpy="0 0 0" />
|
|
<geometry>
|
|
<box size="0.082089 0.07626 0.018102"/>
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<!--Chain from (20) to (23)-->
|
|
<link name="LeftUpperLeg_f1">
|
|
<inertial>
|
|
<mass value="0"/>
|
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
<inertia ixx="0" iyy="0" izz="0" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
</link>
|
|
<link name="LeftUpperLeg_f2">
|
|
<inertial>
|
|
<mass value="0"/>
|
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
<inertia ixx="0" iyy="0" izz="0" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
</link>
|
|
<link name="LeftUpperLeg">
|
|
<inertial>
|
|
<mass value="8.9"/>
|
|
<!--COM origin wrt jLeftHip-->
|
|
<origin xyz="0 0 -0.26304" rpy="0 0 0" />
|
|
<inertia ixx="0.21503" iyy="0.21503" izz="0.019525" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
|
|
<visual>
|
|
<!--box origin wrt jLeftHip-->
|
|
<origin xyz="0 0 -0.26304" rpy="0 0 0" />
|
|
<geometry>
|
|
<cylinder length="0.52609" radius="0.066238"/>
|
|
</geometry>
|
|
<material name="color"/>
|
|
</visual>
|
|
|
|
<collision>
|
|
<!--box origin wrt jLeftHip-->
|
|
<origin xyz="0 0 -0.26304" rpy="0 0 0" />
|
|
<geometry>
|
|
<cylinder length="0.52609" radius="0.066238"/>
|
|
</geometry>
|
|
</collision>
|
|
|
|
</link>
|
|
<link name="LeftLowerLeg_f1">
|
|
<inertial>
|
|
<mass value="0"/>
|
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
<inertia ixx="0" iyy="0" izz="0" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
</link>
|
|
<link name="LeftLowerLeg">
|
|
<inertial>
|
|
<mass value="2.5988"/>
|
|
<!--COM origin wrt jLeftKnee-->
|
|
<origin xyz="0 0 -0.20614" rpy="0 0 0" />
|
|
<inertia ixx="0.037755" iyy="0.037755" izz="0.0018892" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
|
|
<visual>
|
|
<!--box origin wrt jLeftKnee-->
|
|
<origin xyz="0 0 -0.20614" rpy="0 0 0" />
|
|
<geometry>
|
|
<cylinder length="0.41228" radius="0.03813"/>
|
|
</geometry>
|
|
<material name="color"/>
|
|
</visual>
|
|
|
|
<collision>
|
|
<!--box origin wrt jLeftKnee-->
|
|
<origin xyz="0 0 -0.20614" rpy="0 0 0" />
|
|
<geometry>
|
|
<cylinder length="0.41228" radius="0.03813"/>
|
|
</geometry>
|
|
</collision>
|
|
|
|
</link>
|
|
<link name="LeftFoot_f1">
|
|
<inertial>
|
|
<mass value="0"/>
|
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
<inertia ixx="0" iyy="0" izz="0" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
</link>
|
|
<link name="LeftFoot_f2">
|
|
<inertial>
|
|
<mass value="0"/>
|
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
|
<inertia ixx="0" iyy="0" izz="0" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
</link>
|
|
<link name="LeftFoot">
|
|
<inertial>
|
|
<mass value="0.9256"/>
|
|
<!--COM origin wrt jLeftAnkle-->
|
|
<origin xyz="0.061581 0 -0.040628" rpy="0 0 0" />
|
|
<inertia ixx="0.00095786" iyy="0.0038434" izz="0.0037827" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
|
|
<visual>
|
|
<!--box origin wrt jLeftAnkle-->
|
|
<origin xyz="0.061581 0 -0.040628" rpy="0 0 0" />
|
|
<geometry>
|
|
<box size="0.20791 0.07626 0.081257"/>
|
|
</geometry>
|
|
<material name="color"/>
|
|
</visual>
|
|
|
|
<collision>
|
|
<!--box origin wrt jLeftAnkle-->
|
|
<origin xyz="0.061581 0 -0.040628" rpy="0 0 0" />
|
|
<geometry>
|
|
<box size="0.20791 0.07626 0.081257"/>
|
|
</geometry>
|
|
</collision>
|
|
|
|
</link>
|
|
<link name="LeftToe">
|
|
<inertial>
|
|
<mass value="0.1068"/>
|
|
<!--COM origin wrt jLeftBallFoot-->
|
|
<origin xyz="0.041044 0 0" rpy="0 0 0" />
|
|
<inertia ixx="5.4675e-05" iyy="6.289e-05" izz="0.00011173" ixy="0" ixz="0" iyz="0"/>
|
|
</inertial>
|
|
|
|
<visual>
|
|
<!--box origin wrt jLeftBallFoot-->
|
|
<origin xyz="0.041044 0 0" rpy="0 0 0" />
|
|
<geometry>
|
|
<box size="0.082089 0.07626 0.018102"/>
|
|
</geometry>
|
|
<material name="color"/>
|
|
</visual>
|
|
|
|
<collision>
|
|
<!--box origin wrt jLeftBallFoot-->
|
|
<origin xyz="0.041044 0 0" rpy="0 0 0" />
|
|
<geometry>
|
|
<box size="0.082089 0.07626 0.018102"/>
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
|
|
|
|
<!--JOINTS-->
|
|
<!--Chain from (2) to (7)-->
|
|
<joint name="jL5S1_rotx" type="revolute">
|
|
<origin xyz="-0.000575 0 0.11552" rpy="0 0 0"/>
|
|
<parent link="Pelvis"/>
|
|
<child link="L5_f1"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-0.610865" upper="0.610865" />
|
|
<axis xyz="1 0 0" />
|
|
</joint>
|
|
<joint name="jL5S1_roty" type="revolute">
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<parent link="L5_f1"/>
|
|
<child link="L5"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-0.523599" upper="1.309" />
|
|
<axis xyz="0 1 0" />
|
|
</joint>
|
|
<joint name="jL4L3_rotx" type="revolute">
|
|
<origin xyz="0.001044 0 0.089762" rpy="0 0 0"/>
|
|
<parent link="L5"/>
|
|
<child link="L3_f1"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-0.610865" upper="0.610865" />
|
|
<axis xyz="1 0 0" />
|
|
</joint>
|
|
<joint name="jL4L3_roty" type="revolute">
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<parent link="L3_f1"/>
|
|
<child link="L3"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-0.523599" upper="1.309" />
|
|
<axis xyz="0 1 0" />
|
|
</joint>
|
|
<joint name="jL1T12_rotx" type="revolute">
|
|
<origin xyz="0 0 0.081071" rpy="0 0 0"/>
|
|
<parent link="L3"/>
|
|
<child link="T12_f1"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-0.610865" upper="0.610865" />
|
|
<axis xyz="1 0 0" />
|
|
</joint>
|
|
<joint name="jL1T12_roty" type="revolute">
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<parent link="T12_f1"/>
|
|
<child link="T12"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-0.523599" upper="1.309" />
|
|
<axis xyz="0 1 0" />
|
|
</joint>
|
|
<joint name="jT9T8_rotx" type="revolute">
|
|
<origin xyz="0 0 0.081071" rpy="0 0 0"/>
|
|
<parent link="T12"/>
|
|
<child link="T8_f1"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-0.349066" upper="0.349066" />
|
|
<axis xyz="1 0 0" />
|
|
</joint>
|
|
<joint name="jT9T8_roty" type="revolute">
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<parent link="T8_f1"/>
|
|
<child link="T8_f2"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-0.261799" upper="0.698132" />
|
|
<axis xyz="0 1 0" />
|
|
</joint>
|
|
<joint name="jT9T8_rotz" type="revolute">
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<parent link="T8_f2"/>
|
|
<child link="T8"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-0.610865" upper="0.610865" />
|
|
<axis xyz="0 0 1" />
|
|
</joint>
|
|
<joint name="jT1C7_rotx" type="revolute">
|
|
<origin xyz="0 0 0.11612" rpy="0 0 0"/>
|
|
<parent link="T8"/>
|
|
<child link="Neck_f1"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-0.610865" upper="0.610865" />
|
|
<axis xyz="1 0 0" />
|
|
</joint>
|
|
<joint name="jT1C7_roty" type="revolute">
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<parent link="Neck_f1"/>
|
|
<child link="Neck_f2"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-0.959931" upper="1.5708" />
|
|
<axis xyz="0 1 0" />
|
|
</joint>
|
|
<joint name="jT1C7_rotz" type="revolute">
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<parent link="Neck_f2"/>
|
|
<child link="Neck"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-1.22173" upper="1.22173" />
|
|
<axis xyz="0 0 1" />
|
|
</joint>
|
|
<joint name="jC1Head_rotx" type="revolute">
|
|
<origin xyz="0.000829 0 0.096353" rpy="0 0 0"/>
|
|
<parent link="Neck"/>
|
|
<child link="Head_f1"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-0.610865" upper="0.610865" />
|
|
<axis xyz="1 0 0" />
|
|
</joint>
|
|
<joint name="jC1Head_roty" type="revolute">
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<parent link="Head_f1"/>
|
|
<child link="Head"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-0.436332" upper="0.174533" />
|
|
<axis xyz="0 1 0" />
|
|
</joint>
|
|
<!--Chain from (8) to (11)-->
|
|
<joint name="jRightC7Shoulder_rotx" type="revolute">
|
|
<origin xyz="0 -0.029713 0.065446" rpy="0 0 0"/>
|
|
<parent link="T8"/>
|
|
<child link="RightShoulder"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-0.785398" upper="0.0872665" />
|
|
<axis xyz="1 0 0" />
|
|
</joint>
|
|
<joint name="jRightShoulder_rotx" type="revolute">
|
|
<origin xyz="0 -0.15771 0" rpy="0 0 0"/>
|
|
<parent link="RightShoulder"/>
|
|
<child link="RightUpperArm_f1"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-2.35619" upper="1.5708" />
|
|
<axis xyz="1 0 0" />
|
|
</joint>
|
|
<joint name="jRightShoulder_roty" type="revolute">
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<parent link="RightUpperArm_f1"/>
|
|
<child link="RightUpperArm_f2"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-1.5708" upper="1.5708" />
|
|
<axis xyz="0 1 0" />
|
|
</joint>
|
|
<joint name="jRightShoulder_rotz" type="revolute">
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<parent link="RightUpperArm_f2"/>
|
|
<child link="RightUpperArm"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-0.785398" upper="3.14159" />
|
|
<axis xyz="0 0 1" />
|
|
</joint>
|
|
<joint name="jRightElbow_roty" type="revolute">
|
|
<origin xyz="0 -0.29147 0" rpy="0 0 0"/>
|
|
<parent link="RightUpperArm"/>
|
|
<child link="RightForeArm_f1"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-1.5708" upper="1.48353" />
|
|
<axis xyz="0 1 0" />
|
|
</joint>
|
|
<joint name="jRightElbow_rotz" type="revolute">
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<parent link="RightForeArm_f1"/>
|
|
<child link="RightForeArm"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="0" upper="2.53073" />
|
|
<axis xyz="0 0 1" />
|
|
</joint>
|
|
<joint name="jRightWrist_rotx" type="revolute">
|
|
<origin xyz="0.000102 -0.24094 0" rpy="0 0 0"/>
|
|
<parent link="RightForeArm"/>
|
|
<child link="RightHand_f1"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-0.872665" upper="1.0472" />
|
|
<axis xyz="1 0 0" />
|
|
</joint>
|
|
<joint name="jRightWrist_rotz" type="revolute">
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<parent link="RightHand_f1"/>
|
|
<child link="RightHand"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-0.523599" upper="0.349066" />
|
|
<axis xyz="0 0 1" />
|
|
</joint>
|
|
<!--Chain from (12) to (15)-->
|
|
<joint name="jLeftC7Shoulder_rotx" type="revolute">
|
|
<origin xyz="0 0.029713 0.065446" rpy="0 0 0"/>
|
|
<parent link="T8"/>
|
|
<child link="LeftShoulder"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-0.0872665" upper="0.785398" />
|
|
<axis xyz="1 0 0" />
|
|
</joint>
|
|
<joint name="jLeftShoulder_rotx" type="revolute">
|
|
<origin xyz="0 0.15771 0" rpy="0 0 0"/>
|
|
<parent link="LeftShoulder"/>
|
|
<child link="LeftUpperArm_f1"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-1.5708" upper="2.35619" />
|
|
<axis xyz="1 0 0" />
|
|
</joint>
|
|
<joint name="jLeftShoulder_roty" type="revolute">
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<parent link="LeftUpperArm_f1"/>
|
|
<child link="LeftUpperArm_f2"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-1.5708" upper="1.5708" />
|
|
<axis xyz="0 1 0" />
|
|
</joint>
|
|
<joint name="jLeftShoulder_rotz" type="revolute">
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<parent link="LeftUpperArm_f2"/>
|
|
<child link="LeftUpperArm"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-3.14159" upper="0.785398" />
|
|
<axis xyz="0 0 1" />
|
|
</joint>
|
|
<joint name="jLeftElbow_roty" type="revolute">
|
|
<origin xyz="0 0.29147 0" rpy="0 0 0"/>
|
|
<parent link="LeftUpperArm"/>
|
|
<child link="LeftForeArm_f1"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-1.5708" upper="1.48353" />
|
|
<axis xyz="0 1 0" />
|
|
</joint>
|
|
<joint name="jLeftElbow_rotz" type="revolute">
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<parent link="LeftForeArm_f1"/>
|
|
<child link="LeftForeArm"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-2.53073" upper="0" />
|
|
<axis xyz="0 0 1" />
|
|
</joint>
|
|
<joint name="jLeftWrist_rotx" type="revolute">
|
|
<origin xyz="0.000102 0.24094 0" rpy="0 0 0"/>
|
|
<parent link="LeftForeArm"/>
|
|
<child link="LeftHand_f1"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-1.0472" upper="0.872665" />
|
|
<axis xyz="1 0 0" />
|
|
</joint>
|
|
<joint name="jLeftWrist_rotz" type="revolute">
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<parent link="LeftHand_f1"/>
|
|
<child link="LeftHand"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-0.349066" upper="0.523599" />
|
|
<axis xyz="0 0 1" />
|
|
</joint>
|
|
<!--Chain from (16) to (19)-->
|
|
<joint name="jRightHip_rotx" type="revolute">
|
|
<origin xyz="0.000287 -0.098388 0.002817" rpy="0 0 0"/>
|
|
<parent link="Pelvis"/>
|
|
<child link="RightUpperLeg_f1"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-0.785398" upper="0.523599" />
|
|
<axis xyz="1 0 0" />
|
|
</joint>
|
|
<joint name="jRightHip_roty" type="revolute">
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<parent link="RightUpperLeg_f1"/>
|
|
<child link="RightUpperLeg_f2"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-0.261799" upper="2.0944" />
|
|
<axis xyz="0 1 0" />
|
|
</joint>
|
|
<joint name="jRightHip_rotz" type="revolute">
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<parent link="RightUpperLeg_f2"/>
|
|
<child link="RightUpperLeg"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-0.785398" upper="0.785398" />
|
|
<axis xyz="0 0 1" />
|
|
</joint>
|
|
<joint name="jRightKnee_roty" type="revolute">
|
|
<origin xyz="3e-05 0 -0.52609" rpy="0 0 0"/>
|
|
<parent link="RightUpperLeg"/>
|
|
<child link="RightLowerLeg_f1"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="0" upper="2.35619" />
|
|
<axis xyz="0 1 0" />
|
|
</joint>
|
|
<joint name="jRightKnee_rotz" type="revolute">
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<parent link="RightLowerLeg_f1"/>
|
|
<child link="RightLowerLeg"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-0.698132" upper="0.523599" />
|
|
<axis xyz="0 0 1" />
|
|
</joint>
|
|
<joint name="jRightAnkle_rotx" type="revolute">
|
|
<origin xyz="-0.0001 0 -0.41228" rpy="0 0 0"/>
|
|
<parent link="RightLowerLeg"/>
|
|
<child link="RightFoot_f1"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-0.610865" upper="0.785398" />
|
|
<axis xyz="1 0 0" />
|
|
</joint>
|
|
<joint name="jRightAnkle_roty" type="revolute">
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<parent link="RightFoot_f1"/>
|
|
<child link="RightFoot_f2"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-0.523599" upper="0.872665" />
|
|
<axis xyz="0 1 0" />
|
|
</joint>
|
|
<joint name="jRightAnkle_rotz" type="revolute">
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<parent link="RightFoot_f2"/>
|
|
<child link="RightFoot"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-0.436332" upper="0.872665" />
|
|
<axis xyz="0 0 1" />
|
|
</joint>
|
|
<joint name="jRightBallFoot_roty" type="revolute">
|
|
<origin xyz="0.16553 0 -0.067444" rpy="0 0 0"/>
|
|
<parent link="RightFoot"/>
|
|
<child link="RightToe"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-0.174533" upper="1.5708" />
|
|
<axis xyz="0 1 0" />
|
|
</joint>
|
|
<!--Chain from (20) to (23)-->
|
|
<joint name="jLeftHip_rotx" type="revolute">
|
|
<origin xyz="0.000287 0.098388 0.002817" rpy="0 0 0"/>
|
|
<parent link="Pelvis"/>
|
|
<child link="LeftUpperLeg_f1"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-0.523599" upper="0.785398" />
|
|
<axis xyz="1 0 0" />
|
|
</joint>
|
|
<joint name="jLeftHip_roty" type="revolute">
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<parent link="LeftUpperLeg_f1"/>
|
|
<child link="LeftUpperLeg_f2"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-0.261799" upper="2.0944" />
|
|
<axis xyz="0 1 0" />
|
|
</joint>
|
|
<joint name="jLeftHip_rotz" type="revolute">
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<parent link="LeftUpperLeg_f2"/>
|
|
<child link="LeftUpperLeg"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-0.785398" upper="0.785398" />
|
|
<axis xyz="0 0 1" />
|
|
</joint>
|
|
<joint name="jLeftKnee_roty" type="revolute">
|
|
<origin xyz="3e-05 0 -0.52609" rpy="0 0 0"/>
|
|
<parent link="LeftUpperLeg"/>
|
|
<child link="LeftLowerLeg_f1"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="0" upper="2.35619" />
|
|
<axis xyz="0 1 0" />
|
|
</joint>
|
|
<joint name="jLeftKnee_rotz" type="revolute">
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<parent link="LeftLowerLeg_f1"/>
|
|
<child link="LeftLowerLeg"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-0.523599" upper="0.698132" />
|
|
<axis xyz="0 0 1" />
|
|
</joint>
|
|
<joint name="jLeftAnkle_rotx" type="revolute">
|
|
<origin xyz="-0.0001 0 -0.41228" rpy="0 0 0"/>
|
|
<parent link="LeftLowerLeg"/>
|
|
<child link="LeftFoot_f1"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-0.785398" upper="0.610865" />
|
|
<axis xyz="1 0 0" />
|
|
</joint>
|
|
<joint name="jLeftAnkle_roty" type="revolute">
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<parent link="LeftFoot_f1"/>
|
|
<child link="LeftFoot_f2"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-0.523599" upper="0.872665" />
|
|
<axis xyz="0 1 0" />
|
|
</joint>
|
|
<joint name="jLeftAnkle_rotz" type="revolute">
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<parent link="LeftFoot_f2"/>
|
|
<child link="LeftFoot"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-0.872665" upper="0.436332" />
|
|
<axis xyz="0 0 1" />
|
|
</joint>
|
|
<joint name="jLeftBallFoot_roty" type="revolute">
|
|
<origin xyz="0.16553 0 -0.067444" rpy="0 0 0"/>
|
|
<parent link="LeftFoot"/>
|
|
<child link="LeftToe"/>
|
|
<dynamics damping="0.1" friction="0.0"/>
|
|
<limit effort="30" velocity="1.0" lower="-0.174533" upper="1.5708" />
|
|
<axis xyz="0 1 0" />
|
|
</joint>
|
|
|
|
<!-- Sensor 1-->
|
|
<sensor name="Pelvis_gyro" type="gyroscope">
|
|
<parent link="Pelvis"/>
|
|
<origin xyz="-0.068488 -3.8658e-07 0.11696" rpy="-0.27051 1.3344 2.7093"/>
|
|
</sensor>
|
|
<sensor name="Pelvis_accelerometer" type="accelerometer">
|
|
<parent link="Pelvis"/>
|
|
<origin xyz="-0.068488 -3.8658e-07 0.11696" rpy="-0.27051 1.3344 2.7093"/>
|
|
</sensor>
|
|
<!-- Sensor 2-->
|
|
<sensor name="T8_gyro" type="gyroscope">
|
|
<parent link="T8"/>
|
|
<origin xyz="0.12929 8.9756e-08 0.071108" rpy="0.38721 1.2683 0.24211"/>
|
|
</sensor>
|
|
<sensor name="T8_accelerometer" type="accelerometer">
|
|
<parent link="T8"/>
|
|
<origin xyz="0.12929 8.9756e-08 0.071108" rpy="0.38721 1.2683 0.24211"/>
|
|
</sensor>
|
|
<!-- Sensor 3-->
|
|
<sensor name="Head_gyro" type="gyroscope">
|
|
<parent link="Head"/>
|
|
<origin xyz="-0.077436 -1.1223e-07 0.085116" rpy="-2.4386 -0.21231 1.6195"/>
|
|
</sensor>
|
|
<sensor name="Head_accelerometer" type="accelerometer">
|
|
<parent link="Head"/>
|
|
<origin xyz="-0.077436 -1.1223e-07 0.085116" rpy="-2.4386 -0.21231 1.6195"/>
|
|
</sensor>
|
|
<!-- Sensor 4-->
|
|
<sensor name="RightShoulder_gyro" type="gyroscope">
|
|
<parent link="RightShoulder"/>
|
|
<origin xyz="-0.022421 -0.056053 -0.067264" rpy="1.2409 -0.251 -1.4752"/>
|
|
</sensor>
|
|
<sensor name="RightShoulder_accelerometer" type="accelerometer">
|
|
<parent link="RightShoulder"/>
|
|
<origin xyz="-0.022421 -0.056053 -0.067264" rpy="1.2409 -0.251 -1.4752"/>
|
|
</sensor>
|
|
<!-- Sensor 5-->
|
|
<sensor name="RightUpperArm_gyro" type="gyroscope">
|
|
<parent link="RightUpperArm"/>
|
|
<origin xyz="-0.019597 -0.097988 0.019598" rpy="-0.023852 0.034416 -1.4836"/>
|
|
</sensor>
|
|
<sensor name="RightUpperArm_accelerometer" type="accelerometer">
|
|
<parent link="RightUpperArm"/>
|
|
<origin xyz="-0.019597 -0.097988 0.019598" rpy="-0.023852 0.034416 -1.4836"/>
|
|
</sensor>
|
|
<!-- Sensor 6-->
|
|
<sensor name="RightForeArm_gyro" type="gyroscope">
|
|
<parent link="RightForeArm"/>
|
|
<origin xyz="0.0018228 -0.1988 0.019855" rpy="-0.23839 0.042417 -1.238"/>
|
|
</sensor>
|
|
<sensor name="RightForeArm_accelerometer" type="accelerometer">
|
|
<parent link="RightForeArm"/>
|
|
<origin xyz="0.0018228 -0.1988 0.019855" rpy="-0.23839 0.042417 -1.238"/>
|
|
</sensor>
|
|
<!-- Sensor 7-->
|
|
<sensor name="RightHand_gyro" type="gyroscope">
|
|
<parent link="RightHand"/>
|
|
<origin xyz="3.4392e-08 -0.055431 0.020157" rpy="-0.1235 0.039699 -1.3751"/>
|
|
</sensor>
|
|
<sensor name="RightHand_accelerometer" type="accelerometer">
|
|
<parent link="RightHand"/>
|
|
<origin xyz="3.4392e-08 -0.055431 0.020157" rpy="-0.1235 0.039699 -1.3751"/>
|
|
</sensor>
|
|
<!-- Sensor 8-->
|
|
<sensor name="LeftShoulder_gyro" type="gyroscope">
|
|
<parent link="LeftShoulder"/>
|
|
<origin xyz="-0.022421 0.056053 -0.067264" rpy="-1.3117 -0.3446 1.3956"/>
|
|
</sensor>
|
|
<sensor name="LeftShoulder_accelerometer" type="accelerometer">
|
|
<parent link="LeftShoulder"/>
|
|
<origin xyz="-0.022421 0.056053 -0.067264" rpy="-1.3117 -0.3446 1.3956"/>
|
|
</sensor>
|
|
<!-- Sensor 9-->
|
|
<sensor name="LeftUpperArm_gyro" type="gyroscope">
|
|
<parent link="LeftUpperArm"/>
|
|
<origin xyz="-0.019597 0.097989 0.019598" rpy="0.10179 0.025936 1.4178"/>
|
|
</sensor>
|
|
<sensor name="LeftUpperArm_accelerometer" type="accelerometer">
|
|
<parent link="LeftUpperArm"/>
|
|
<origin xyz="-0.019597 0.097989 0.019598" rpy="0.10179 0.025936 1.4178"/>
|
|
</sensor>
|
|
<!-- Sensor 10-->
|
|
<sensor name="LeftForeArm_gyro" type="gyroscope">
|
|
<parent link="LeftForeArm"/>
|
|
<origin xyz="0.0018196 0.1988 0.019876" rpy="0.33797 -0.011593 1.2119"/>
|
|
</sensor>
|
|
<sensor name="LeftForeArm_accelerometer" type="accelerometer">
|
|
<parent link="LeftForeArm"/>
|
|
<origin xyz="0.0018196 0.1988 0.019876" rpy="0.33797 -0.011593 1.2119"/>
|
|
</sensor>
|
|
<!-- Sensor 11-->
|
|
<sensor name="LeftHand_gyro" type="gyroscope">
|
|
<parent link="LeftHand"/>
|
|
<origin xyz="1.4957e-06 0.055432 0.020158" rpy="0.20961 0.078707 1.2361"/>
|
|
</sensor>
|
|
<sensor name="LeftHand_accelerometer" type="accelerometer">
|
|
<parent link="LeftHand"/>
|
|
<origin xyz="1.4957e-06 0.055432 0.020158" rpy="0.20961 0.078707 1.2361"/>
|
|
</sensor>
|
|
<!-- Sensor 12-->
|
|
<sensor name="RightUpperLeg_gyro" type="gyroscope">
|
|
<parent link="RightUpperLeg"/>
|
|
<origin xyz="0.015207 -0.053543 -0.31632" rpy="2.7056 1.3522 1.1266"/>
|
|
</sensor>
|
|
<sensor name="RightUpperLeg_accelerometer" type="accelerometer">
|
|
<parent link="RightUpperLeg"/>
|
|
<origin xyz="0.015207 -0.053543 -0.31632" rpy="2.7056 1.3522 1.1266"/>
|
|
</sensor>
|
|
<!-- Sensor 13-->
|
|
<sensor name="RightLowerLeg_gyro" type="gyroscope">
|
|
<parent link="RightLowerLeg"/>
|
|
<origin xyz="0.031416 0.0071913 -0.13516" rpy="-3.0013 1.5099 -2.862"/>
|
|
</sensor>
|
|
<sensor name="RightLowerLeg_accelerometer" type="accelerometer">
|
|
<parent link="RightLowerLeg"/>
|
|
<origin xyz="0.031416 0.0071913 -0.13516" rpy="-3.0013 1.5099 -2.862"/>
|
|
</sensor>
|
|
<!-- Sensor 14-->
|
|
<sensor name="RightFoot_gyro" type="gyroscope">
|
|
<parent link="RightFoot"/>
|
|
<origin xyz="0.094314 0.0015696 -0.013315" rpy="0.19638 0.42312 0.033114"/>
|
|
</sensor>
|
|
<sensor name="RightFoot_accelerometer" type="accelerometer">
|
|
<parent link="RightFoot"/>
|
|
<origin xyz="0.094314 0.0015696 -0.013315" rpy="0.19638 0.42312 0.033114"/>
|
|
</sensor>
|
|
<!-- Sensor 15-->
|
|
<sensor name="LeftUpperLeg_gyro" type="gyroscope">
|
|
<parent link="LeftUpperLeg"/>
|
|
<origin xyz="0.015207 0.053543 -0.31632" rpy="2.7741 1.4429 -1.9595"/>
|
|
</sensor>
|
|
<sensor name="LeftUpperLeg_accelerometer" type="accelerometer">
|
|
<parent link="LeftUpperLeg"/>
|
|
<origin xyz="0.015207 0.053543 -0.31632" rpy="2.7741 1.4429 -1.9595"/>
|
|
</sensor>
|
|
<!-- Sensor 16-->
|
|
<sensor name="LeftLowerLeg_gyro" type="gyroscope">
|
|
<parent link="LeftLowerLeg"/>
|
|
<origin xyz="0.031416 -0.0071914 -0.13516" rpy="2.5557 1.4555 2.3984"/>
|
|
</sensor>
|
|
<sensor name="LeftLowerLeg_accelerometer" type="accelerometer">
|
|
<parent link="LeftLowerLeg"/>
|
|
<origin xyz="0.031416 -0.0071914 -0.13516" rpy="2.5557 1.4555 2.3984"/>
|
|
</sensor>
|
|
<!-- Sensor 17-->
|
|
<sensor name="LeftFoot_gyro" type="gyroscope">
|
|
<parent link="LeftFoot"/>
|
|
<origin xyz="0.094314 -0.0015696 -0.013315" rpy="-0.36816 0.44411 0.22503"/>
|
|
</sensor>
|
|
<sensor name="LeftFoot_accelerometer" type="accelerometer">
|
|
<parent link="LeftFoot"/>
|
|
<origin xyz="0.094314 -0.0015696 -0.013315" rpy="-0.36816 0.44411 0.22503"/>
|
|
</sensor>
|
|
|
|
|
|
</robot> |