<?xml version="1.0" encoding="utf-8"?>
<robot name="smallrobotarm">
  
  <!-- Base Link -->
  <link name="base">
    <inertial>
      <origin xyz="0.0798346870153417 0.0433232349939573 -1.02988227219215E-07" rpy="0 0 0" />
      <mass value="0.707769774040757" />
      <inertia ixx="0.000506653131023491" ixy="-1.41343083468259E-06" ixz="-3.24600458978432E-10" 
               iyy="0.00156991777339284" iyz="1.90139870225094E-09" izz="0.00131591710050895" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="/Users/brucezhao/mit/smallrobotarm/meshes/base.obj" />
      </geometry>
      <material name="base_color">
        <color rgba="0.792 0.820 0.933 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="/Users/brucezhao/mit/smallrobotarm/meshes/base.obj" />
      </geometry>
    </collision>
  </link>

  <!-- Joint 1 -->
  <link name="link1">
    <inertial>
      <origin xyz="0.00437876253723003 0.0425110342831608 0.0017253451974267" rpy="0 0 0" />
      <mass value="0.921341505953049" />
      <inertia ixx="0.00100174129209879" ixy="-1.61120988900486E-05" ixz="0.000178813513793838" 
               iyy="0.00150686724578868" iyz="-1.13331975524474E-05" izz="0.0010437690500384" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="/Users/brucezhao/mit/smallrobotarm/meshes/link1.obj" />
      </geometry>
      <material name="link1_color">
        <color rgba="0.506 1.0 0.608 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="/Users/brucezhao/mit/smallrobotarm/meshes/link1.obj" />
      </geometry>
    </collision>
  </link>

  <joint name="joint1" type="revolute">
    <origin xyz="0 0 0.038" rpy="1.5708 0 0" />
    <parent link="base" />
    <child link="link1" />
    <axis xyz="0 -1 0" />
    <limit lower="-3.14159" upper="3.14159" effort="100" velocity="2.0" />
    <dynamics damping="0.1" />
  </joint>

  <!-- Joint 1 Actuator -->
  <actuator name="motor1">
    <mechanicalReduction>1</mechanicalReduction>
    <hardwareInterface>PositionJointInterface</hardwareInterface>
    <joint name="joint1" />
    <limit effort="150" velocity="3.0" />
  </actuator>

  <!-- Joint 2 -->
  <link name="link2">
    <inertial>
      <origin xyz="0.000697831131533724 0.032167355852308 -0.00116890113473487" rpy="0 0 0" />
      <mass value="0.362035782383313" />
      <inertia ixx="0.000286663821063491" ixy="7.21154567497148E-08" ixz="1.09444203974042E-06" 
               iyy="0.000151092099626246" iyz="1.0082066542099E-05" izz="0.000283678138962545" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="/Users/brucezhao/mit/smallrobotarm/meshes/link2.obj" />
      </geometry>
      <material name="link2_color">
        <color rgba="0.976 1.0 0.741 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="/Users/brucezhao/mit/smallrobotarm/meshes/link2.obj" />
      </geometry>
    </collision>
  </link>

  <joint name="joint2" type="revolute">
    <origin xyz="-0.047 0.095 0" rpy="0 0 0" />
    <parent link="link1" />
    <child link="link2" />
    <axis xyz="0 0 1" />
    <limit lower="-2.0" upper="2.0" effort="50" velocity="2.0" />
    <dynamics damping="0.1" />
  </joint>

  <!-- Joint 2 Actuator -->
  <actuator name="motor2">
    <mechanicalReduction>1</mechanicalReduction>
    <hardwareInterface>PositionJointInterface</hardwareInterface>
    <joint name="joint2" />
    <limit effort="80" velocity="2.5" />
  </actuator>

  <!-- Joint 3 -->
  <link name="link3">
    <inertial>
      <origin xyz="0.00543161046423633 -0.0015873375675293 0.00806752940458632" rpy="0 0 0" />
      <mass value="0.12720918425723" />
      <inertia ixx="6.75294390234621E-05" ixy="-9.20225110044402E-07" ixz="5.27930198494255E-07" 
               iyy="3.7644075597843E-05" iyz="1.75846807887217E-06" izz="6.15956996039113E-05" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="/Users/brucezhao/mit/smallrobotarm/meshes/link3.obj" />
      </geometry>
      <material name="link3_color">
        <color rgba="0.745 0.196 1.0 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="/Users/brucezhao/mit/smallrobotarm/meshes/link3.obj" />
      </geometry>
    </collision>
  </link>

  <joint name="joint3" type="revolute">
    <origin xyz="0 0.11 0" rpy="0 0 0" />
    <parent link="link2" />
    <child link="link3" />
    <axis xyz="0 0 1" />
    <limit lower="-2.0" upper="2.0" effort="30" velocity="2.0" />
    <dynamics damping="0.1" />
  </joint>

  <!-- Joint 3 Actuator -->
  <actuator name="motor3">
    <mechanicalReduction>1</mechanicalReduction>
    <hardwareInterface>PositionJointInterface</hardwareInterface>
    <joint name="joint3" />
    <limit effort="50" velocity="2.5" />
  </actuator>

  <!-- Joint 4 -->
  <link name="link4">
    <inertial>
      <origin xyz="-0.0553757183825251 -3.16586826770715E-06 -0.000455446113251836" rpy="0 0 0" />
      <mass value="0.113566785546949" />
      <inertia ixx="2.31151153547162E-05" ixy="-4.71867273854617E-08" ixz="-7.13828194676154E-06" 
               iyy="8.0017191893651E-05" iyz="-5.85653488889767E-10" izz="7.66544070494088E-05" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="/Users/brucezhao/mit/smallrobotarm/meshes/link4.obj" />
      </geometry>
      <material name="link4_color">
        <color rgba="0.459 1.0 0.988 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="/Users/brucezhao/mit/smallrobotarm/meshes/link4.obj" />
      </geometry>
    </collision>
  </link>

  <joint name="joint4" type="revolute">
    <origin xyz="0 0.026 0" rpy="0 0 0" />
    <parent link="link3" />
    <child link="link4" />
    <axis xyz="-1 0 0" />
    <limit lower="-2.0" upper="2.0" effort="20" velocity="2.0" />
    <dynamics damping="0.1" />
  </joint>

  <!-- Joint 4 Actuator -->
  <actuator name="motor4">
    <mechanicalReduction>1</mechanicalReduction>
    <hardwareInterface>PositionJointInterface</hardwareInterface>
    <joint name="joint4" />
    <limit effort="30" velocity="2.5" />
  </actuator>

  <!-- Joint 5 -->
  <link name="link5">
    <inertial>
      <origin xyz="-0.00199240746860804 -0.000281060475205541 0.00240649478028462" rpy="0 0 0" />
      <mass value="0.0346391394447667" />
      <inertia ixx="6.10998330221079E-06" ixy="3.73408644099324E-08" ixz="-1.38143653143755E-07" 
               iyy="6.17736943468667E-06" iyz="-2.89787007775608E-08" izz="5.4231987389802E-06" />
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="/Users/brucezhao/mit/smallrobotarm/meshes/link5.obj" />
      </geometry>
      <material name="link5_color">
        <color rgba="1.0 0.459 0.384 1" />
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0" />
      <geometry>
        <mesh filename="/Users/brucezhao/mit/smallrobotarm/meshes/link5.obj" />
      </geometry>
    </collision>
  </link>

  <joint name="joint5" type="revolute">
    <origin xyz="-0.1175 0 0" rpy="0 0 0" />
    <parent link="link4" />
    <child link="link5" />
    <axis xyz="0 0 -1" />
    <limit lower="-2.0" upper="2.0" effort="10" velocity="2.0" />
    <dynamics damping="0.1" />
  </joint>

  <!-- Joint 5 Actuator -->
  <actuator name="motor5">
    <mechanicalReduction>1</mechanicalReduction>
    <hardwareInterface>PositionJointInterface</hardwareInterface>
    <joint name="joint5" />
    <limit effort="15" velocity="2.5" />
  </actuator>

  <!-- Optional: Add effort actuators for torque control -->
  <actuator name="effort_motor1">
    <mechanicalReduction>1</mechanicalReduction>
    <hardwareInterface>EffortJointInterface</hardwareInterface>
    <joint name="joint1" />
    <limit effort="150" />
  </actuator>

  <actuator name="effort_motor2">
    <mechanicalReduction>1</mechanicalReduction>
    <hardwareInterface>EffortJointInterface</hardwareInterface>
    <joint name="joint2" />
    <limit effort="80" />
  </actuator>

  <actuator name="effort_motor3">
    <mechanicalReduction>1</mechanicalReduction>
    <hardwareInterface>EffortJointInterface</hardwareInterface>
    <joint name="joint3" />
    <limit effort="50" />
  </actuator>

  <actuator name="effort_motor4">
    <mechanicalReduction>1</mechanicalReduction>
    <hardwareInterface>EffortJointInterface</hardwareInterface>
    <joint name="joint4" />
    <limit effort="30" />
  </actuator>

  <actuator name="effort_motor5">
    <mechanicalReduction>1</mechanicalReduction>
    <hardwareInterface>EffortJointInterface</hardwareInterface>
    <joint name="joint5" />
    <limit effort="15" />
  </actuator>

  <!-- Add transmissions for actuation -->
  <transmission name="tran1">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint1">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor1">
      <mechanicalReduction>1</mechanicalReduction>
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </actuator>
  </transmission>

  <transmission name="tran2">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint2">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor2">
      <mechanicalReduction>1</mechanicalReduction>
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </actuator>
  </transmission>

  <transmission name="tran3">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint3">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor3">
      <mechanicalReduction>1</mechanicalReduction>
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </actuator>
  </transmission>

  <transmission name="tran4">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint4">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor4">
      <mechanicalReduction>1</mechanicalReduction>
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </actuator>
  </transmission>

  <transmission name="tran5">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint5">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor5">
      <mechanicalReduction>1</mechanicalReduction>
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </actuator>
  </transmission>

</robot>