123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111 |
- <?xml version="1.0"?>
- <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
- <xacro:property name="M_PI" value="3.1415926535897931" />
- <xacro:macro name="telemax_gripper_finger" params="parent *origin prefix mimic">
-
- <xacro:property name="middle_joint_multiplier" value="${1.02/0.2}" />
- <xacro:property name="middle_joint_offset" value="-0.12" />
- <xacro:property name="end_joint_multiplier" value="${-1 * 1.02/0.2}" />
- <xacro:property name="end_joint_offset" value="0.12" />
- <joint name="${prefix}_finger_middle_joint" type="revolute">
- <xacro:insert_block name="origin" />
- <parent link="${parent}"/>
- <child link="${prefix}_finger_middle_link"/>
- <axis xyz="0 0 1" />
- <mimic joint="${mimic}" multiplier="${middle_joint_multiplier}" offset="${middle_joint_offset}"/>
- <limit lower="-0.12" upper="1.02" velocity="1" effort="1000" />
- </joint>
- <link name="${prefix}_finger_middle_link">
- <!-- <inertial>
- <origin xyz="${-0.04 + (0.04+0.04+0.1246)/2} ${0.0634 - (0.0634+0.12-0.08)/2} ${-0.04 + (0.04+0.05)/2}" rpy="0 0 0"/>
- <mass value="1.0" />
- <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
- </inertial>-->
- <xacro:inertial_cuboid_with_pose mass="0.5" x_length="0.14" y_length="0.04" z_length="0.025" >
- <origin xyz="0.05 0.01 0.0" rpy="0 0 0"/>
- </xacro:inertial_cuboid_with_pose>
- <visual>
- <geometry>
- <mesh filename="package://drz_telemax_description/meshes/finger_middle_link.dae" scale="0.001 ${0.001} 0.001"/>
- </geometry>
- </visual>
- <collision>
- <origin xyz="0.05 0.01 0.0" rpy="0 0 0"/>
- <geometry>
- <box size="0.14 0.04 0.025" />
- </geometry>
- </collision>
- </link>
- <joint name="${prefix}_finger_end_joint" type="revolute">
- <parent link="${prefix}_finger_middle_link"/>
- <child link="${prefix}_finger_end_link"/>
- <origin xyz="0.110 0.0 0.0" rpy="0 0 0" />
- <axis xyz="0 0 1" />
- <mimic joint="${mimic}" multiplier="${end_joint_multiplier}" offset="${end_joint_offset}"/>
- <limit lower="-1.02" upper="0.12" velocity="1" effort="1000" />
- </joint>
- <link name="${prefix}_finger_end_link">
- <!-- <inertial>
- <origin xyz="${-0.04 + (0.04+0.04+0.1246)/2} ${0.0634 - (0.0634+0.12-0.08)/2} ${-0.04 + (0.04+0.05)/2}" rpy="0 0 0"/>
- <mass value="1.0" />
- <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
- </inertial>-->
- <xacro:inertial_cuboid_with_pose mass="0.5" x_length="0.08" y_length="0.03" z_length="0.0275" >
- <origin xyz="0.05 -0.01225 0.0" rpy="0 0 0"/>
- </xacro:inertial_cuboid_with_pose>
- <visual>
- <geometry>
- <mesh filename="package://drz_telemax_description/meshes/finger_end_link.dae" scale="0.001 ${0.001} 0.001"/>
- </geometry>
- </visual>
- <collision>
- <origin xyz="0.05 -0.01225 0.0" rpy="0 0 0"/>
- <geometry>
- <box size="0.08 0.03 0.0275" />
- </geometry>
- </collision>
- <collision>
- <origin xyz="0.0 0.0 0.0" rpy="0 0 ${-M_PI/4}"/>
- <geometry>
- <box size="0.06 0.03 0.0275" />
- </geometry>
- </collision>
- </link>
- <gazebo>
- <plugin name="mimic_${prefix}_finger_middle_joint" filename="libroboticsgroup_gazebo_mimic_joint_plugin.so">
- <joint>${mimic}</joint>
- <mimicJoint>${prefix}_finger_middle_joint</mimicJoint>
- <multiplier>${middle_joint_multiplier}</multiplier>
- <offset>${middle_joint_offset}</offset>
- <maxEffort>4</maxEffort>
- <hasPID/>
- </plugin>
- <plugin name="mimic_${prefix}_finger_end_joint" filename="libroboticsgroup_gazebo_mimic_joint_plugin.so">
- <joint>${mimic}</joint>
- <mimicJoint>${prefix}_finger_end_joint</mimicJoint>
- <multiplier>${end_joint_multiplier}</multiplier>
- <offset>${end_joint_offset}</offset>
- <maxEffort>4</maxEffort>
- <hasPID/>
- </plugin>
- </gazebo>
- </xacro:macro>
- </robot>
|