<?xml version="1.0"?>
<robot name="wrap_two_module_rot">
  <!-- ===== MODULE 1 ===== -->
  <!-- base = servo body captured in the both-sides WRAP holder -->
  <link name="base_link">
    <visual><origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry><mesh filename="st3215.stl" scale="0.001 0.001 0.001"/></geometry>
      <material name="servo"><color rgba="0.35 0.35 0.40 1"/></material></visual>
    <visual><origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry><mesh filename="wrap_holder.stl" scale="0.001 0.001 0.001"/></geometry>
      <material name="holder"><color rgba="0.20 0.52 0.85 1"/></material></visual>
  </link>

  <!-- output 1: revolute about +Y through the real output axis (-25.5,0) mm -->
  <joint name="output" type="revolute">
    <parent link="base_link"/><child link="yoke_link"/>
    <origin xyz="-0.0255 0 0" rpy="0 0 0"/><axis xyz="0 1 0"/>
    <limit lower="-1.74" upper="1.74" effort="2.94" velocity="3.0"/>
  </joint>

  <!-- yoke 1 grips horn + idler; reaches out to its far end at x~=-118mm -->
  <link name="yoke_link">
    <visual><origin xyz="0.0255 0 0" rpy="0 0 0"/>
      <geometry><mesh filename="yoke_exact.stl" scale="0.001 0.001 0.001"/></geometry>
      <material name="yoke"><color rgba="0.85 0.60 0.20 1"/></material></visual>
  </link>

  <!-- ===== MODULE 2 (copy of module 1, seated at the yoke's far end) ===== -->
  <!-- carried by yoke 1; no connector geometry yet, just positioned so they could connect.
       reach: servo2 base sits -0.1047m from servo1 base (= -(34.7+70)/1000, yoke link_len 70),
       which in the yoke frame (origin at the output, -0.0255) is -0.1047-(-0.0255) = -0.0792m.
       Same orientation -> module 2's holder front face meets module 1's yoke far end. -->
  <joint name="connect_1_2" type="fixed">
    <parent link="yoke_link"/><child link="base_link2"/>
    <origin xyz="-0.105 0 0" rpy="1.5708 0 0"/>
  </joint>

  <link name="base_link2">
    <visual><origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry><mesh filename="st3215.stl" scale="0.001 0.001 0.001"/></geometry>
      <material name="servo2"><color rgba="0.35 0.35 0.40 1"/></material></visual>
    <visual><origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry><mesh filename="wrap_holder.stl" scale="0.001 0.001 0.001"/></geometry>
      <material name="holder2"><color rgba="0.25 0.70 0.55 1"/></material></visual>
  </link>

  <!-- output 2 -->
  <joint name="output2" type="revolute">
    <parent link="base_link2"/><child link="yoke_link2"/>
    <origin xyz="-0.0255 0 0" rpy="0 0 0"/><axis xyz="0 1 0"/>
    <limit lower="-1.74" upper="1.74" effort="2.94" velocity="3.0"/>
  </joint>

  <link name="yoke_link2">
    <visual><origin xyz="0.0255 0 0" rpy="0 0 0"/>
      <geometry><mesh filename="yoke_exact.stl" scale="0.001 0.001 0.001"/></geometry>
      <material name="yoke2"><color rgba="0.90 0.45 0.20 1"/></material></visual>
  </link>
</robot>
