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

  <xacro:include filename="$(find vx300s_description)/urdf/vx300s_transmission.xacro" />
  <xacro:include filename="$(find vx300s_description)/urdf/vx300s_gazebo.xacro" />
  <material name="interbotix_black">
    <texture filename="package://interbotix_xsarm_descriptions/meshes/interbotix_black.png" />
  </material>

  <xacro:macro name="vx300s" params="prefix hw_interface">

    <xacro:vx300s_transmission prefix="${prefix}" hw_interface="${hw_interface}" />
    <xacro:vx300s_gazebo prefix="${prefix}" />

    <xacro:property name="pi_offset" value="0.00001" />

    <link name="${prefix}base_link">
      <visual>
        <origin rpy="0 0 ${pi/2}" xyz="0 0 0" />
        <geometry>
          <mesh
            filename="package://interbotix_xsarm_descriptions/meshes/vx300s_meshes/vx300s_1_base.stl"
            scale="0.001 0.001 0.001" />
        </geometry>
        <material name="interbotix_black" />
      </visual>
      <collision>
        <origin rpy="0 0 ${pi/2}" xyz="0 0 0" />
        <geometry>
          <mesh
            filename="package://interbotix_xsarm_descriptions/meshes/vx300s_meshes/vx300s_1_base.stl"
            scale="0.001 0.001 0.001" />
        </geometry>
      </collision>
      <inertial>
        <origin rpy="0 0 ${pi/2}" xyz="-0.0534774000 -0.0005625750 0.0205961000" />
        <mass value="0.969034" />
        <inertia ixx="0.0060240000" iyy="0.0017000000" izz="0.0071620000" ixy="0.0000471300"
          ixz="0.0000038510" iyz="-0.0000841500" />
      </inertial>
    </link>

    <joint name="${prefix}waist" type="revolute">
      <axis xyz="0 0 1" />
      <limit effort="10" lower="${-pi + pi_offset}" upper="${pi - pi_offset}" velocity="${pi}" />
      <origin rpy="0 0 0" xyz="0 0 0.079" />
      <parent link="${prefix}base_link" />
      <child link="${prefix}shoulder_link" />
      <dynamics friction="0.1" />
    </joint>

    <link name="${prefix}shoulder_link">
      <visual>
        <origin rpy="0 0 ${pi/2}" xyz="0 0 -0.003" />
        <geometry>
          <mesh
            filename="package://interbotix_xsarm_descriptions/meshes/vx300s_meshes/vx300s_2_shoulder.stl"
            scale="0.001 0.001 0.001" />
        </geometry>
        <material name="interbotix_black" />
      </visual>
      <collision>
        <origin rpy="0 0 ${pi/2}" xyz="0 0 -0.003" />
        <geometry>
          <mesh
            filename="package://interbotix_xsarm_descriptions/meshes/vx300s_meshes/vx300s_2_shoulder.stl"
            scale="0.001 0.001 0.001" />
        </geometry>
      </collision>
      <inertial>
        <origin rpy="0 0 ${pi/2}" xyz="0.0002592330 -0.0000033552 0.0116129000" />
        <mass value="0.798614" />
        <inertia ixx="0.0009388000" iyy="0.0011380000" izz="0.0012010000" ixy="-0.0000000010"
          ixz="-0.0000000191" iyz="0.0000059568" />
      </inertial>
    </link>

    <joint name="${prefix}shoulder" type="revolute">
      <axis xyz="0 1 0" />
      <limit effort="20" lower="${radians(-106)}" upper="${radians(72)}" velocity="${pi}" />
      <origin rpy="0 0 0" xyz="0 0 0.04805" />
      <parent link="${prefix}shoulder_link" />
      <child link="${prefix}upper_arm_link" />
      <dynamics friction="0.1" />
    </joint>

    <link name="${prefix}upper_arm_link">
      <visual>
        <origin rpy="0 0 ${pi/2}" xyz="0 0 0" />
        <geometry>
          <mesh
            filename="package://interbotix_xsarm_descriptions/meshes/vx300s_meshes/vx300s_3_upper_arm.stl"
            scale="0.001 0.001 0.001" />
        </geometry>
        <material name="interbotix_black" />
      </visual>
      <collision>
        <origin rpy="0 0 ${pi/2}" xyz="0 0 0" />
        <geometry>
          <mesh
            filename="package://interbotix_xsarm_descriptions/meshes/vx300s_meshes/vx300s_3_upper_arm.stl"
            scale="0.001 0.001 0.001" />
        </geometry>
      </collision>
      <inertial>
        <origin rpy="0 0 ${pi/2}" xyz="0.0206949000 0.0000000004 0.2264590000" />
        <mass value="0.792592" />
        <inertia ixx="0.0089250000" iyy="0.0089370000" izz="0.0009357000" ixy="0.0000000000"
          ixz="0.0000000000" iyz="0.0012010000" />
      </inertial>
    </link>

    <joint name="${prefix}elbow" type="revolute">
      <axis xyz="0 1 0" />
      <limit effort="15" lower="${radians(-101)}" upper="${radians(92)}" velocity="${pi}" />
      <origin rpy="0 0 0" xyz="0.05955 0 0.3" />
      <parent link="${prefix}upper_arm_link" />
      <child link="${prefix}upper_forearm_link" />
      <dynamics friction="0.1" />
    </joint>

    <link name="${prefix}upper_forearm_link">
      <visual>
        <origin rpy="0 0 0" xyz="0 0 0" />
        <geometry>
          <mesh
            filename="package://interbotix_xsarm_descriptions/meshes/vx300s_meshes/vx300s_4_upper_forearm.stl"
            scale="0.001 0.001 0.001" />
        </geometry>
        <material name="interbotix_black" />
      </visual>
      <collision>
        <origin rpy="0 0 0" xyz="0 0 0" />
        <geometry>
          <mesh
            filename="package://interbotix_xsarm_descriptions/meshes/vx300s_meshes/vx300s_4_upper_forearm.stl"
            scale="0.001 0.001 0.001" />
        </geometry>
      </collision>
      <inertial>
        <origin rpy="0 0 0" xyz="0.105723 0 0" />
        <mass value="0.322228" />
        <inertia ixx="0.0001524000" iyy="0.0013420000" izz="0.0014410000" ixy="-0.0000188300"
          ixz="-0.0000084064" iyz="0.0000012564" />
      </inertial>
    </link>

    <joint name="${prefix}forearm_roll" type="revolute">
      <axis xyz="1 0 0" />
      <limit effort="2" lower="${-pi + pi_offset}" upper="${pi - pi_offset}" velocity="${pi}" />
      <origin rpy="0 0 0" xyz="0.2 0 0" />
      <parent link="${prefix}upper_forearm_link" />
      <child link="${prefix}lower_forearm_link" />
      <dynamics friction="0.1" />
    </joint>

    <link name="${prefix}lower_forearm_link">
      <visual>
        <origin rpy="${pi} 0 0" xyz="0 0 0" />
        <geometry>
          <mesh
            filename="package://interbotix_xsarm_descriptions/meshes/vx300s_meshes/vx300s_5_lower_forearm.stl"
            scale="0.001 0.001 0.001" />
        </geometry>
        <material name="interbotix_black" />
      </visual>
      <collision>
        <origin rpy="${pi} 0 0" xyz="0 0 0" />
        <geometry>
          <mesh
            filename="package://interbotix_xsarm_descriptions/meshes/vx300s_meshes/vx300s_5_lower_forearm.stl"
            scale="0.001 0.001 0.001" />
        </geometry>
      </collision>
      <inertial>
        <origin rpy="${pi} 0 0" xyz="0.0513477000 0.0068046200 0" />
        <mass value="0.414823" />
        <inertia ixx="0.0001753000" iyy="0.0005269000" izz="0.0005911000" ixy="-0.0000852800"
          ixz="0" iyz="0" />
      </inertial>
    </link>

    <joint name="${prefix}wrist_angle" type="revolute">
      <axis xyz="0 1 0" />
      <limit effort="5" lower="${radians(-107)}" upper="${radians(128)}" velocity="${pi}" />
      <origin rpy="0 0 0" xyz="0.1 0 0" />
      <parent link="${prefix}lower_forearm_link" />
      <child link="${prefix}wrist_link" />
      <dynamics friction="0.1" />
    </joint>

    <link name="${prefix}wrist_link">
      <visual>
        <origin rpy="0 0 ${pi/2}" xyz="0 0 0" />
        <geometry>
          <mesh
            filename="package://interbotix_xsarm_descriptions/meshes/vx300s_meshes/vx300s_6_wrist.stl"
            scale="0.001 0.001 0.001" />
        </geometry>
        <material name="interbotix_black" />
      </visual>
      <collision>
        <origin rpy="0 0 ${pi/2}" xyz="0 0 0" />
        <geometry>
          <mesh
            filename="package://interbotix_xsarm_descriptions/meshes/vx300s_meshes/vx300s_6_wrist.stl"
            scale="0.001 0.001 0.001" />
        </geometry>
      </collision>
      <inertial>
        <origin rpy="0 0 ${pi/2}" xyz="0.0467430000 -0.0000076652 0.0105650000" />
        <mass value="0.115395" />
        <inertia ixx="0.0000463100" iyy="0.0000451400" izz="0.0000527000" ixy="0.0000000195"
          ixz="0.0000000023" iyz="0.0000042002" />
      </inertial>
    </link>

    <joint name="${prefix}wrist_rotate" type="revolute">
      <axis xyz="1 0 0" />
      <limit effort="1" lower="${-pi/2 + pi_offset}" upper="${pi/2 - pi_offset}" velocity="${pi}" />
      <origin rpy="0 0 0" xyz="0.069744 0 0" />
      <parent link="${prefix}wrist_link" />
      <child link="${prefix}gripper_link" />
      <dynamics friction="0.1" />
    </joint>

    <link name="${prefix}gripper_link">
      <visual>
        <origin rpy="0 0 ${pi/2}" xyz="-0.02 0 0" />
        <geometry>
          <mesh
            filename="package://interbotix_xsarm_descriptions/meshes/vx300s_meshes/vx300s_7_gripper.stl"
            scale="0.001 0.001 0.001" />
        </geometry>
        <material name="interbotix_black" />
      </visual>
      <collision>
        <origin rpy="0 0 ${pi/2}" xyz="-0.02 0 0" />
        <geometry>
          <mesh
            filename="package://interbotix_xsarm_descriptions/meshes/vx300s_meshes/vx300s_7_gripper.stl"
            scale="0.001 0.001 0.001" />
        </geometry>
      </collision>
      <inertial>
        <origin rpy="0 0 ${pi/2}" xyz="0.0230010000 0.0000000000 0.0115230000" />
        <mass value="0.097666" />
        <inertia ixx="0.0000326800" iyy="0.0000243600" izz="0.0000211900" ixy="0.0000000000"
          ixz="0.0000000000" iyz="0.0000002785" />
      </inertial>
    </link>

    <joint name="${prefix}ee_arm" type="fixed">
      <axis xyz="1 0 0" />
      <origin rpy="0 0 0" xyz="0.042825 0 0" />
      <parent link="${prefix}gripper_link" />
      <child link="${prefix}ee_arm_link" />
    </joint>

    <!-- no visual -->
    <link name="${prefix}ee_arm_link">
      <inertial>
        <mass value="0.001" />
        <inertia ixx="0.0001" iyy="0.0001" izz="0.0001" ixy="0" ixz="0" iyz="0" />
      </inertial>
    </link>

    <joint name="${prefix}gripper_bar" type="fixed">
      <axis xyz="1 0 0" />
      <origin rpy="0 0 0" xyz="0 0 0" />
      <parent link="${prefix}ee_arm_link" />
      <child link="${prefix}gripper_bar_link" />
    </joint>

    <link name="${prefix}gripper_bar_link">
      <visual>
        <origin rpy="0 0 ${pi/2}" xyz="-0.063 0 0" />
        <geometry>
          <mesh
            filename="package://interbotix_xsarm_descriptions/meshes/vx300s_meshes/vx300s_9_gripper_bar.stl"
            scale="0.001 0.001 0.001" />
        </geometry>
        <material name="interbotix_black" />
      </visual>
      <collision>
        <origin rpy="0 0 ${pi/2}" xyz="-0.063 0 0" />
        <geometry>
          <mesh
            filename="package://interbotix_xsarm_descriptions/meshes/vx300s_meshes/vx300s_9_gripper_bar.stl"
            scale="0.001 0.001 0.001" />
        </geometry>
      </collision>
      <inertial>
        <origin rpy="0 0 ${pi/2}" xyz="0.0067940000 -0.0000004272 -0.0007760000" />
        <mass value="0.150986" />
        <inertia ixx="0.0000789500" iyy="0.0003283000" izz="0.0003095000" ixy="-0.0000000012"
          ixz="0.0000001341" iyz="0.0000017465" />
      </inertial>
    </link>

    <joint name="${prefix}ee_bar" type="fixed">
      <axis xyz="1 0 0" />
      <origin rpy="0 0 0" xyz="0.025875 0 0" />
      <parent link="${prefix}gripper_bar_link" />
      <child link="${prefix}fingers_link" />
    </joint>
    
    <!-- no visual -->
    <link name="${prefix}fingers_link">
      <inertial>
        <mass value="0.001" />
        <inertia ixx="0.0001" iyy="0.0001" izz="0.0001" ixy="0" ixz="0" iyz="0" />
      </inertial>
    </link>

    <joint name="${prefix}left_finger" type="prismatic">
      <axis xyz="0 1 0" />
      <limit effort="5" lower="0.021" upper="0.057" velocity="1" />
      <origin rpy="0 0 0" xyz="0 0 0" />
      <parent link="${prefix}fingers_link" />
      <child link="${prefix}left_finger_link" />
      <dynamics friction="0.1" />
    </joint>

    <link name="${prefix}left_finger_link">
      <visual>
        <origin rpy="${pi/2} ${-pi} ${pi/2}" xyz="-0.0404 -0.0575 0" />
        <geometry>
          <mesh
            filename="package://interbotix_xsarm_descriptions/meshes/vx300s_meshes/vx300s_10_gripper_finger.stl"
            scale="0.001 0.001 0.001" />
        </geometry>
        <material name="interbotix_black" />
      </visual>
      <collision>
        <origin rpy="${pi/2} ${-pi} ${pi/2}" xyz="-0.0404 -0.0575 0" />
        <geometry>
          <mesh
            filename="package://interbotix_xsarm_descriptions/meshes/vx300s_meshes/vx300s_10_gripper_finger.stl"
            scale="0.001 0.001 0.001" />
        </geometry>
      </collision>
      <inertial>
        <origin rpy="${pi} ${pi} ${pi/2}" xyz="0.0173440000 -0.0060692000 0.0000000000" />
        <mass value="0.034796" />
        <inertia ixx="0.0000243300" iyy="0.0000125500" izz="0.0000141700" ixy="-0.0000024004"
          ixz="0.0000000000" iyz="0.0000000000" />
      </inertial>
    </link>

    <joint name="${prefix}right_finger" type="prismatic">
      <axis xyz="0 1 0" />
      <limit effort="5" lower="-0.057" upper="-0.021" velocity="1" />
      <origin rpy="0 0 0" xyz="0 0 0" />
      <parent link="${prefix}fingers_link" />
      <child link="${prefix}right_finger_link" />
      <dynamics friction="0.1" />
      <mimic joint="${prefix}left_finger" multiplier="-1" offset="0" />
    </joint>

    <link name="${prefix}right_finger_link">
      <visual>
        <origin rpy="${-pi/2} ${pi} ${-pi/2}" xyz="-0.0404 0.0575 0 " />
        <geometry>
          <mesh
            filename="package://interbotix_xsarm_descriptions/meshes/vx300s_meshes/vx300s_10_gripper_finger.stl"
            scale="0.001 0.001 0.001" />
        </geometry>
        <material name="interbotix_black" />
      </visual>
      <collision>
        <origin rpy="${-pi/2} ${pi} ${-pi/2}" xyz="-0.0404 0.0575 0 " />
        <geometry>
          <mesh
            filename="package://interbotix_xsarm_descriptions/meshes/vx300s_meshes/vx300s_10_gripper_finger.stl"
            scale="0.001 0.001 0.001" />
        </geometry>
      </collision>
      <inertial>
        <origin rpy="0 ${pi} ${pi/2}" xyz="0.0173440000 0.0060692000  0.0000000000" />
        <mass value="0.034796" />
        <inertia ixx="0.0000243300" iyy="0.0000125500" izz="0.0000141700" ixy="0.0000024001"
          ixz="0.0000000000" iyz="0.0000000000" />
      </inertial>
    </link>

    <joint name="${prefix}ee_gripper" type="fixed">
      <axis xyz="1 0 0" />
      <origin rpy="0 0 0" xyz="0.0385 0 0" />
      <parent link="${prefix}fingers_link" />
      <child link="${prefix}ee_gripper_link" />
    </joint>

    <!-- no visual -->    
    <link name="${prefix}ee_gripper_link">
      <inertial>
        <mass value="0.001" />
        <inertia ixx="0.0001" iyy="0.0001" izz="0.0001" ixy="0" ixz="0" iyz="0" />
      </inertial>
    </link>


  </xacro:macro>
</robot>