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

  <xacro:property name="robot_model"
    value="wx250s_custom"
    scope="global"/>
  <xacro:property name="dof"
    value="6"
    scope="global"/>

  <xacro:arg name="robot_name"                      default="${robot_model}"/>
  <xacro:arg name="base_link_frame"                 default="base_link"/>
  <xacro:arg name="use_world_frame"                 default="true"/>
  <xacro:arg name="use_gripper"                     default="false"/>
  <xacro:arg name="external_urdf_loc"               default=""/>
  <xacro:arg name="hardware_type"                   default="actual"/>

  <xacro:property name="mesh_directory"             value="package://interbotix_xsarm_descriptions/meshes/${robot_model}_meshes"/>
  <xacro:property name="urdf_loc"                   value="$(arg external_urdf_loc)"/>
  <xacro:property name="hardware_type"              value="$(arg hardware_type)"/>
  <xacro:property name="pi_offset"                  value="0.00001"/>

  <xacro:property name="waist_limit_effort"         value="10"/> 
  <xacro:property name="waist_limit_lower"          value="${-pi + pi_offset}"/>
  <xacro:property name="waist_limit_upper"          value="${pi - pi_offset}"/>
  <xacro:property name="waist_limit_vel"            value="${pi}"/>

  <xacro:property name="shoulder_limit_effort"      value="100"/><!-- 20 -->
  <xacro:property name="shoulder_limit_lower"       value="${radians(-108)}"/>
  <xacro:property name="shoulder_limit_upper"       value="${radians(114)}"/>
  <xacro:property name="shoulder_limit_vel"         value="${pi}"/>

  <xacro:property name="elbow_limit_effort"         value="15"/>
  <xacro:property name="elbow_limit_lower"          value="${radians(-123)}"/>
  <xacro:property name="elbow_limit_upper"          value="${radians(92)}"/>
  <xacro:property name="elbow_limit_vel"            value="${pi}"/>

  <xacro:property name="forearm_roll_limit_effort"   value="2"/>
  <xacro:property name="forearm_roll_limit_lower"    value="${-pi + pi_offset}"/>
  <xacro:property name="forearm_roll_limit_upper"    value="${pi - pi_offset}"/>
  <xacro:property name="forearm_roll_limit_vel"      value="${pi}"/>

  <xacro:property name="wrist_angle_limit_effort"   value="5"/>
  <xacro:property name="wrist_angle_limit_lower"    value="${radians(-100)}"/>
  <xacro:property name="wrist_angle_limit_upper"    value="${radians(123)}"/>
  <xacro:property name="wrist_angle_limit_vel"      value="${pi}"/>

  <xacro:property name="wrist_rotate_limit_effort"  value="1"/>
  <xacro:property name="wrist_rotate_limit_lower"   value="${-pi + pi_offset}"/>
  <xacro:property name="wrist_rotate_limit_upper"   value="${pi - pi_offset}"/>
  <xacro:property name="wrist_rotate_limit_vel"     value="${pi}"/>

  <xacro:property name="gripper_limit_vel"          value="${pi}"/>

  <xacro:property name="finger_limit_effort"        value="5"/>
  <xacro:property name="finger_limit_lower"         value="0.015"/>
  <xacro:property name="finger_limit_upper"         value="0.037"/>
  <xacro:property name="finger_limit_vel"           value="1"/>

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

  <xacro:if value="$(arg use_world_frame)">
    <link name="world"/>
    <joint name="fixed" type="fixed">
      <parent
        link="world"/>
      <child
        link="$(arg robot_name)/$(arg base_link_frame)"/>
    </joint>
  </xacro:if> <!-- use_world_frame -->

  <link name="$(arg robot_name)/$(arg base_link_frame)">
    <visual>
      <origin
        rpy="0 0 ${pi/2}"
        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 ${pi/2}"
        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 ${pi/2}"
        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="${waist_limit_effort}"
      lower="${waist_limit_lower}"
      upper="${waist_limit_upper}"
      velocity="${waist_limit_vel}"/>
    <origin
      rpy="0 0 0"
      xyz="0 0 0.072"/>
    <parent
      link="$(arg robot_name)/$(arg base_link_frame)"/>
    <child
      link="$(arg robot_name)/shoulder_link"/>
    <dynamics
      friction="0.1"
      damping="0.1"/>
  </joint>

  <link name="$(arg robot_name)/shoulder_link">
    <visual>
      <origin
        rpy="0 0 ${pi/2}"
        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 ${pi/2}"
        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 ${pi/2}"
        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="${shoulder_limit_effort}"
      lower="${shoulder_limit_lower}"
      upper="${shoulder_limit_upper}"
      velocity="${shoulder_limit_vel}"/>
    <origin
      rpy="0 0 0"
      xyz="0 0 0.03865"/>
    <parent
      link="$(arg robot_name)/shoulder_link"/>
    <child
      link="$(arg robot_name)/upper_arm_link"/>
    <dynamics
      friction="0.1"
      damping="0.1"/>
  </joint>

  <link name="$(arg robot_name)/upper_arm_link">
    <visual>
      <origin
        rpy="0 0 ${pi/2}"
        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 ${pi/2}"
        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 ${pi/2}"
        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="${elbow_limit_effort}"
      lower="${elbow_limit_lower}"
      upper="${elbow_limit_upper}"
      velocity="${elbow_limit_vel}"/>
    <origin
      rpy="0 0 0"
      xyz="0.04975 0 0.25"/>
    <parent
      link="$(arg robot_name)/upper_arm_link"/>
    <child
      link="$(arg robot_name)/upper_forearm_link"/>
    <dynamics
      friction="0.1"
      damping="0.1"/>
  </joint>

  <link name="$(arg robot_name)/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="${forearm_roll_limit_effort}"
      lower="${forearm_roll_limit_lower}"
      upper="${forearm_roll_limit_upper}"
      velocity="${forearm_roll_limit_vel}"/>
    <origin
      rpy="0 0 0"
      xyz="0.175 0 0"/>
    <parent
      link="$(arg robot_name)/upper_forearm_link"/>
    <child
      link="$(arg robot_name)/lower_forearm_link"/>
    <dynamics
      friction="0.1"
      damping="0.1"/>
  </joint>

  <link name="$(arg robot_name)/lower_forearm_link">
    <visual>
      <origin
        rpy="${pi} 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="${pi} 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="${pi} 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="${wrist_angle_limit_effort}"
      lower="${wrist_angle_limit_lower}"
      upper="${wrist_angle_limit_upper}"
      velocity="${wrist_angle_limit_vel}"/>
    <origin
      rpy="0 0 0"
      xyz="0.075 0 0"/>
    <parent
      link="$(arg robot_name)/lower_forearm_link"/>
    <child
      link="$(arg robot_name)/wrist_link"/>
    <dynamics
      friction="0.1"
      damping="0.1"/>
  </joint>

  <link name="$(arg robot_name)/wrist_link">
    <visual>
      <origin
        rpy="0 0 ${pi/2}"
        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 ${pi/2}"
        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 ${pi/2}"
        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>

    <joint name="wrist_rotate" type="revolute">
      <axis xyz="1 0 0"/>
      <limit
        effort="${wrist_rotate_limit_effort}"
        lower="${wrist_rotate_limit_lower}"
        upper="${wrist_rotate_limit_upper}"
        velocity="${wrist_rotate_limit_vel}"/>
      <origin
        rpy="0 0 0"
        xyz="0.065 0 0"/>
      <parent
        link="$(arg robot_name)/wrist_link"/>
      <child
        link="$(arg robot_name)/sma_attach_link"/>
      <dynamics
        friction="0.1"
        damping="0.1"/>
    </joint>

    <link name="$(arg robot_name)/sma_attach_link">
      <visual>
        <origin
          rpy="0 0 ${pi/2}"
          xyz="0.007 0 0"/>
        <geometry>
          <mesh
            filename="${mesh_directory}/attach.stl"
            scale="0.001 0.001 0.001"/>
        </geometry>
        <material name="interbotix_black"/>
      </visual>
      <collision>
        <origin
          rpy="0 0 ${pi/2}"
          xyz="0.007 0 0"/>
        <geometry>
          <mesh
            filename="${mesh_directory}/attach.stl"
            scale="0.001 0.001 0.001"/>
        </geometry>
      </collision>
      <inertial>
        <origin
          rpy="0 0 ${pi/2}"
          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="sma" type="fixed">
      <axis xyz="1 0 0"/>
      <origin
        rpy="0 0 0"
        xyz="0.196 0 0"/>
      <parent
        link="$(arg robot_name)/sma_attach_link"/>
      <child
        link="$(arg robot_name)/sma_link"/>
    </joint>

    <link name="$(arg robot_name)/sma_link">
      <visual>
        <origin
          rpy="0 0 0"
          xyz="0 0 0"/>
        <geometry>
          <mesh
            filename="${mesh_directory}/sma.stl"
            scale="0.001 0.001 0.001"/>
        </geometry>
      </visual>
      <collision>
        <origin
          rpy="0 0 0"
          xyz="0 0 0"/>
        <geometry>
          <mesh
            filename="${mesh_directory}/sma.stl"
            scale="0.001 0.001 0.001"/>
        </geometry>
      </collision>
      <inertial>
        <mass value="0.1"/>
        <inertia
          ixx="0.001"
          iyy="0.001"
          izz="0.001"
          ixy="0"
          ixz="0"
          iyz="0"/>
      </inertial>
    </link>
    
    <joint name="sma2" type="fixed">
      <axis xyz="1 0 0"/>
      <origin
        rpy="0 0 0"
        xyz="0 0 0"/>
      <parent
        link="$(arg robot_name)/sma_link"/>
      <child
        link="$(arg robot_name)/sma_link2"/>
    </joint>
    
    <link name="$(arg robot_name)/sma_link2">
      <visual>
        <origin
          rpy="0 0 0"
          xyz="0 0 0"/>
        <geometry>
          <mesh
            filename="${mesh_directory}/sma.stl"
            scale="0.001 0.001 0.001"/>
        </geometry>
      </visual>
      <collision>
        <origin
          rpy="0 0 0"
          xyz="0 0 0"/>
        <geometry>
          <mesh
            filename="${mesh_directory}/sma.stl"
            scale="0.001 0.001 0.001"/>
        </geometry>
      </collision>
      <inertial>
        <mass value="0"/>
        <inertia
          ixx="0.001"
          iyy="0.001"
          izz="0.001"
          ixy="0"
          ixz="0"
          iyz="0"/>
      </inertial>
    </link>

  <xacro:if value="${hardware_type == 'gz_classic'}">
    <xacro:gazebo_configs/>
  </xacro:if>

  <xacro:if value="${urdf_loc != ''}">
    <xacro:include filename="${urdf_loc}"/>
  </xacro:if>

</robot>
