runtime/rasmt_support/urdf/tools/rasmt_hand_macro.xacro
2022-04-07 15:17:46 +00:00

189 lines
4.1 KiB
XML

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find rasmt_support)/urdf/tools/rasmt_hand_gazebo.xacro"/>
<xacro:include filename="$(find rasmt_support)/urdf/tools/rasmt_hand_control.xacro"/>
<xacro:macro name="rasmt_hand" params="prefix sim">
<link
name="${prefix}_Grip_Body">
<inertial>
<origin
xyz="0.0016793 -0.00053126 -0.024041"
rpy="0 0 0" />
<mass
value="0.72351" />
<inertia
ixx="0.00045979"
ixy="-2.1983E-08"
ixz="-6.5154E-06"
iyy="0.00048572"
iyz="8.3162E-07"
izz="0.00082469" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://rasmt_support/meshes/visual/Grip_Body.dae" />
</geometry>
<material
name="">
<color
rgba="0.50196 1 0.50196 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://rasmt_support/meshes/collision/Grip_Body.stl" />
</geometry>
</collision>
</link>
<joint
name="${prefix}_Fix"
type="fixed">
<origin
xyz="0 0.0 0.12324"
rpy="-3.14159265358979 0 0" />
<parent
link="${prefix}_Dock_Link" />
<child
link="${prefix}_Grip_Body" />
<axis
xyz="0 0 0" />
</joint>
<link
name="${prefix}_Grip_L">
<inertial>
<origin
xyz="-0.010118 0.010281 -0.0038252"
rpy="0 0 0" />
<mass
value="0.021223" />
<inertia
ixx="6.5436E-06"
ixy="-3.114E-06"
ixz="2.8479E-06"
iyy="1.9726E-05"
iyz="1.6432E-06"
izz="1.6355E-05" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://rasmt_support/meshes/visual/Grip_L.dae" />
</geometry>
<material
name="">
<color
rgba="0.75294 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://rasmt_support/meshes/collision/Grip_L.stl" />
</geometry>
</collision>
</link>
<joint
name="${prefix}_Slide_1"
type="prismatic">
<origin
xyz="0 -0.02 -0.09305"
rpy="3.1416 0 -1.5708" />
<parent
link="${prefix}_Grip_Body" />
<child
link="${prefix}_Grip_L" />
<axis
xyz="-1 0 0" />
<limit
effort="20"
lower="0.0"
upper="0.06"
velocity="0.2"/>
</joint>
<link
name="${prefix}_Grip_R">
<inertial>
<origin
xyz="-0.010118 0.010281 -0.0038252"
rpy="0 0 0" />
<mass
value="0.021223" />
<inertia
ixx="6.5436E-06"
ixy="-3.114E-06"
ixz="2.8479E-06"
iyy="1.9726E-05"
iyz="1.6432E-06"
izz="1.6355E-05" />
</inertial>
<visual>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://rasmt_support/meshes/visual/Grip_R.dae" />
</geometry>
<material
name="">
<color
rgba="0.75294 1 1 1" />
</material>
</visual>
<collision>
<origin
xyz="0 0 0"
rpy="0 0 0" />
<geometry>
<mesh
filename="package://rasmt_support/meshes/collision/Grip_R.stl" />
</geometry>
</collision>
</link>
<joint
name="${prefix}_Slide_2"
type="prismatic">
<origin
xyz="0 0.02 -0.09305"
rpy="3.1416 0 1.5708" />
<parent
link="${prefix}_Grip_Body" />
<child
link="${prefix}_Grip_R" />
<axis
xyz="-1 0 0" />
<limit
effort="20"
lower="0.0"
upper="0.06"
velocity="0.2"/>
<!--mimic joint="${prefix}_Slide_1"/-->
</joint>
<xacro:rasmt_hand_hi prefix="${prefix}" sim="${sim}"/>
<xacro:rasmt_hand_gazebo prefix="${prefix}"/>
</xacro:macro>
</robot>