<?xml version='1.0' encoding='utf-8'?>
<robot name="so_arm100">
  
  <!-- Materials -->
  <material name="3d_printed">
    <color rgba="1.0 0.82 0.12 1.0"/>
  </material>
  <material name="sts3215">
    <color rgba="0.1 0.1 0.1 1.0"/>
  </material>

  <!-- Link base -->
  <link name="base">
    <inertial>
      <!-- Approximate inertial values -->
      <mass value="1.0"/>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="assets/Base.stl"/>
      </geometry>
      <material name="3d_printed"/>
    </visual>
    <visual>
      <geometry>
        <mesh filename="assets/Base_Motor.stl"/>
      </geometry>
      <material name="sts3215"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="assets/Base.stl"/>
      </geometry>
    </collision>
  </link>

  <!-- Link shoulder -->
  <link name="shoulder">
    <inertial>
      <mass value="0.119226"/>
      <origin xyz="-9.07886e-05 0.0590972 0.031089" rpy="0 0 0"/>
      <inertia ixx="5.94278e-05" ixy="0" ixz="0" iyy="5.89975e-05" iyz="0" izz="3.13712e-05"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="assets/Rotation_Pitch.stl"/>
      </geometry>
      <material name="3d_printed"/>
    </visual>
    <visual>
      <geometry>
        <mesh filename="assets/Rotation_Pitch_Motor.stl"/>
      </geometry>
      <material name="sts3215"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="assets/Rotation_Pitch.stl"/>
      </geometry>
    </collision>
  </link>

  <!-- Link upper_arm -->
  <link name="upper_arm">
    <inertial>
      <mass value="0.162409"/>
      <origin xyz="-1.72052e-05 0.0701802 0.00310545" rpy="0 0 0"/>
      <inertia ixx="0.000213312" ixy="0" ixz="0" iyy="0.000167164" iyz="0" izz="7.01522e-05"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="assets/Upper_Arm.stl"/>
      </geometry>
      <material name="3d_printed"/>
    </visual>
    <visual>
      <geometry>
        <mesh filename="assets/Upper_Arm_Motor.stl"/>
      </geometry>
      <material name="sts3215"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="assets/Upper_Arm.stl"/>
      </geometry>
    </collision>
  </link>

  <!-- Link lower_arm -->
  <link name="lower_arm">
    <inertial>
      <mass value="0.147968"/>
      <origin xyz="-0.00339604 0.00137796 0.0768007" rpy="0 0 0"/>
      <inertia ixx="0.000138803" ixy="0" ixz="0" iyy="0.000107748" iyz="0" izz="4.84242e-05"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="assets/Lower_Arm.stl"/>
      </geometry>
      <material name="3d_printed"/>
    </visual>
    <visual>
      <geometry>
        <mesh filename="assets/Lower_Arm_Motor.stl"/>
      </geometry>
      <material name="sts3215"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="assets/Lower_Arm.stl"/>
      </geometry>
    </collision>
  </link>

  <!-- Link wrist -->
  <link name="wrist">
    <inertial>
      <mass value="0.0661321"/>
      <origin xyz="-0.00852653 -0.0352279 -2.34622e-05" rpy="0 0 0"/>
      <inertia ixx="3.45403e-05" ixy="0" ixz="0" iyy="2.39041e-05" iyz="0" izz="1.94704e-05"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="assets/Wrist_Pitch_Roll.stl"/>
      </geometry>
      <material name="3d_printed"/>
    </visual>
    <visual>
      <geometry>
        <mesh filename="assets/Wrist_Pitch_Roll_Motor.stl"/>
      </geometry>
      <material name="sts3215"/>
    </visual>
    <collision>
      <geometry>
        <mesh filename="assets/Wrist_Pitch_Roll.stl"/>
      </geometry>
    </collision>
  </link>

  <!-- Link gripper -->
  <link name="gripper">
    <inertial>
      <mass value="0.0929859"/>
      <origin xyz="0.00552377 -0.0280167 0.000483583" rpy="0 0 0"/>
      <inertia ixx="5.03136e-05" ixy="0" ixz="0" iyy="4.64098e-05" iyz="0" izz="2.72961e-05"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="assets/Fixed_Jaw.stl"/>
      </geometry>
      <material name="3d_printed"/>
    </visual>
    <visual>
      <geometry>
        <mesh filename="assets/Fixed_Jaw_Motor.stl"/>
      </geometry>
      <material name="sts3215"/>
    </visual>
  </link>

  <!-- Link jaw -->
  <link name="jaw">
    <inertial>
      <mass value="0.0202444"/>
      <origin xyz="-0.00161745 -0.0303473 0.000449646" rpy="0 0 0"/>
      <inertia ixx="1.11265e-05" ixy="0" ixz="0" iyy="8.99651e-06" iyz="0" izz="2.99548e-06"/>
    </inertial>
    <visual>
      <geometry>
        <mesh filename="assets/Moving_Jaw.stl"/>
      </geometry>
      <material name="3d_printed"/>
    </visual>
  </link>


  <!-- Joint from base to shoulder -->
  <joint name="shoulder_pan" type="revolute">
    <parent link="base"/>
    <child link="shoulder"/>
    <origin xyz="0 -0.0452 0.0165" rpy="1.57079 0 0"/>
    <axis xyz="0 1 0"/>
    <limit lower="-2" upper="2" effort="35" velocity="1"/>
  </joint>

  <transmission name="shoulder_pan_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="shoulder_pan">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor1">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

  <!-- Joint from shoulder to upper_arm -->
  <joint name="shoulder_lift" type="revolute">
    <parent link="shoulder"/>
    <child link="upper_arm"/>
    <origin xyz="0 0.1025 0.0306" rpy="-1.8 0 0"/>
    <axis xyz="1 0 0"/>
    <limit lower="0" upper="3.5" effort="35" velocity="1"/>
  </joint>

  <transmission name="shoulder_lift_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="shoulder_lift">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor2">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

  <!-- Joint from upper_arm to lower_arm -->
  <joint name="elbow_flex" type="revolute">
    <parent link="upper_arm"/>
    <child link="lower_arm"/>
    <origin xyz="0 0.11257 0.028" rpy="1.57079 0 0"/>
    <axis xyz="1 0 0"/>
    <limit lower="-3.14158" upper="0" effort="35" velocity="1"/>
  </joint>

  <transmission name="elbow_flex_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="elbow_flex">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor3">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

  <!-- Joint from lower_arm to wrist -->
  <joint name="wrist_flex" type="revolute">
    <parent link="lower_arm"/>
    <child link="wrist"/>
    <origin xyz="0 0.0052 0.1349" rpy="-1 0 0"/>
    <axis xyz="1 0 0"/>
    <limit lower="-2.5" upper="1.2" effort="35" velocity="1"/>
  </joint>

  <transmission name="wrist_flex_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="wrist_flex">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor4">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

  <!-- Joint from wrist to gripper -->
  <joint name="wrist_roll" type="revolute">
    <parent link="wrist"/>
    <child link="gripper"/>
    <origin xyz="0 -0.0601 0" rpy="0 1.57079 0"/>
    <axis xyz="0 1 0"/>
    <limit lower="-3.14158" upper="3.14158" effort="35" velocity="1"/>
  </joint>

  <transmission name="wrist_roll_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="wrist_roll">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor5">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

  <!-- Joint from gripper to jaw -->
  <joint name="gripper" type="revolute">
    <parent link="gripper"/>
    <child link="jaw"/>
    <origin xyz="-0.0202 -0.0244 0" rpy="0 3.14158 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="-0.2" upper="2.0" effort="35" velocity="1"/>
  </joint>

  <transmission name="gripper_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="gripper">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor6">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

</robot> 