<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

  <xacro:include filename="$(find rasmt_support)/urdf/robot/rasmt_single_control.xacro"/>
  <xacro:include filename="$(find rasmt_support)/urdf/robot/rasmt_single_gazebo.xacro"/>

<xacro:macro name="rasmt_single" params="prefix parent sim xyz gripper_length">


  <joint name="to_${parent}" type="fixed">
    <parent link="${parent}"/>
    <child link="${prefix}_Base_Link"/>
    <!--origin xyz="0 0 1.15" rpy="3.14159 0 3.14159"/-->
    <origin xyz="${xyz}" rpy="0 0 0"/>
    <axis xyz="0 0 0"/>
  </joint>


  <link name="${prefix}_Base_Link">
    <inertial>
      <origin xyz="-0.0030651 -3.2739E-05 0.082353" rpy="0 0 0" />
      <mass value="5.2929" />
      <inertia
        ixx="0.0076169"
        ixy="1.0121E-05"
        ixz="-0.00010622"
        iyy="0.0076597"
        iyz="6.5117E-05"
        izz="0.01165" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://rasmt_support/meshes/visual/Base_Link.dae" />
      </geometry>
      <material
        name="">
        <color rgba="0.75294 0.75294 0.75294 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://rasmt_support/meshes/collision/Base_Link.STL" />
      </geometry>
    </collision>
  </link>

  <link name="${prefix}_Fork_1">
    <inertial>
      <origin xyz="0.043764 -0.0066984 -0.032285" rpy="0 0 0" />
      <mass value="0.67797" />
      <inertia
        ixx="0.0014091"
        ixy="-6.2674E-05"
        ixz="0.00057897"
        iyy="0.0017329"
        iyz="4.7826E-05"
        izz="0.0019056" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="package://rasmt_support/meshes/visual/Fork_1.dae" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.75294 0.75294 0.75294 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://rasmt_support/meshes/collision/Fork_1.STL" />
      </geometry>
    </collision>
  </link>

  <joint
    name="${prefix}_Rot_Z_1"
    type="revolute">
    <origin
      xyz="0 0 0.2533"
      rpy="-3.1416 0 0" />
    <parent
      link="${prefix}_Base_Link" />
    <child
      link="${prefix}_Fork_1" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-3.1416"
      upper="3.1416"
      effort="5.149"
      velocity="0.99903" />
  </joint>

  <link
    name="${prefix}_Link_1">
    <inertial>
      <origin
        xyz="-0.00042264 0.0 0.075171"
        rpy="0 0 0" />
      <mass
        value="1.8959" />
      <inertia
        ixx="0.0029317"
        ixy="-9.417E-06"
        ixz="5.5248E-05"
        iyy="0.0031666"
        iyz="-9.3067E-05"
        izz="0.0015477" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://rasmt_support/meshes/visual/Link_1.dae" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.75294 0.75294 0.75294 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://rasmt_support/meshes/collision/Link_1.STL" />
      </geometry>
    </collision>
  </link>

  <joint
    name="${prefix}_Rot_Y_1"
    type="revolute">
    <origin
      xyz="0.1045 0.0 -0.0795"
      rpy="3.1416 0 0" />
    <parent
      link="${prefix}_Fork_1" />
    <child
      link="${prefix}_Link_1" />
    <axis
      xyz="0 1 0" />
    <limit
      lower="-1.5707"
      upper="2.2863"
      effort="5.149"
      velocity="0.99903" />
  </joint>

  <link
    name="${prefix}_Fork_2">
    <inertial>
      <origin
        xyz="0.043764 0.0066984 0.032285"
        rpy="0 0 0" />
      <mass
        value="0.67797" />
      <inertia
        ixx="0.0014091"
        ixy="6.2674E-05"
        ixz="-0.00057897"
        iyy="0.0017329"
        iyz="4.7826E-05"
        izz="0.0019056" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://rasmt_support/meshes/visual/Fork_2.dae" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.75294 0.75294 0.75294 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://rasmt_support/meshes/collision/Fork_2.STL" />
      </geometry>
    </collision>
  </link>

  <joint
    name="${prefix}_Rot_Z_2"
    type="revolute">
    <origin
      xyz="0 0.0 0.17502"
      rpy="0 0 0" />
    <parent
      link="${prefix}_Link_1" />
    <child
      link="${prefix}_Fork_2" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-3.1416"
      upper="3.1416"
      effort="5.149"
      velocity="0.99903" />
  </joint>

  <link
    name="${prefix}_Link_2">
    <inertial>
      <origin
        xyz="-0.00042264 0.0 0.075171"
        rpy="0 0 0" />
      <mass
        value="1.8959" />
      <inertia
        ixx="0.0029317"
        ixy="-9.4171E-06"
        ixz="5.5248E-05"
        iyy="0.0031666"
        iyz="-9.3067E-05"
        izz="0.0015477" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://rasmt_support/meshes/visual/Link_2.dae" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.75294 0.75294 0.75294 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://rasmt_support/meshes/collision/Link_2.STL" />
      </geometry>
    </collision>
  </link>

  <joint
    name="${prefix}_Rot_Y_2"
    type="revolute">
    <origin
      xyz="0.1045 0.0 0.0795"
      rpy="0 0 0" />
    <parent
      link="${prefix}_Fork_2" />
    <child
      link="${prefix}_Link_2" />
    <axis
      xyz="0 1 0" />
    <limit
      lower="-1.5707"
      upper="2.2863"
      effort="5.149"
      velocity="0.99903" />
  </joint>

  <link
    name="${prefix}_Fork_3">
    <inertial>
      <origin
        xyz="0.0437644555284691 0.00669835350471771 0.0322846229336455"
        rpy="0 0 0" />
      <mass
        value="0.677972982551887" />
      <inertia
        ixx="0.00140908677953665"
        ixy="6.26743492445164E-05"
        ixz="-0.000578970009504215"
        iyy="0.00173285340301686"
        iyz="4.78263030621606E-05"
        izz="0.00190561919128035" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://rasmt_support/meshes/visual/Fork_3.dae" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://rasmt_support/meshes/collision/Fork_3.STL" />
      </geometry>
    </collision>
  </link>

  <joint
    name="${prefix}_Rot_Z_3"
    type="revolute">
    <origin
      xyz="0 0.0 0.175"
      rpy="0 0 0" />
    <parent
      link="${prefix}_Link_2" />
    <child
      link="${prefix}_Fork_3" />
    <axis
      xyz="0 0 1" />
    <limit
      lower="-3.14159"
      upper="3.14159"
      effort="5.149"
      velocity="0.99903" />
  </joint>

  <link
    name="${prefix}_Dock_Link">
    <inertial>
      <origin
        xyz="1.08992929317431E-05 0.0 0.0566747528784688"
        rpy="0 0 0" />
      <mass
        value="1.81040750781824" />
      <inertia
        ixx="0.00136087225665183"
        ixy="-1.57915998142718E-06"
        ixz="-1.31398366941724E-06"
        iyy="0.00136397029450427"
        iyz="-0.00010058418524025"
        izz="0.00124273705160787" />
    </inertial>
    <visual>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://rasmt_support/meshes/visual/Dock_Link.dae" />
      </geometry>
      <material
        name="">
        <color
          rgba="0.752941176470588 0.752941176470588 0.752941176470588 1" />
      </material>
    </visual>
    <collision>
      <origin
        xyz="0 0 0"
        rpy="0 0 0" />
      <geometry>
        <mesh
          filename="package://rasmt_support/meshes/collision/Dock_Link.STL" />
      </geometry>
    </collision>
  </link>

  <joint
    name="${prefix}_Rot_Y_4"
    type="revolute">
    <origin
      xyz="0.1045 0.0 0.0795"
      rpy="0 0 0" />
    <parent
      link="${prefix}_Fork_3" />
    <child
      link="${prefix}_Dock_Link" />
    <axis
      xyz="0 1 0" />
    <limit
      lower="-1.5707"
      upper="2.2863"
      effort="5.149"
      velocity="0.99903" />
  </joint>
  <xacro:rasmt_single_gazebo prefix="${prefix}"/>
  <xacro:rasmt_single_control prefix="${prefix}" sim="${sim}"/>

  <link name="${prefix}_tool0"/>
  <joint name="${prefix}_tool0_fixed" type="fixed">
      <parent link="${prefix}_Dock_Link"/>
      <child link="${prefix}_tool0"/>
      <origin xyz="0 0 0.12324" rpy="0 0 0"/>
      <axis xyz="0 0 0"/>
  </joint>

  <link name="${prefix}_tool_end_point"/>
  <joint name="${prefix}_tool_end_point_to_tool0" type="fixed">
      <parent link="${prefix}_tool0"/>
      <child link="${prefix}_tool_end_point"/>
      <origin xyz="0 0 ${gripper_length}" rpy="0 0 0"/>
      <axis xyz="0 0 0"/>
  </joint>

  </xacro:macro>
</robot>
