<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from rasmt.xacro                    | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="rasmt">

  <mujoco>
    <compiler balanceinertia="true" discardvisual="true" fusestatic="true" meshdir="../meshes_mjcf/" strippath="true"/>
  </mujoco>
  
  
  <link name="world"/>
  <joint name="to_world" type="fixed">
    <parent link="world"/>
    <child link="rasmt_Base_Link"/>
    <!--origin xyz="0 0 1.15" rpy="3.14159 0 3.14159"/-->
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="rasmt_Base_Link">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.0030651 -3.2739E-05 0.082353"/>
      <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 rpy="0 0 0" xyz="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 rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://rasmt_support/meshes/collision/Base_Link.STL"/>
      </geometry>
    </collision>
  </link>
  <link name="rasmt_Fork_1">
    <inertial>
      <origin rpy="0 0 0" xyz="0.043764 -0.0066984 -0.032285"/>
      <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 rpy="0 0 0" xyz="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 rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://rasmt_support/meshes/collision/Fork_1.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="rasmt_Rot_Z_1" type="revolute">
    <origin rpy="-3.1416 0 0" xyz="0 0 0.2533"/>
    <parent link="rasmt_Base_Link"/>
    <child link="rasmt_Fork_1"/>
    <axis xyz="0 0 1"/>
    <limit effort="5.149" lower="-3.1416" upper="3.1416" velocity="0.99903"/>
  </joint>
  <link name="rasmt_Link_1">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.00042264 0.0 0.075171"/>
      <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 rpy="0 0 0" xyz="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 rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://rasmt_support/meshes/collision/Link_1.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="rasmt_Rot_Y_1" type="revolute">
    <origin rpy="3.1416 0 0" xyz="0.1045 0.0 -0.0795"/>
    <parent link="rasmt_Fork_1"/>
    <child link="rasmt_Link_1"/>
    <axis xyz="0 1 0"/>
    <limit effort="5.149" lower="-1.5707" upper="2.2863" velocity="0.99903"/>
  </joint>
  <link name="rasmt_Fork_2">
    <inertial>
      <origin rpy="0 0 0" xyz="0.043764 0.0066984 0.032285"/>
      <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 rpy="0 0 0" xyz="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 rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://rasmt_support/meshes/collision/Fork_2.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="rasmt_Rot_Z_2" type="revolute">
    <origin rpy="0 0 0" xyz="0 0.0 0.17502"/>
    <parent link="rasmt_Link_1"/>
    <child link="rasmt_Fork_2"/>
    <axis xyz="0 0 1"/>
    <limit effort="5.149" lower="-3.1416" upper="3.1416" velocity="0.99903"/>
  </joint>
  <link name="rasmt_Link_2">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.00042264 0.0 0.075171"/>
      <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 rpy="0 0 0" xyz="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 rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://rasmt_support/meshes/collision/Link_2.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="rasmt_Rot_Y_2" type="revolute">
    <origin rpy="0 0 0" xyz="0.1045 0.0 0.0795"/>
    <parent link="rasmt_Fork_2"/>
    <child link="rasmt_Link_2"/>
    <axis xyz="0 1 0"/>
    <limit effort="5.149" lower="-1.5707" upper="2.2863" velocity="0.99903"/>
  </joint>
  <link name="rasmt_Fork_3">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0437644555284691 0.00669835350471771 0.0322846229336455"/>
      <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 rpy="0 0 0" xyz="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 rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://rasmt_support/meshes/collision/Fork_3.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="rasmt_Rot_Z_3" type="revolute">
    <origin rpy="0 0 0" xyz="0 0.0 0.175"/>
    <parent link="rasmt_Link_2"/>
    <child link="rasmt_Fork_3"/>
    <axis xyz="0 0 1"/>
    <limit effort="5.149" lower="-3.14159" upper="3.14159" velocity="0.99903"/>
  </joint>
  <link name="rasmt_Dock_Link">
    <inertial>
      <origin rpy="0 0 0" xyz="1.08992929317431E-05 0.0 0.0566747528784688"/>
      <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 rpy="0 0 0" xyz="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 rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://rasmt_support/meshes/collision/Dock_Link.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="rasmt_Rot_Y_4" type="revolute">
    <origin rpy="0 0 0" xyz="0.1045 0.0 0.0795"/>
    <parent link="rasmt_Fork_3"/>
    <child link="rasmt_Dock_Link"/>
    <axis xyz="0 1 0"/>
    <limit effort="5.149" lower="-1.5707" upper="2.2863" velocity="0.99903"/>
  </joint>
  <!-- ros_control-plugin -->
  <!-- <xacro:if value="${sim == 'gazebo'}">
            <gazebo>
                <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
                    <parameters>$(find rasmt_support)/config/rasmt_effort_controllers.yaml</parameters>
                    <robotNamespace>/${prefix}</robotNamespace>
                </plugin>
            </gazebo>      
        </xacro:if> -->
  <gazebo>
    <plugin filename="ign_ros2_control-system" name="ign_ros2_control::IgnitionROS2ControlPlugin">
      <parameters>/home/bill-finger/rasms_ws/install/share/rasmt_support/config/rasmt_effort_controllers.yaml</parameters>
    </plugin>
  </gazebo>
  <!-- <gazebo reference="${prefix}_Base_Link">
            <mu1>0.2</mu1>
            <mu2>0.2</mu2>
            <turnGravityOff>true</turnGravityOff>
        </gazebo>

        <gazebo reference="${prefix}_Fork_1">
            <mu1>0.2</mu1>
            <mu2>0.2</mu2>
            <turnGravityOff>true</turnGravityOff>
        </gazebo>

        <gazebo reference="${prefix}_Link_1">
            <mu1>0.2</mu1>
            <mu2>0.2</mu2>
            <turnGravityOff>true</turnGravityOff>
        </gazebo>

        <gazebo reference="${prefix}_Fork_2">
            <mu1>0.2</mu1>
            <mu2>0.2</mu2>
            <turnGravityOff>true</turnGravityOff>
        </gazebo>

        <gazebo reference="${prefix}_Link_2">
            <mu1>0.2</mu1>
            <mu2>0.2</mu2>
            <turnGravityOff>true</turnGravityOff>
        </gazebo>

        <gazebo reference="${prefix}_Fork_3">
            <mu1>0.2</mu1>
            <mu2>0.2</mu2>
            <turnGravityOff>true</turnGravityOff>
        </gazebo>

        <gazebo reference="${prefix}_Dock_Link">
            <mu1>0.2</mu1>
            <mu2>0.2</mu2>
            <turnGravityOff>true</turnGravityOff>
        </gazebo> -->
  <!-- arg for control mode -->
  <ros2_control name="rasmt_single_hi" type="system">
    <hardware>
      <plugin>ign_ros2_control/IgnitionSystem</plugin>
    </hardware>
    <joint name="rasmt_Rot_Z_1">
      <command_interface name="position">
        <!-- better to use radians as min max first -->
        <param name="min">-1</param>
        <param name="max"> 1</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <joint name="rasmt_Rot_Y_1">
      <command_interface name="position">
        <param name="min">-1</param>
        <param name="max"> 1</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <joint name="rasmt_Rot_Z_2">
      <command_interface name="position">
        <param name="min">-1</param>
        <param name="max"> 1</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <joint name="rasmt_Rot_Y_2">
      <command_interface name="position">
        <param name="min">-1</param>
        <param name="max"> 1</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <joint name="rasmt_Rot_Z_3">
      <command_interface name="position">
        <param name="min">-1</param>
        <param name="max"> 1</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <joint name="rasmt_Rot_Y_4">
      <command_interface name="position">
        <param name="min">-1</param>
        <param name="max"> 1</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
  </ros2_control>
  <link name="rasmt_tool0"/>
  <joint name="rasmt_tool0_fixed" type="fixed">
    <parent link="rasmt_Dock_Link"/>
    <child link="rasmt_tool0"/>
    <origin rpy="0 0 0" xyz="0 0 0.12324"/>
    <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}_Dock_Link"/>
      <child link="${prefix}_tool_end_point"/>
      <origin xyz="0 0 ${gripper_length+0.12324}" rpy="0 0 0"/>
      <axis xyz="0 0 0"/>
  </joint-->
  <link name="rasmt_Grip_Body">
    <inertial>
      <origin rpy="0 0 0" xyz="0.0016793 -0.00053126 -0.024041"/>
      <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 rpy="0 0 0" xyz="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 rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://rasmt_support/meshes/collision/Grip_Body.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="rasmt_Fix" type="fixed">
    <origin rpy="-3.14159265358979 0 0" xyz="0 0.0 0.12324"/>
    <parent link="rasmt_Dock_Link"/>
    <child link="rasmt_Grip_Body"/>
    <axis xyz="0 0 0"/>
  </joint>
  <link name="rasmt_Grip_L">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.010118 0.010281 -0.0038252"/>
      <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 rpy="0 0 0" xyz="-0.02 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 rpy="0 0 0" xyz="-0.02 0 0"/>
      <geometry>
        <mesh filename="package://rasmt_support/meshes/collision/Grip_L.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="rasmt_Slide_1" type="prismatic">
    <origin rpy="3.1416 0 -1.5708" xyz="0 0 -0.09305"/>
    <parent link="rasmt_Grip_Body"/>
    <child link="rasmt_Grip_L"/>
    <axis xyz="1 0 0"/>
    <limit effort="20" lower="0.0" upper="0.06" velocity="0.2"/>
  </joint>
  <link name="rasmt_Grip_R">
    <inertial>
      <origin rpy="0 0 0" xyz="-0.010118 0.010281 -0.0038252"/>
      <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 rpy="0 0 0" xyz="-0.02 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 rpy="0 0 0" xyz="-0.02 0 0"/>
      <geometry>
        <mesh filename="package://rasmt_support/meshes/collision/Grip_R.STL"/>
      </geometry>
    </collision>
  </link>
  <joint name="rasmt_Slide_2" type="prismatic">
    <origin rpy="3.1416 0 1.5708" xyz="0 0 -0.09305"/>
    <parent link="rasmt_Grip_Body"/>
    <child link="rasmt_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>
  <!-- arg for control mode -->
  <ros2_control name="rasmt_hand_hi" type="system">
    <hardware>
      <plugin>ign_ros2_control/IgnitionSystem</plugin>
    </hardware>
    <joint name="rasmt_Slide_1">
      <command_interface name="position">
        <!-- better to use radians as min max first -->
        <param name="min">0</param>
        <param name="max">10</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
    <joint name="rasmt_Slide_2">
      <!-- <param name="mimic">${prefix}_Slide_1</param>
                <param name="multiplier">1</param> -->
      <command_interface name="position">
        <param name="min">0</param>
        <param name="max">10</param>
      </command_interface>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
      <state_interface name="effort"/>
    </joint>
  </ros2_control>
  <!-- ros_control-plugin -->
  <!--gazebo>
            <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
                <robot_sim_type>gazebo_ros2_control/GazeboSystem</robot_sim_type>
                <parameters>$(find rasmt_support)/config/rasmt_hand_controllers.yaml</parameters>
                <robotNamespace>/${prefix}</robotNamespace>
            </plugin>
        </gazebo-->
  <!-- <gazebo reference="${prefix}_Grip_Body">
            <mu1>0.2</mu1>
            <mu2>0.2</mu2>
            <turnGravityOff>true</turnGravityOff>
        </gazebo>

        <gazebo reference="${prefix}_Grip_L">
            <mu1>0.2</mu1>
            <mu2>0.2</mu2>
            <turnGravityOff>true</turnGravityOff>
        </gazebo>

        <gazebo reference="${prefix}_Grip_R">
            <mu1>0.2</mu1>
            <mu2>0.2</mu2>
            <turnGravityOff>true</turnGravityOff>
        </gazebo> -->
</robot>
