<?xml version="1.0" encoding="utf-8"?>
<robot name="rotary_cartpole">

  <!-- Fixed world frame -->
  <link name="world"/>

  <!-- Base: motor housing, fixed to world -->
  <link name="base_link">
    <inertial>
      <origin xyz="-0.00011 0.00117 0.06055" rpy="0 0 0"/>
      <mass value="0.921"/>
      <inertia ixx="0.002385" iyy="0.002484" izz="0.000559"
               ixy="0.0" iyz="-0.000149" ixz="6e-06"/>
    </inertial>
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="meshes/base_link.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry>
        <mesh filename="meshes/base_link.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>

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

  <!-- Arm: horizontal rotating arm driven by motor.
       Real mass ~10g (Fusion assumed dense material, exported 279g). -->
  <link name="arm">
    <inertial>
      <origin xyz="0.00005 0.0065 0.00563" rpy="0 0 0"/>
      <mass value="0.010"/>
      <inertia ixx="2.70e-06" iyy="7.80e-07" izz="2.44e-06"
               ixy="0.0" iyz="7.20e-08" ixz="0.0"/>
    </inertial>
    <visual>
      <origin xyz="0.006947 -0.00395 -0.14796" rpy="0 0 0"/>
      <geometry>
        <mesh filename="meshes/arm_1.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin xyz="0.006947 -0.00395 -0.14796" rpy="0 0 0"/>
      <geometry>
        <mesh filename="meshes/arm_1.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>

  <!-- Motor joint: base → arm, rotates around vertical z-axis -->
  <joint name="motor_joint" type="revolute">
    <origin xyz="-0.006947 0.00395 0.14796" rpy="0 0 0"/>
    <parent link="base_link"/>
    <child link="arm"/>
    <axis xyz="0 0 1"/>
    <limit lower="-1.5708" upper="1.5708" effort="10.0" velocity="200.0"/>
    <dynamics damping="0.001"/>
  </joint>

  <!-- Pendulum: swings freely at the end of the arm.
       Real mass: 5g pendulum + 10g weight at the tip (70mm from bearing) = 15g total.
       (Fusion assumed dense material, exported 57g for the pendulum alone.) -->
  <link name="pendulum">
    <inertial>
      <!-- Combined CoM: 5g rod (CoM ~35mm) + 10g tip weight at 70mm from pivot.
           Tip at (0.07, -0.07, 0) → 45° diagonal in +X/-Y.
           CoM = (5×0.035+10×0.07)/15 = 0.0583 along both +X and -Y.
           Inertia tensor rotated 45° to match diagonal rod axis. -->
      <origin xyz="0.1583 -0.0983 -0.0" rpy="0 0 0"/>
      <mass value="0.015"/>
      <inertia ixx="6.16e-06" iyy="6.16e-06" izz="1.23e-05"
               ixy="6.10e-06" iyz="0.0" ixz="0.0"/>
    </inertial>
    <visual>
      <origin xyz="0.006895 -0.023224 -0.162953" rpy="0 0 0"/>
      <geometry>
        <mesh filename="meshes/pendulum_1.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
    <collision>
      <origin xyz="0.006895 -0.023224 -0.162953" rpy="0 0 0"/>
      <geometry>
        <mesh filename="meshes/pendulum_1.stl" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
  </link>

  <!-- Pendulum joint: arm → pendulum, bearing axis along Y.
       Joint origin corrected from mesh analysis (Fusion2URDF was 180mm off).
       rpy pitch +90° so qpos=0 = pendulum hanging down (gravity-stable). -->
  <joint name="pendulum_joint" type="continuous">
    <origin xyz="0.000052 0.019274 0.014993" rpy="0 1.5708 0"/>
    <parent link="arm"/>
    <child link="pendulum"/>
    <axis xyz="0 -1 0"/>
    <dynamics damping="0.0001"/>
  </joint>

</robot>
