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

  <material name="interbotix_black">
    <texture filename="package://interbotix_xsarm_descriptions/meshes/interbotix_black.png"/>
  </material>


    <link name="world"/>
    <joint name="fixed" type="fixed">
      <parent
        link="world"/>
      <child
        link="wx250s_custom/base_link"/>
    </joint>


  <link name="wx250s_custom/base_link">
    <visual>
      <origin
        rpy="0 0 1.5707963267948966"
        xyz="0 0 0"/>
      <geometry>
        <mesh
          filename="${mesh_directory}/base.stl"
          scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="interbotix_black"/>
    </visual>
    <collision>
      <origin
        rpy="0 0 1.5707963267948966"
        xyz="0 0 0"/>
      <geometry>
        <mesh
          filename="${mesh_directory}/base.stl"
          scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
    <inertial>
      <origin
        rpy="0 0 1.5707963267948966"
        xyz="-0.0380446000 0.0006138920 0.0193354000"/>
      <mass value="0.538736"/>
      <inertia
        ixx="0.0021150000"
        iyy="0.0006921000"
        izz="0.0025240000"
        ixy="-0.0000163500"
        ixz="0.0000006998"
        iyz="0.0000464200"/>
    </inertial>
  </link>

  <joint name="waist" type="revolute">
    <axis xyz="0 0 1"/>
    <limit
      effort="10"
      lower="-3.141582653589793"
      upper="3.141582653589793"
      velocity="3.141592653589793"/>
    <origin
      rpy="0 0 0"
      xyz="0 0 0.072"/>
    <parent
      link="wx250s_custom/base_link"/>
    <child
      link="wx250s_custom/shoulder_link"/>
    <dynamics
      friction="0.1"
      damping="0.1"/>
  </joint>

  <link name="wx250s_custom/shoulder_link">
    <visual>
      <origin
        rpy="0 0 1.5707963267948966"
        xyz="0 0 -0.003"/>
      <geometry>
        <mesh
          filename="${mesh_directory}/shoulder.stl"
          scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="interbotix_black"/>
    </visual>
    <collision>
      <origin
        rpy="0 0 1.5707963267948966"
        xyz="0 0 -0.003"/>
      <geometry>
        <mesh
          filename="${mesh_directory}/shoulder.stl"
          scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
    <inertial>
      <origin
        rpy="0 0 1.5707963267948966"
        xyz="0.0000223482 0.0000414609 0.0066287000"/>
      <mass value="0.480879"/>
      <inertia
        ixx="0.0003790000"
        iyy="0.0005557000"
        izz="0.0005889000"
        ixy="0.0000000022"
        ixz="-0.0000003561"
        iyz="0.0000012245"/>
    </inertial>
  </link>

  <joint name="shoulder" type="revolute">
    <axis xyz="0 1 0"/>
    <limit
      effort="20"
      lower="-1.8849555921538759"
      upper="-1.9896753472735356"
      velocity="3.141592653589793"/>
    <origin
      rpy="0 0 0"
      xyz="0 0 0.03865"/>
    <parent
      link="wx250s_custom/shoulder_link"/>
    <child
      link="wx250s_custom/upper_arm_link"/>
    <dynamics
      friction="0.1"
      damping="0.1"/>
  </joint>

  <link name="wx250s_custom/upper_arm_link">
    <visual>
      <origin
        rpy="0 0 1.5707963267948966"
        xyz="0 0 0"/>
      <geometry>
        <mesh
          filename="${mesh_directory}/upper_arm.stl"
          scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="interbotix_black"/>
    </visual>
    <collision>
      <origin
        rpy="0 0 1.5707963267948966"
        xyz="0 0 0"/>
      <geometry>
        <mesh
          filename="${mesh_directory}/upper_arm.stl"
          scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
    <inertial>
      <origin
        rpy="0 0 1.5707963267948966"
        xyz="0.0171605000 0.0000002725 0.1913230000"/>
      <mass value="0.430811"/>
      <inertia
        ixx="0.0034630000"
        iyy="0.0035870000"
        izz="0.0004566000"
        ixy="-0.0000000001"
        ixz="-0.0000000002"
        iyz="0.0004272000"/>
    </inertial>
  </link>

  <joint name="elbow" type="revolute">
    <axis xyz="0 1 0"/>
    <limit
      effort="15"
      lower="-2.1467549799530254"
      upper="1.605702911834783"
      velocity="3.141592653589793"/>
    <origin
      rpy="0 0 0"
      xyz="0.04975 0 0.25"/>
    <parent
      link="wx250s_custom/upper_arm_link"/>
    <child
      link="wx250s_custom/upper_forearm_link"/>
    <dynamics
      friction="0.1"
      damping="0.1"/>
  </joint>

  <link name="wx250s_custom/upper_forearm_link">
    <visual>
      <origin
        rpy="0 0 0"
        xyz="0 0 0"/>
      <geometry>
        <mesh
          filename="${mesh_directory}/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="${mesh_directory}/upper_forearm.stl"
          scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
    <inertial>
      <origin
        rpy="0 0 0"
        xyz="0.1079630000 0.0001158760 0"/>
      <mass value="0.234589"/>
      <inertia
        ixx="0.0000397100"
        iyy="0.0008878000"
        izz="0.0008880000"
        ixy="0.0000023528"
        ixz="0"
        iyz="0"/>
    </inertial>
  </link>

  <joint name="forearm_roll" type="revolute">
    <axis xyz="1 0 0"/>
    <limit
      effort="2"
      lower="-3.141582653589793"
      upper="3.141582653589793"
      velocity="3.141592653589793"/>
    <origin
      rpy="0 0 0"
      xyz="0.175 0 0"/>
    <parent
      link="wx250s_custom/upper_forearm_link"/>
    <child
      link="wx250s_custom/lower_forearm_link"/>
    <dynamics
      friction="0.1"
      damping="0.1"/>
  </joint>

  <link name="wx250s_custom/lower_forearm_link">
    <visual>
      <origin
        rpy="3.141592653589793 0 0"
        xyz="0 0 0"/>
      <geometry>
        <mesh
          filename="${mesh_directory}/lower_forearm.stl"
          scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="interbotix_black"/>
    </visual>
    <collision>
      <origin
        rpy="3.141592653589793 0 0"
        xyz="0 0 0"/>
      <geometry>
        <mesh
          filename="${mesh_directory}/lower_forearm.stl"
          scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
    <inertial>
      <origin
        rpy="3.141592653589793 0 0"
        xyz="0.0374395000 0.0052225200 0"/>
      <mass value="0.220991"/>
      <inertia
        ixx="0.0000636900"
        iyy="0.0001677000"
        izz="0.0001834000"
        ixy="-0.0000229200"
        ixz="0"
        iyz="0"/>
    </inertial>
  </link>

  <joint name="wrist_angle" type="revolute">
    <axis xyz="0 1 0"/>
    <limit
      effort="5"
      lower="-1.7453292519943295"
      upper="2.1467549799530254"
      velocity="3.141592653589793"/>
    <origin
      rpy="0 0 0"
      xyz="0.075 0 0"/>
    <parent
      link="wx250s_custom/lower_forearm_link"/>
    <child
      link="wx250s_custom/wrist_link"/>
    <dynamics
      friction="0.1"
      damping="0.1"/>
  </joint>

  <link name="wx250s_custom/wrist_link">
    <visual>
      <origin
        rpy="0 0 1.5707963267948966"
        xyz="0 0 0"/>
      <geometry>
        <mesh
          filename="${mesh_directory}/wrist.stl"
          scale="0.001 0.001 0.001"/>
      </geometry>
      <material name="interbotix_black"/>
    </visual>
    <collision>
      <origin
        rpy="0 0 1.5707963267948966"
        xyz="0 0 0"/>
      <geometry>
        <mesh
          filename="${mesh_directory}/wrist.stl"
          scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
    <inertial>
      <origin
        rpy="0 0 1.5707963267948966"
        xyz="0.0423600000 -0.0000106630 0.0105770000"/>
      <mass value="0.084957"/>
      <inertia
        ixx="0.0000308200"
        iyy="0.0000282200"
        izz="0.0000315200"
        ixy="0.0000000191"
        ixz="0.0000000023"
        iyz="0.0000025481"/>
    </inertial>
  </link>

  <!-- Include the gripper if used -->



    <joint name="wrist_rotate" type="revolute">
      <axis xyz="1 0 0"/>
      <limit
        effort="1"
        lower="-3.141582653589793"
        upper="3.141582653589793"
        velocity="3.141592653589793"/>
      <origin
        rpy="0 0 0"
        xyz="0.065 0 0"/>
      <parent
        link="wx250s_custom/wrist_link"/>
      <child
        link="wx250s_custom/gripper_link"/>
      <dynamics
        friction="0.1"
        damping="0.1"/>
    </joint>

    <link name="wx250s_custom/gripper_link">
      <visual>
        <origin
          rpy="0 0 1.5707963267948966"
          xyz="-0.02 0 0"/>
        <geometry>
          <mesh
            filename="${mesh_directory}/gripper.stl"
            scale="0.001 0.001 0.001"/>
        </geometry>
        <material name="interbotix_black"/>
      </visual>
      <collision>
        <origin
          rpy="0 0 1.5707963267948966"
          xyz="-0.02 0 0"/>
        <geometry>
          <mesh
            filename="${mesh_directory}/gripper.stl"
            scale="0.001 0.001 0.001"/>
        </geometry>
      </collision>
      <inertial>
        <origin
          rpy="0 0 1.5707963267948966"
          xyz="0.0216310000 0.0000002516 0.0114100000"/>
        <mass value="0.072885"/>
        <inertia
          ixx="0.0000253700"
          iyy="0.0000183600"
          izz="0.0000167400"
          ixy="0.0000000000"
          ixz="0.0000000000"
          iyz="0.0000004340"/>
      </inertial>
    </link>

    <joint name="ee_arm" type="fixed">
      <axis xyz="1 0 0"/>
      <origin
        rpy="0 0 0"
        xyz="0.043 0 0"/>
      <parent
        link="wx250s_custom/gripper_link"/>
      <child
        link="wx250s_custom/ee_arm_link"/>
    </joint>

    <link name="wx250s_custom/ee_arm_link">
      <inertial>
        <mass value="0.001"/>
        <inertia
          ixx="0.001"
          iyy="0.001"
          izz="0.001"
          ixy="0"
          ixz="0"
          iyz="0"/>
      </inertial>
    </link>

    <joint name="gripper" type="continuous">
      <axis xyz="1 0 0"/>
      <limit
        effort="1"
        velocity="3.141592653589793"/>
      <origin
        rpy="0 0 0"
        xyz="0.0055 0 0"/>
      <parent
        link="wx250s_custom/ee_arm_link"/>
      <child
        link="wx250s_custom/gripper_prop_link"/>
      <dynamics
        friction="0.1"
        damping="0.1"/>
    </joint>

    <link name="wx250s_custom/gripper_prop_link">
      <visual>
        <origin
          rpy="0 0 1.5707963267948966"
          xyz="-0.0685 0 0"/>
        <geometry>
          <mesh
            filename="${mesh_directory}/gripper_prop.stl"
            scale="0.001 0.001 0.001"/>
        </geometry>
        <material name="interbotix_black"/>
      </visual>
      <collision>
        <origin
          rpy="0 0 1.5707963267948966"
          xyz="-0.0685 0 0"/>
        <geometry>
          <mesh
            filename="${mesh_directory}/gripper_prop.stl"
            scale="0.001 0.001 0.001"/>
        </geometry>
      </collision>
      <inertial>
        <origin
          rpy="0 0 1.5707963267948966"
          xyz="0.0008460000 -0.0000016817 0.0000420000"/>
        <mass value="0.00434"/>
        <inertia
          ixx="0.0000005923"
          iyy="0.0000011156"
          izz="0.0000005743"
          ixy="0.0000000000"
          ixz="0.0000003195"
          iyz="-0.0000000004"/>
      </inertial>
    </link>

    <!-- If the AR tag is being used, then add the AR tag mount -->




    <!-- If the gripper bar is being used, then also add the gripper bar -->



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

      <link name="wx250s_custom/gripper_bar_link">
        <visual>
          <origin
            rpy="0 0 1.5707963267948966"
            xyz="-0.063 0 0"/>
          <geometry>
            <mesh
              filename="${mesh_directory}/gripper_bar.stl"
              scale="0.001 0.001 0.001"/>
          </geometry>
          <material name="interbotix_black"/>
        </visual>
        <collision>
          <origin
            rpy="0 0 1.5707963267948966"
            xyz="-0.063 0 0"/>
          <geometry>
            <mesh
              filename="${mesh_directory}/gripper_bar.stl"
              scale="0.001 0.001 0.001"/>
          </geometry>
        </collision>
        <inertial>
          <origin
            rpy="0 0 1.5707963267948966"
            xyz="0.0096870000 0.0000008177 0.0049620000"/>
          <mass value="0.034199"/>
          <inertia
            ixx="0.0000074125"
            iyy="0.0000284300"
            izz="0.0000286000"
            ixy="-0.0000000008"
            ixz="-0.0000000006"
            iyz="-0.0000013889"/>
        </inertial>
      </link>

      <joint name="ee_bar" type="fixed">
        <axis xyz="1 0 0"/>
        <origin
          rpy="0 0 0"
          xyz="0.023 0 0"/>
        <parent
          link="wx250s_custom/gripper_bar_link"/>
        <child
          link="wx250s_custom/fingers_link"/>
      </joint>

      <link name="wx250s_custom/fingers_link">
        <inertial>
          <mass value="0.001"/>
          <inertia
            ixx="0.001"
            iyy="0.001"
            izz="0.001"
            ixy="0"
            ixz="0"
            iyz="0"/>
        </inertial>
      </link>

      <!-- If the gripper fingers are being used, add those as well -->



        <joint name="left_finger" type="prismatic">
          <axis xyz="0 1 0"/>
          <limit
            effort="5"
            lower="0.015"
            upper="0.037"
            velocity="1"/>
          <origin
            rpy="0 0 0"
            xyz="0 0 0"/>
          <parent
            link="wx250s_custom/fingers_link"/>
          <child
            link="wx250s_custom/left_finger_link"/>
          <dynamics
            friction="0.1"
            damping="0.1"/>
        </joint>

        <link name="wx250s_custom/left_finger_link">
          <visual>
            <origin
              rpy="3.141592653589793 3.141592653589793 0"
              xyz="0 0.005 0"/>
            <geometry>
              <mesh
                filename="${mesh_directory}/gripper_finger.stl"
                scale="0.001 0.001 0.001"/>
            </geometry>
            <material name="interbotix_black"/>
          </visual>
          <collision>
            <origin
              rpy="3.141592653589793 3.141592653589793 0"
              xyz="0 0.005 0"/>
            <geometry>
              <mesh
                filename="${mesh_directory}/gripper_finger.stl"
                scale="0.001 0.001 0.001"/>
            </geometry>
          </collision>
          <inertial>
            <origin
              rpy="3.141592653589793 3.141592653589793 1.5707963267948966"
              xyz="0.0138160000 0.0000000000 0.0000000000"/>
            <mass value="0.016246"/>
            <inertia
              ixx="0.0000047310"
              iyy="0.0000015506"
              izz="0.0000037467"
              ixy="-0.0000004560"
              ixz="0.0000000000"
              iyz="0.0000000000"/>
          </inertial>
        </link>

        <joint name="right_finger" type="prismatic">
          <axis xyz="0 1 0"/>
          <limit
            effort="5"
            lower="-0.037"
            upper="-0.015"
            velocity="1"/>
          <origin
            rpy="0 0 0"
            xyz="0 0 0"/>
          <parent
            link="wx250s_custom/fingers_link"/>
          <child
            link="wx250s_custom/right_finger_link"/>
          <dynamics
            friction="0.1"
            damping="0.1"/>

            <mimic
              joint="left_finger"
              multiplier="-1"
              offset="0"/>

        </joint>

        <link name="wx250s_custom/right_finger_link">
          <visual>
            <origin
              rpy="0 3.141592653589793 0"
              xyz="0 -0.005 0"/>
            <geometry>
              <mesh
                filename="${mesh_directory}/gripper_finger.stl"
                scale="0.001 0.001 0.001"/>
            </geometry>
            <material name="interbotix_black"/>
          </visual>
          <collision>
            <origin
              rpy="0 3.141592653589793 0"
              xyz="0 -0.005 0"/>
            <geometry>
              <mesh
                filename="${mesh_directory}/gripper_finger.stl"
                scale="0.001 0.001 0.001"/>
            </geometry>
          </collision>
          <inertial>
            <origin
              rpy="0 3.141592653589793 1.5707963267948966"
              xyz="0.0138160000 0.0000000000  0.0000000000"/>
            <mass value="0.016246"/>
            <inertia
              ixx="0.0000047310"
              iyy="0.0000015506"
              izz="0.0000037467"
              ixy="0.0000004560"
              ixz="0.0000000000"
              iyz="0.0000000000"/>
          </inertial>
        </link>

        <joint name="ee_gripper" type="fixed">
          <axis xyz="1 0 0"/>
          <origin
            rpy="0 0 0"
            xyz="0.128 0 0"/>
          <parent
            link="wx250s_custom/fingers_link"/>
          <child
            link="wx250s_custom/ee_gripper_link"/>
        </joint>

        <link name="wx250s_custom/ee_gripper_link">
          <inertial>
            <mass value="0.001"/>
            <inertia
              ixx="0.001"
              iyy="0.001"
              izz="0.001"
              ixy="0"
              ixz="0"
              iyz="0"/>
          </inertial>
        </link>

</robot>
