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

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
  <xacro:include filename="$(find drz_telemax_description)/urdf/drz_telemax_arm.gazebo.xacro"/>
  <xacro:include filename="$(find drz_telemax_description)/urdf/drz_telemax_gripper_finger.urdf.xacro"/>

<!--   <xacro:include filename="$(find drz_telemax_description)/urdf/materials.urdf.xacro"/> -->
  <xacro:include filename="$(find hector_xacro_tools)/urdf/joint_macros.urdf.xacro"/>
  <xacro:include filename="$(find hector_xacro_tools)/urdf/inertia_tensors.urdf.xacro" />

  <xacro:property name="M_PI" value="3.1415926535897931" />

  <xacro:property name="turretTurnLower" value="-210.0"/>
  <xacro:property name="upperArmTiltLower" value="-156.0"/>
  <xacro:property name="lowerArmTiltLower" value="-170.0"/>
  <xacro:property name="lowerArmTurnLower" value="-210.0"/>
  <xacro:property name="gripperTiltLower" value="-130.0"/><!--TODO wenn Kamera auf Handgelenk gesteckt wird, aendern sich die Werte-->

  <xacro:property name="turretTurnUpper" value="210.0"/>
  <xacro:property name="upperArmTiltUpper" value="60.0"/>
  <xacro:property name="lowerArmTiltUpper" value="170.0"/>
  <xacro:property name="lowerArmTurnUpper" value="210.0"/>
  <xacro:property name="gripperTiltUpper" value="130.0"/>

  <xacro:property name="turretTurnMaxVel" value="36.0"/>
  <xacro:property name="upperArmTiltMaxVel" value="18.0"/>
  <xacro:property name="lowerArmTiltMaxVel" value="36.0"/>
  <xacro:property name="lowerArmTurnMaxVel" value="72.0"/>
  <xacro:property name="gripperTiltMaxVel" value="78.0"/>
  <xacro:property name="gripperTurnMaxVel" value="74.0"/>

  <xacro:property name="turretTurnMaxEffort" value="10000.0"/>
  <xacro:property name="upperArmTiltMaxEffort" value="10000.0"/>
  <xacro:property name="lowerArmTiltMaxEffort" value="10000.0"/>
  <xacro:property name="lowerArmTurnMaxEffort" value="10000.0"/>
  <xacro:property name="gripperTiltMaxEffort" value="10000.0"/>
  <xacro:property name="gripperTurnMaxEffort" value="10000.0"/>

  <xacro:property name="gripperForwardLower" value="0.03"/>
  <xacro:property name="gripperJawRightLower" value="0.0"/>
  <xacro:property name="gripperWidthLower" value="0.0"/>

  <xacro:property name="gripperForwardUpper" value="0.06"/>
  <xacro:property name="gripperJawRightUpper" value="0.06"/>
  <xacro:property name="gripperWidthUpper" value="0.12"/>

  <xacro:property name="gripperForwardMaxVel" value="0.08"/>
  <xacro:property name="gripperJawRightMaxVel" value="0.0085"/>
  <xacro:property name="gripperWidthMaxVel" value="0.017"/>

  <xacro:property name="gripperForwardMaxEffort" value="1.0"/>
  <xacro:property name="gripperJawRightMaxEffort" value="1.0"/>
  <xacro:property name="gripperWidthMaxEffort" value="1.0"/>

  <xacro:macro name="telemax_6dof_manipulator" params="parent *origin">
    <joint name="manipulator_base_joint" type="fixed">
      <xacro:insert_block name="origin" />
      <parent link="${parent}"/>
      <child link="manipulator_base_link"/>
    </joint>

    <link name="manipulator_base_link">
      <xacro:inertial_cuboid_with_pose mass="1" x_length="0.02" y_length="0.02" z_length="0.02" >
        <origin xyz="0 0 0.0" rpy="0 0 0"/>
      </xacro:inertial_cuboid_with_pose>
      <visual>
        <geometry>
          <mesh filename="package://drz_telemax_description/meshes/manipulator_base_link.dae" scale="0.001 0.001 0.001"/>
        </geometry>
      </visual>
      <collision>
        <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
        <geometry>
          <cylinder radius="0.1" length="0.02" />
        </geometry>
      </collision>
    </link>

    <joint name="arm_joint_0" type="revolute">
      <parent link="manipulator_base_link"/>
      <child link="arm_link_0"/>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <axis xyz="0 0 1" />
      <limit lower="${turretTurnLower * M_PI/180.0}" upper="${turretTurnUpper * M_PI/180.0}" velocity="${turretTurnMaxVel * M_PI/180.0}" effort="${turretTurnMaxEffort}" />
    </joint>

    <link name="arm_link_0">
      <xacro:inertial_cuboid_with_pose mass="3" x_length="0.25" y_length="0.2" z_length="0.18" >
        <origin xyz="${-0.115 + (0.115+0.123)/2} ${-0.0715 + (0.0715+0.1142)/2} ${(0.092+0.160)/2.0}" rpy="0 0 0"/>
      </xacro:inertial_cuboid_with_pose>

      <visual>
        <geometry>
          <mesh filename="package://drz_telemax_description/meshes/arm_link_0.dae" scale="0.001 0.001 0.001"/>
        </geometry>
      </visual>

      <!-- Tower body -->
      <collision>
        <origin xyz="0.01 0.025 0.165" rpy="0 0 0"/>
        <geometry>
          <box size="0.25 0.2 0.18" />
        </geometry>
      </collision>

      <!-- Tower cylinder -->
      <collision>
        <origin xyz="0.0 0.0 0.03" rpy="0 0 0"/>
        <geometry>
          <cylinder radius="0.075" length="0.09" />
        </geometry>
      </collision>

      <!-- Ring at tower base -->
      <collision>
        <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
        <geometry>
          <cylinder radius="0.1" length="0.02" />
        </geometry>
      </collision>
      
      <!-- Antenna 1 -->
      <collision>
        <origin xyz="${0.01 - 0.25/2} ${0.025 + 0.2/2 - 0.02875} ${0.165 + 0.18/2 + 0.22/2}" rpy="0 0 0"/>
        <geometry>
          <cylinder radius="0.01" length="0.22" />
        </geometry>
      </collision>
      
      <!-- Antenna 2 -->
      <collision>
        <origin xyz="${0.01 - 0.25/2 + 0.13} ${0.025 + 0.2/2} ${0.165 + 0.18/2 + 0.22/2}" rpy="0 0 0"/>
        <geometry>
          <cylinder radius="0.01" length="0.22" />
        </geometry>
      </collision>
    </link>


    <joint name="arm_joint_1" type="revolute">
      <parent link="arm_link_0"/>
      <child link="arm_link_1"/>
      <origin xyz="0.06 -0.0815 0.14" rpy="0 0 0" />
      <axis xyz="0 1 0" />
      <limit lower="${upperArmTiltLower * M_PI/180.0}" upper="${upperArmTiltUpper * M_PI/180.0}" velocity="${upperArmTiltMaxVel * M_PI/180.0}" effort="${upperArmTiltMaxEffort}" />
    </joint>

    <link name="arm_link_1">

<!--      <inertial>
        <origin xyz="${-0.066 + (0.066+0.089)/2} 0 0" 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="5" x_length="0.8" y_length="0.08" z_length="0.08" >
        <origin xyz="${0.4 - 0.07} 0 0" rpy="0 0 0"/>
      </xacro:inertial_cuboid_with_pose>

      <visual>
        <geometry>
          <mesh filename="package://drz_telemax_description/meshes/arm_link_1.dae" scale="0.001 0.001 0.001"/>
        </geometry>
      </visual>

      <collision>
        <origin xyz="0.01 -0.005 0" rpy="0 0 0"/>
        <geometry>
          <box size="0.16 0.08 0.110" />
        </geometry>
      </collision>
      <collision>
        <origin xyz="0.26 -0.005 0" rpy="0 0 0"/>
        <geometry>
          <box size="0.36 0.08 0.08" />
        </geometry>
      </collision>
      <collision>
        <origin xyz="0.56 -0.02 0" rpy="0 0 0"/>
        <geometry>
          <box size="0.35 0.085 0.13" />
        </geometry>
      </collision>
    </link>

    <joint name="arm_joint_2" type="revolute">
      <parent link="arm_link_1"/>
      <child link="arm_link_2"/>
      <origin xyz="0.67 0.081 0.0" rpy="0 0 0" />
      <axis xyz="0 1 0" />
      <limit lower="${lowerArmTiltLower * M_PI/180.0}" upper="${lowerArmTiltUpper * M_PI/180.0}" velocity="${lowerArmTiltMaxVel * M_PI/180.0}" effort="${lowerArmTiltMaxEffort}" />
    </joint>

    <link name="arm_link_2">

<!--      <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="1" x_length="0.15" y_length="0.14" z_length="0.15" >
        <origin xyz="0.01 0.01 0.01" rpy="0 0 0"/>
      </xacro:inertial_cuboid_with_pose>

      <visual>
        <geometry>
          <mesh filename="package://drz_telemax_description/meshes/arm_link_2.dae" scale="0.001 0.001 0.001"/>
        </geometry>
      </visual>

      <collision>
        <origin xyz="0.01 0.01 0.01" rpy="0 0 0"/>
        <geometry>
          <box size="0.15 0.14 0.15" />
        </geometry>
      </collision>

      <collision>
        <origin xyz="0.11 0.0 0.0" rpy="0 0 0"/>
        <geometry>
          <box size="0.05 0.09 0.09" />
        </geometry>
      </collision>
    </link>

    <joint name="arm_joint_3" type="revolute">
      <parent link="arm_link_2"/>
      <child link="arm_link_3"/>
      <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
      <axis xyz="1 0 0" />
      <limit lower="${lowerArmTurnLower * M_PI/180.0}" upper="${lowerArmTurnUpper * M_PI/180.0}" velocity="${lowerArmTurnMaxVel * M_PI/180.0}" effort="${lowerArmTurnMaxEffort}" />
    </joint>

    <link name="arm_link_3">
<!--      <inertial>
        <origin xyz="${0.494/2} ${0.0523 - 0.1195/2} 0" 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="3" x_length="0.45" y_length="0.09" z_length="0.09" >
<!--         <origin xyz="${0.494/2} ${0.0523 - 0.1195/2} 0" rpy="0 0 0"/> -->
        <origin xyz="0.3 0.0 0.0" rpy="0 0 0"/>
      </xacro:inertial_cuboid_with_pose>

      <visual>
        <geometry>
          <mesh filename="package://drz_telemax_description/meshes/arm_link_3.dae" scale="0.001 0.001 0.001"/>
        </geometry>
      </visual>

      <collision>
        <origin xyz="0.3 0.0 0.0" rpy="0 0 0"/>
        <geometry>
          <box size="0.35 0.09 0.09" />
        </geometry>
      </collision>
      <collision>
        <origin xyz="0.53 0.055 0" rpy="0 0 0"/>
        <geometry>
          <box size="0.25 0.045 0.09" />
        </geometry>
      </collision>
      <collision>
        <origin xyz="0.46 0.01 0" rpy="0 0 0"/>
        <geometry>
          <box size="0.13 0.13 0.09" />
        </geometry>
      </collision>
    </link>

    <joint name="arm_joint_4" type="revolute">
      <parent link="arm_link_3"/>
      <child link="arm_link_4"/>
      <origin xyz="0.615 0.0 0.0" rpy="0 0 0" />
      <axis xyz="0 1 0" />
      <limit lower="${gripperTiltLower * M_PI/180.0}" upper="${gripperTiltUpper * M_PI/180.0}" velocity="${gripperTiltMaxVel * M_PI/180.0}" effort="${gripperTiltMaxEffort}" />
    </joint>

    <link name="arm_link_4">
<!--      <inertial>
        <origin xyz="${-0.059 + 0.168/2} ${-0.046 + 0.076/2.0} ${-0.0367 + (0.0513+0.0367)/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="1" x_length="0.21" y_length="0.1" z_length="0.09" >
        <origin xyz="0.025 -0.01 0.005" rpy="0 0 0"/>
      </xacro:inertial_cuboid_with_pose>

      <visual>
        <geometry>
          <mesh filename="package://drz_telemax_description/meshes/arm_link_4.dae" scale="0.001 0.001 0.001"/>
        </geometry>
      </visual>

      <collision>
        <origin xyz="0.025 -0.01 0.005" rpy="0 0 0"/>
        <geometry>
          <box size="0.21 0.1 0.09" />
        </geometry>
      </collision>

      <collision>
        <origin xyz="0.0625 -0.00125 0.055" rpy="0 0 0"/>
        <geometry>
          <cylinder radius="0.03" length="0.02" />
        </geometry>
      </collision>
    </link>

    <joint name="arm_joint_5" type="continuous">
      <parent link="arm_link_4"/>
      <child link="arm_link_5"/>
      <origin xyz="0.12887 0.0 0.00065" rpy="0 0 0" />
      <axis xyz="1 0 0" />
      <limit velocity="${gripperTurnMaxVel*M_PI/180.0}" effort="${gripperTurnMaxEffort}" />
    </joint>

    <link name="arm_link_5">
<!--      <inertial>
        <origin xyz="${0.063/2} 0 ${-0.0525 + (0.0184+0.0525)/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.075" y_length="0.14" z_length="0.045" >
        <origin xyz="0.036 0.0 0.0" rpy="0 0 0"/>
      </xacro:inertial_cuboid_with_pose>

      <visual>
        <geometry>
          <mesh filename="package://drz_telemax_description/meshes/arm_link_5.dae" scale="0.001 0.001 0.001"/>
        </geometry>
      </visual>

      <collision>
        <origin xyz="0.036 0.0 0.0" rpy="0 0 0"/>
        <geometry>
          <box size="0.075 0.14 0.045" />
        </geometry>
      </collision>

      <collision>
        <origin xyz="0.03 0.0 0.035" rpy="0 0 0"/>
        <geometry>
          <box size="0.045 0.045 0.045" />
        </geometry>
      </collision>

    </link>

    <!-- Gripper -->
    <joint name="gripper_joint" type="revolute">
      <origin xyz="0 0 0" rpy="0 0 0" />
      <axis xyz="0 0 1" />
      <limit lower="0" upper="0.2" velocity="0.02" effort="1000" />
      <parent link="arm_link_5" />
      <child link="gripper_servo_link" />
    </joint>

    <link name="gripper_servo_link">
      <xacro:inertial_cuboid_with_pose mass="0.01" x_length="0.05" y_length="0.05" z_length="0.05" >
        <origin xyz="0 0 0" rpy="0 0 0" />
      </xacro:inertial_cuboid_with_pose>

<!--      <visual>
        <origin xyz="0 0.0 0.0" rpy="0.0 0.0 0.0" />
        <geometry>
          <cylinder length="0.01" radius="0.01" />
        </geometry>
      </visual>

      <collision>
        <origin xyz="0 0.0 0.0" rpy="0.0 0.0 0.0" />
        <geometry>
          <cylinder length="0.01" radius="0.01" />
        </geometry>
      </collision>-->
    </link>

    <xacro:telemax_gripper_finger parent="arm_link_5" prefix="left" mimic="gripper_joint">
        <origin xyz="0.035 0.039 0" rpy="0 0 0"/>
    </xacro:telemax_gripper_finger>

    <xacro:telemax_gripper_finger parent="arm_link_5" prefix="right" mimic="gripper_joint">
        <origin xyz="0.035 -0.039 0" rpy="${M_PI} 0 0"/>
    </xacro:telemax_gripper_finger>

    <!-- Virtual TCP link -->
    <joint name="gripper_tcp_joint" type="fixed">
      <origin xyz="0.234 0 0" rpy="0 0 0" />
      <axis xyz="0 0 1" />
      <parent link="arm_link_5" />
      <child link="gripper_tcp_link" />
    </joint>

    <link name="gripper_tcp_link">
    </link>

    <!-- Transmissions -->
    <xacro:joint_position_transmission name="arm_joint_0"/>
    <xacro:joint_position_transmission name="arm_joint_1"/>
    <xacro:joint_position_transmission name="arm_joint_2"/>
    <xacro:joint_position_transmission name="arm_joint_3"/>
    <xacro:joint_position_transmission name="arm_joint_4"/>
    <xacro:joint_position_transmission name="arm_joint_5"/>
    <xacro:joint_position_transmission name="gripper_joint"/>

    <!-- Gazebo plugins -->
    <xacro:telemax_6dof_manipulator_gazebo_plugins/>
  </xacro:macro>


</robot>