650 lines
22 KiB
XML
650 lines
22 KiB
XML
<robot name="g1_dual_arm">
|
|
<mujoco>
|
|
<compiler meshdir="meshes" discardvisual="false"/>
|
|
</mujoco>
|
|
|
|
<!-- [CAUTION] uncomment when convert to mujoco -->
|
|
<!-- <link name="world"></link>
|
|
<joint name="floating_base_joint" type="floating">
|
|
<parent link="world"/>
|
|
<child link="waist_yaw_link"/>
|
|
</joint> -->
|
|
|
|
<!-- Torso -->
|
|
<link name="waist_yaw_link">
|
|
<inertial>
|
|
<origin xyz="0.003964 0 0.018769" rpy="0 0 0"/>
|
|
<mass value="0.244"/>
|
|
<inertia ixx="9.9587E-05" ixy="-1.833E-06" ixz="-1.2617E-05" iyy="0.00012411" iyz="-1.18E-07" izz="0.00015586"/>
|
|
</inertial>
|
|
<visual>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="meshes/waist_yaw_link.STL"/>
|
|
</geometry>
|
|
<material name="white">
|
|
<color rgba="0.7 0.7 0.7 1"/>
|
|
</material>
|
|
</visual>
|
|
</link>
|
|
<link name="waist_roll_link">
|
|
<inertial>
|
|
<origin xyz="0 -0.000236 0.010111" rpy="0 0 0"/>
|
|
<mass value="0.047"/>
|
|
<inertia ixx="7.515E-06" ixy="0" ixz="0" iyy="6.398E-06" iyz="9.9E-08" izz="3.988E-06"/>
|
|
</inertial>
|
|
<visual>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="meshes/waist_roll_link.STL"/>
|
|
</geometry>
|
|
<material name="white">
|
|
<color rgba="0.7 0.7 0.7 1"/>
|
|
</material>
|
|
</visual>
|
|
</link>
|
|
<joint name="waist_roll_joint" type="fixed">
|
|
<origin xyz="-0.0039635 0 0.035" rpy="0 0 0"/>
|
|
<parent link="waist_yaw_link"/>
|
|
<child link="waist_roll_link"/>
|
|
<axis xyz="1 0 0"/>
|
|
<limit lower="-0.52" upper="0.52" effort="50" velocity="37"/>
|
|
</joint>
|
|
<link name="torso_link">
|
|
<inertial>
|
|
<origin xyz="0.002601 0.000257 0.153719" rpy="0 0 0"/>
|
|
<mass value="8.562"/>
|
|
<inertia ixx="0.065674966" ixy="-8.597E-05" ixz="-0.001737252" iyy="0.053535188" iyz="8.6899E-05" izz="0.030808125"/>
|
|
</inertial>
|
|
<visual>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="meshes/torso_link.STL"/>
|
|
</geometry>
|
|
<material name="white">
|
|
<color rgba="0.7 0.7 0.7 1"/>
|
|
</material>
|
|
</visual>
|
|
<collision>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="meshes/torso_link.STL"/>
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<joint name="waist_pitch_joint" type="fixed">
|
|
<origin xyz="0 0 0.019" rpy="0 0 0"/>
|
|
<parent link="waist_roll_link"/>
|
|
<child link="torso_link"/>
|
|
<axis xyz="0 1 0"/>
|
|
<limit lower="-0.52" upper="0.52" effort="50" velocity="37"/>
|
|
</joint>
|
|
|
|
<!-- LOGO -->
|
|
<joint name="logo_joint" type="fixed">
|
|
<origin xyz="0.0039635 0 -0.054" rpy="0 0 0"/>
|
|
<parent link="torso_link"/>
|
|
<child link="logo_link"/>
|
|
</joint>
|
|
<link name="logo_link">
|
|
<inertial>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<mass value="0.001"/>
|
|
<inertia ixx="1e-7" ixy="0" ixz="0" iyy="1e-7" iyz="0" izz="1e-7"/>
|
|
</inertial>
|
|
<visual>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="meshes/logo_link.STL"/>
|
|
</geometry>
|
|
<material name="dark">
|
|
<color rgba="0.2 0.2 0.2 1"/>
|
|
</material>
|
|
</visual>
|
|
<collision>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="meshes/logo_link.STL"/>
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
|
|
<!-- Head -->
|
|
<link name="head_link">
|
|
<inertial>
|
|
<origin xyz="0.005267 0.000299 0.449869" rpy="0 0 0"/>
|
|
<mass value="1.036"/>
|
|
<inertia ixx="0.004085051" ixy="-2.543E-06" ixz="-6.9455E-05" iyy="0.004185212" iyz="-3.726E-06" izz="0.001807911"/>
|
|
</inertial>
|
|
<visual>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="meshes/head_link.STL"/>
|
|
</geometry>
|
|
<material name="dark">
|
|
<color rgba="0.2 0.2 0.2 1"/>
|
|
</material>
|
|
</visual>
|
|
<collision>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="meshes/head_link.STL"/>
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<joint name="head_joint" type="fixed">
|
|
<origin xyz="0.0039635 0 -0.054" rpy="0 0 0"/>
|
|
<parent link="torso_link"/>
|
|
<child link="head_link"/>
|
|
</joint>
|
|
|
|
<!-- Waist Support -->
|
|
<link name="waist_support_link">
|
|
<inertial>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<mass value="0.001"/>
|
|
<inertia ixx="1e-7" ixy="0" ixz="0" iyy="1e-7" iyz="0" izz="1e-7"/>
|
|
</inertial>
|
|
<visual>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="meshes/waist_support_link.STL"/>
|
|
</geometry>
|
|
<material name="white">
|
|
<color rgba="0.7 0.7 0.7 1"/>
|
|
</material>
|
|
</visual>
|
|
<collision>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="meshes/waist_support_link.STL"/>
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<joint name="waist_support_joint" type="fixed">
|
|
<origin xyz="0.0039635 0 -0.054" rpy="0 0 0"/>
|
|
<parent link="torso_link"/>
|
|
<child link="waist_support_link"/>
|
|
</joint>
|
|
|
|
<!-- IMU -->
|
|
<link name="imu_in_torso"></link>
|
|
<joint name="imu_in_torso_joint" type="fixed">
|
|
<origin xyz="-0.03959 -0.00224 0.13792" rpy="0 0 0"/>
|
|
<parent link="torso_link"/>
|
|
<child link="imu_in_torso"/>
|
|
</joint>
|
|
|
|
<link name="imu_in_pelvis"></link>
|
|
<joint name="imu_in_pelvis_joint" type="fixed">
|
|
<origin xyz="0.04525 0 -0.08339" rpy="0 0 0"/>
|
|
<parent link="pelvis"/>
|
|
<child link="imu_in_pelvis"/>
|
|
</joint>
|
|
|
|
<!-- d435 -->
|
|
<link name="d435_link"></link>
|
|
<joint name="d435_joint" type="fixed">
|
|
<origin xyz="0.0576235 0.01753 0.41987" rpy="0 0.8307767239493009 0"/>
|
|
<parent link="torso_link"/>
|
|
<child link="d435_link"/>
|
|
</joint>
|
|
|
|
<!-- mid360 -->
|
|
<link name="mid360_link"></link>
|
|
<joint name="mid360_joint" type="fixed">
|
|
<origin xyz="0.0002835 0.00003 0.40618" rpy="0 0.04014257279586953 0"/>
|
|
<parent link="torso_link"/>
|
|
<child link="mid360_link"/>
|
|
</joint>
|
|
|
|
<!-- Arm -->
|
|
<link name="left_shoulder_pitch_link">
|
|
<inertial>
|
|
<origin xyz="0 0.035892 -0.011628" rpy="0 0 0"/>
|
|
<mass value="0.718"/>
|
|
<inertia ixx="0.0004291" ixy="-9.2E-06" ixz="6.4E-06" iyy="0.000453" iyz="2.26E-05" izz="0.000423"/>
|
|
</inertial>
|
|
<visual>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="meshes/left_shoulder_pitch_link.STL"/>
|
|
</geometry>
|
|
<material name="white">
|
|
<color rgba="0.7 0.7 0.7 1"/>
|
|
</material>
|
|
</visual>
|
|
<collision>
|
|
<origin xyz="0 0.04 -0.01" rpy="0 1.5707963267948966 0"/>
|
|
<geometry>
|
|
<cylinder radius="0.03" length="0.05"/>
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<joint name="left_shoulder_pitch_joint" type="revolute">
|
|
<origin xyz="0.0039563 0.10022 0.23778" rpy="0.27931 5.4949E-05 -0.00019159"/>
|
|
<parent link="torso_link"/>
|
|
<child link="left_shoulder_pitch_link"/>
|
|
<axis xyz="0 1 0"/>
|
|
<limit lower="-3.0892" upper="2.6704" effort="25" velocity="37"/>
|
|
</joint>
|
|
<link name="left_shoulder_roll_link">
|
|
<inertial>
|
|
<origin xyz="-0.000227 0.00727 -0.063243" rpy="0 0 0"/>
|
|
<mass value="0.643"/>
|
|
<inertia ixx="0.0006177" ixy="-1E-06" ixz="8.7E-06" iyy="0.0006912" iyz="-5.3E-06" izz="0.0003894"/>
|
|
</inertial>
|
|
<visual>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="meshes/left_shoulder_roll_link.STL"/>
|
|
</geometry>
|
|
<material name="white">
|
|
<color rgba="0.7 0.7 0.7 1"/>
|
|
</material>
|
|
</visual>
|
|
<collision>
|
|
<origin xyz="-0.004 0.006 -0.053" rpy="0 0 0"/>
|
|
<geometry>
|
|
<cylinder radius="0.03" length="0.03"/>
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<joint name="left_shoulder_roll_joint" type="revolute">
|
|
<origin xyz="0 0.038 -0.013831" rpy="-0.27925 0 0"/>
|
|
<parent link="left_shoulder_pitch_link"/>
|
|
<child link="left_shoulder_roll_link"/>
|
|
<axis xyz="1 0 0"/>
|
|
<limit lower="-1.5882" upper="2.2515" effort="25" velocity="37"/>
|
|
</joint>
|
|
<link name="left_shoulder_yaw_link">
|
|
<inertial>
|
|
<origin xyz="0.010773 -0.002949 -0.072009" rpy="0 0 0"/>
|
|
<mass value="0.734"/>
|
|
<inertia ixx="0.0009988" ixy="7.9E-06" ixz="0.0001412" iyy="0.0010605" iyz="-2.86E-05" izz="0.0004354"/>
|
|
</inertial>
|
|
<visual>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="meshes/left_shoulder_yaw_link.STL"/>
|
|
</geometry>
|
|
<material name="white">
|
|
<color rgba="0.7 0.7 0.7 1"/>
|
|
</material>
|
|
</visual>
|
|
<collision>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="meshes/left_shoulder_yaw_link.STL"/>
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<joint name="left_shoulder_yaw_joint" type="revolute">
|
|
<origin xyz="0 0.00624 -0.1032" rpy="0 0 0"/>
|
|
<parent link="left_shoulder_roll_link"/>
|
|
<child link="left_shoulder_yaw_link"/>
|
|
<axis xyz="0 0 1"/>
|
|
<limit lower="-2.618" upper="2.618" effort="25" velocity="37"/>
|
|
</joint>
|
|
<link name="left_elbow_link">
|
|
<inertial>
|
|
<origin xyz="0.064956 0.004454 -0.010062" rpy="0 0 0"/>
|
|
<mass value="0.6"/>
|
|
<inertia ixx="0.0002891" ixy="6.53E-05" ixz="1.72E-05" iyy="0.0004152" iyz="-5.6E-06" izz="0.0004197"/>
|
|
</inertial>
|
|
<visual>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="meshes/left_elbow_link.STL"/>
|
|
</geometry>
|
|
<material name="white">
|
|
<color rgba="0.7 0.7 0.7 1"/>
|
|
</material>
|
|
</visual>
|
|
<collision>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="meshes/left_elbow_link.STL"/>
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<joint name="left_elbow_joint" type="revolute">
|
|
<origin xyz="0.015783 0 -0.080518" rpy="0 0 0"/>
|
|
<parent link="left_shoulder_yaw_link"/>
|
|
<child link="left_elbow_link"/>
|
|
<axis xyz="0 1 0"/>
|
|
<limit lower="-1.0472" upper="2.0944" effort="25" velocity="37"/>
|
|
</joint>
|
|
<joint name="left_wrist_roll_joint" type="revolute">
|
|
<origin xyz="0.100 0.00188791 -0.010" rpy="0 0 0"/>
|
|
<axis xyz="1 0 0"/>
|
|
<parent link="left_elbow_link"/>
|
|
<child link="left_wrist_roll_link"/>
|
|
<limit effort="25" velocity="37" lower="-1.972222054" upper="1.972222054"/>
|
|
</joint>
|
|
<link name="left_wrist_roll_link">
|
|
<inertial>
|
|
<origin xyz="0.01713944778 0.00053759094 0.00000048864" rpy="0 0 0"/>
|
|
<mass value="0.08544498"/>
|
|
<inertia ixx="0.00004821544023" ixy="-0.00000424511021" ixz="0.00000000510599" iyy="0.00003722899093" iyz="-0.00000000123525" izz="0.00005482106541"/>
|
|
</inertial>
|
|
<visual>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="meshes/left_wrist_roll_link.STL"/>
|
|
</geometry>
|
|
<material name="white">
|
|
<color rgba="0.7 0.7 0.7 1"/>
|
|
</material>
|
|
</visual>
|
|
<collision>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="meshes/left_wrist_roll_link.STL"/>
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<joint name="left_wrist_pitch_joint" type="revolute">
|
|
<origin xyz="0.038 0 0" rpy="0 0 0"/>
|
|
<axis xyz="0 1 0"/>
|
|
<parent link="left_wrist_roll_link"/>
|
|
<child link="left_wrist_pitch_link"/>
|
|
<limit effort="5" velocity="22" lower="-1.614429558" upper="1.614429558"/>
|
|
</joint>
|
|
<link name="left_wrist_pitch_link">
|
|
<inertial>
|
|
<origin xyz="0.02299989837 -0.00111685314 -0.00111658096" rpy="0 0 0"/>
|
|
<mass value="0.48404956"/>
|
|
<inertia ixx="0.00016579646273" ixy="-0.00001231206746" ixz="0.00001231699194" iyy="0.00042954057410" iyz="0.00000081417712" izz="0.00042953697654"/>
|
|
</inertial>
|
|
<visual>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="meshes/left_wrist_pitch_link.STL"/>
|
|
</geometry>
|
|
<material name="white">
|
|
<color rgba="0.7 0.7 0.7 1"/>
|
|
</material>
|
|
</visual>
|
|
<collision>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="meshes/left_wrist_pitch_link.STL"/>
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<joint name="left_wrist_yaw_joint" type="revolute">
|
|
<origin xyz="0.046 0 0" rpy="0 0 0"/>
|
|
<axis xyz="0 0 1"/>
|
|
<parent link="left_wrist_pitch_link"/>
|
|
<child link="left_wrist_yaw_link"/>
|
|
<limit effort="5" velocity="22" lower="-1.614429558" upper="1.614429558"/>
|
|
</joint>
|
|
<link name="left_wrist_yaw_link">
|
|
<inertial>
|
|
<origin xyz="0.02200381568 0.00049485096 0.00053861123" rpy="0 0 0"/>
|
|
<mass value="0.08457647"/>
|
|
<inertia ixx="0.00004929128828" ixy="-0.00000045735494" ixz="0.00000445867591" iyy="0.00005973338134" iyz="0.00000043217198" izz="0.00003928083826"/>
|
|
</inertial>
|
|
<visual>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="meshes/left_wrist_yaw_link.STL"/>
|
|
</geometry>
|
|
<material name="white">
|
|
<color rgba="0.7 0.7 0.7 1"/>
|
|
</material>
|
|
</visual>
|
|
<collision>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="meshes/left_wrist_yaw_link.STL"/>
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<joint name="left_hand_palm_joint" type="fixed">
|
|
<origin xyz="0.0415 0.003 0" rpy="0 0 0"/>
|
|
<parent link="left_wrist_yaw_link"/>
|
|
<child link="left_rubber_hand"/>
|
|
</joint>
|
|
<link name="left_rubber_hand">
|
|
<inertial>
|
|
<origin xyz="0.05361310808 -0.00295905240 0.00215413091" rpy="0 0 0"/>
|
|
<mass value="0.170"/>
|
|
<inertia ixx="0.00010099485234748" ixy="0.00003618590790516" ixz="-0.00000074301518642" iyy="0.00028135871571621" iyz="0.00000330189743286" izz="0.00021894770413514"/>
|
|
</inertial>
|
|
<visual>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="meshes/left_rubber_hand.STL"/>
|
|
</geometry>
|
|
<material name="white">
|
|
<color rgba="0.7 0.7 0.7 1"/>
|
|
</material>
|
|
</visual>
|
|
</link>
|
|
<link name="right_shoulder_pitch_link">
|
|
<inertial>
|
|
<origin xyz="0 -0.035892 -0.011628" rpy="0 0 0"/>
|
|
<mass value="0.718"/>
|
|
<inertia ixx="0.0004291" ixy="9.2E-06" ixz="6.4E-06" iyy="0.000453" iyz="-2.26E-05" izz="0.000423"/>
|
|
</inertial>
|
|
<visual>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="meshes/right_shoulder_pitch_link.STL"/>
|
|
</geometry>
|
|
<material name="white">
|
|
<color rgba="0.7 0.7 0.7 1"/>
|
|
</material>
|
|
</visual>
|
|
<collision>
|
|
<origin xyz="0 -0.04 -0.01" rpy="0 1.5707963267948966 0"/>
|
|
<geometry>
|
|
<cylinder radius="0.03" length="0.05"/>
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<joint name="right_shoulder_pitch_joint" type="revolute">
|
|
<origin xyz="0.0039563 -0.10021 0.23778" rpy="-0.27931 5.4949E-05 0.00019159"/>
|
|
<parent link="torso_link"/>
|
|
<child link="right_shoulder_pitch_link"/>
|
|
<axis xyz="0 1 0"/>
|
|
<limit lower="-3.0892" upper="2.6704" effort="25" velocity="37"/>
|
|
</joint>
|
|
<link name="right_shoulder_roll_link">
|
|
<inertial>
|
|
<origin xyz="-0.000227 -0.00727 -0.063243" rpy="0 0 0"/>
|
|
<mass value="0.643"/>
|
|
<inertia ixx="0.0006177" ixy="1E-06" ixz="8.7E-06" iyy="0.0006912" iyz="5.3E-06" izz="0.0003894"/>
|
|
</inertial>
|
|
<visual>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="meshes/right_shoulder_roll_link.STL"/>
|
|
</geometry>
|
|
<material name="white">
|
|
<color rgba="0.7 0.7 0.7 1"/>
|
|
</material>
|
|
</visual>
|
|
<collision>
|
|
<origin xyz="-0.004 -0.006 -0.053" rpy="0 0 0"/>
|
|
<geometry>
|
|
<cylinder radius="0.03" length="0.03"/>
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<joint name="right_shoulder_roll_joint" type="revolute">
|
|
<origin xyz="0 -0.038 -0.013831" rpy="0.27925 0 0"/>
|
|
<parent link="right_shoulder_pitch_link"/>
|
|
<child link="right_shoulder_roll_link"/>
|
|
<axis xyz="1 0 0"/>
|
|
<limit lower="-2.2515" upper="1.5882" effort="25" velocity="37"/>
|
|
</joint>
|
|
<link name="right_shoulder_yaw_link">
|
|
<inertial>
|
|
<origin xyz="0.010773 0.002949 -0.072009" rpy="0 0 0"/>
|
|
<mass value="0.734"/>
|
|
<inertia ixx="0.0009988" ixy="-7.9E-06" ixz="0.0001412" iyy="0.0010605" iyz="2.86E-05" izz="0.0004354"/>
|
|
</inertial>
|
|
<visual>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="meshes/right_shoulder_yaw_link.STL"/>
|
|
</geometry>
|
|
<material name="white">
|
|
<color rgba="0.7 0.7 0.7 1"/>
|
|
</material>
|
|
</visual>
|
|
<collision>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="meshes/right_shoulder_yaw_link.STL"/>
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<joint name="right_shoulder_yaw_joint" type="revolute">
|
|
<origin xyz="0 -0.00624 -0.1032" rpy="0 0 0"/>
|
|
<parent link="right_shoulder_roll_link"/>
|
|
<child link="right_shoulder_yaw_link"/>
|
|
<axis xyz="0 0 1"/>
|
|
<limit lower="-2.618" upper="2.618" effort="25" velocity="37"/>
|
|
</joint>
|
|
<link name="right_elbow_link">
|
|
<inertial>
|
|
<origin xyz="0.064956 -0.004454 -0.010062" rpy="0 0 0"/>
|
|
<mass value="0.6"/>
|
|
<inertia ixx="0.0002891" ixy="-6.53E-05" ixz="1.72E-05" iyy="0.0004152" iyz="5.6E-06" izz="0.0004197"/>
|
|
</inertial>
|
|
<visual>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="meshes/right_elbow_link.STL"/>
|
|
</geometry>
|
|
<material name="white">
|
|
<color rgba="0.7 0.7 0.7 1"/>
|
|
</material>
|
|
</visual>
|
|
<collision>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="meshes/right_elbow_link.STL"/>
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<joint name="right_elbow_joint" type="revolute">
|
|
<origin xyz="0.015783 0 -0.080518" rpy="0 0 0"/>
|
|
<parent link="right_shoulder_yaw_link"/>
|
|
<child link="right_elbow_link"/>
|
|
<axis xyz="0 1 0"/>
|
|
<limit lower="-1.0472" upper="2.0944" effort="25" velocity="37"/>
|
|
</joint>
|
|
<joint name="right_wrist_roll_joint" type="revolute">
|
|
<origin xyz="0.100 -0.00188791 -0.010" rpy="0 0 0"/>
|
|
<axis xyz="1 0 0"/>
|
|
<parent link="right_elbow_link"/>
|
|
<child link="right_wrist_roll_link"/>
|
|
<limit effort="25" velocity="37" lower="-1.972222054" upper="1.972222054"/>
|
|
</joint>
|
|
<link name="right_wrist_roll_link">
|
|
<inertial>
|
|
<origin xyz="0.01713944778 -0.00053759094 0.00000048864" rpy="0 0 0"/>
|
|
<mass value="0.08544498"/>
|
|
<inertia ixx="0.00004821544023" ixy="0.00000424511021" ixz="0.00000000510599" iyy="0.00003722899093" iyz="0.00000000123525" izz="0.00005482106541"/>
|
|
</inertial>
|
|
<visual>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="meshes/right_wrist_roll_link.STL"/>
|
|
</geometry>
|
|
<material name="white">
|
|
<color rgba="0.7 0.7 0.7 1"/>
|
|
</material>
|
|
</visual>
|
|
<collision>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="meshes/right_wrist_roll_link.STL"/>
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<joint name="right_wrist_pitch_joint" type="revolute">
|
|
<origin xyz="0.038 0 0" rpy="0 0 0"/>
|
|
<axis xyz="0 1 0"/>
|
|
<parent link="right_wrist_roll_link"/>
|
|
<child link="right_wrist_pitch_link"/>
|
|
<limit effort="5" velocity="22" lower="-1.614429558" upper="1.614429558"/>
|
|
</joint>
|
|
<link name="right_wrist_pitch_link">
|
|
<inertial>
|
|
<origin xyz="0.02299989837 0.00111685314 -0.00111658096" rpy="0 0 0"/>
|
|
<mass value="0.48404956"/>
|
|
<inertia ixx="0.00016579646273" ixy="0.00001231206746" ixz="0.00001231699194" iyy="0.00042954057410" iyz="-0.00000081417712" izz="0.00042953697654"/>
|
|
</inertial>
|
|
<visual>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="meshes/right_wrist_pitch_link.STL"/>
|
|
</geometry>
|
|
<material name="white">
|
|
<color rgba="0.7 0.7 0.7 1"/>
|
|
</material>
|
|
</visual>
|
|
<collision>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="meshes/right_wrist_pitch_link.STL"/>
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<joint name="right_wrist_yaw_joint" type="revolute">
|
|
<origin xyz="0.046 0 0" rpy="0 0 0"/>
|
|
<axis xyz="0 0 1"/>
|
|
<parent link="right_wrist_pitch_link"/>
|
|
<child link="right_wrist_yaw_link"/>
|
|
<limit effort="5" velocity="22" lower="-1.614429558" upper="1.614429558"/>
|
|
</joint>
|
|
<link name="right_wrist_yaw_link">
|
|
<inertial>
|
|
<origin xyz="0.02200381568 -0.00049485096 0.00053861123" rpy="0 0 0"/>
|
|
<mass value="0.08457647"/>
|
|
<inertia ixx="0.00004929128828" ixy="0.00000045735494" ixz="0.00000445867591" iyy="0.00005973338134" iyz="-0.00000043217198" izz="0.00003928083826"/>
|
|
</inertial>
|
|
<visual>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="meshes/right_wrist_yaw_link.STL"/>
|
|
</geometry>
|
|
<material name="white">
|
|
<color rgba="0.7 0.7 0.7 1"/>
|
|
</material>
|
|
</visual>
|
|
<collision>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="meshes/right_wrist_yaw_link.STL"/>
|
|
</geometry>
|
|
</collision>
|
|
</link>
|
|
<joint name="right_hand_palm_joint" type="fixed">
|
|
<origin xyz="0.0415 -0.003 0" rpy="0 0 0"/>
|
|
<parent link="right_wrist_yaw_link"/>
|
|
<child link="right_rubber_hand"/>
|
|
</joint>
|
|
<link name="right_rubber_hand">
|
|
<inertial>
|
|
<origin xyz="0.05361310808 0.00295905240 0.00215413091" rpy="0 0 0"/>
|
|
<mass value="0.170"/>
|
|
<inertia ixx="0.00010099485234748" ixy="-0.00003618590790516" ixz="-0.00000074301518642" iyy="0.00028135871571621" iyz="-0.00000330189743286" izz="0.00021894770413514"/>
|
|
</inertial>
|
|
<visual>
|
|
<origin xyz="0 0 0" rpy="0 0 0"/>
|
|
<geometry>
|
|
<mesh filename="meshes/right_rubber_hand.STL"/>
|
|
</geometry>
|
|
<material name="white">
|
|
<color rgba="0.7 0.7 0.7 1"/>
|
|
</material>
|
|
</visual>
|
|
</link>
|
|
</robot> |