Thesis-Hector-VR/Hector_v2/Assets/Telemax/drz_telemax_description/urdf/drz_telemax_gripper_finger.urdf.xacro

<?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>