<?xml version="1.0"?>
<!--
  agenticros_arm.urdf.xacro

  URDF mirror of models/agenticros_arm/model.sdf, fed to robot_state_publisher
  so RViz's RobotModel display can render the arm. The forward-kinematics
  topology MUST match the SDF link-by-link or RViz will draw the arm in the
  wrong pose:

    base_link -> shoulder_pan_joint     -> shoulder_link
              -> shoulder_lift_joint    -> upper_arm_link
              -> elbow_joint            -> forearm_link
              -> wrist_1_joint          -> wrist_1_link
              -> wrist_2_joint          -> wrist_2_link
              -> wrist_3_joint          -> wrist_3_link
              -> fixed                  -> tool0

  Conventions:
    * Each joint origin is expressed in its PARENT link's frame.
    * Cylinder visuals are drawn along the link's local +Z by default; we
      rotate them where needed to lay them flat along +X (matching how the
      SDF orients each link via its <pose>).
    * Joint axes match the SDF exactly (Z for pan/wrist_2, Y for the rest).
    * Joint limits match the SDF; URDF doesn't really use them for
      visualization but MoveIt2 (Phase 3b) will.

  Physics, sensors, gravity tuning, and PD gains all live in the SDF; this
  file is purely for visualization in RViz.
-->
<robot name="agenticros_arm" xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!-- ===== material palette (matches the SDF colours) ===== -->
  <material name="agros_dark">  <color rgba="0.20 0.20 0.20 1"/></material>
  <material name="agros_blue">  <color rgba="0.25 0.50 0.75 1"/></material>
  <material name="agros_orange"><color rgba="0.95 0.60 0.10 1"/></material>
  <material name="agros_red">   <color rgba="1.00 0.10 0.10 1"/></material>

  <!-- ============================================================
       base_link  (static pedestal, fixed to world)
       ============================================================ -->
  <link name="base_link">
    <visual>
      <origin xyz="0 0 0.075" rpy="0 0 0"/>
      <geometry><cylinder radius="0.08" length="0.15"/></geometry>
      <material name="agros_dark"/>
    </visual>
    <collision>
      <origin xyz="0 0 0.075" rpy="0 0 0"/>
      <geometry><cylinder radius="0.08" length="0.15"/></geometry>
    </collision>
    <inertial>
      <mass value="10.0"/>
      <inertia ixx="0.05" iyy="0.05" izz="0.05" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>

  <!-- ============================================================
       shoulder_link  (rotated around Z)
       ============================================================ -->
  <link name="shoulder_link">
    <visual>
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <geometry><cylinder radius="0.07" length="0.10"/></geometry>
      <material name="agros_blue"/>
    </visual>
    <inertial>
      <mass value="3.0"/>
      <inertia ixx="0.008" iyy="0.008" izz="0.005" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>
  <joint name="shoulder_pan_joint" type="revolute">
    <parent link="base_link"/>
    <child  link="shoulder_link"/>
    <origin xyz="0 0 0.205" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="-3.14159" upper="3.14159" velocity="2.0" effort="30.0"/>
  </joint>

  <!-- ============================================================
       upper_arm_link  (rotated around Y) - laid along +X
       ============================================================ -->
  <link name="upper_arm_link">
    <visual>
      <origin xyz="0.20 0 0" rpy="0 1.5707963 0"/>
      <geometry><cylinder radius="0.045" length="0.40"/></geometry>
      <material name="agros_orange"/>
    </visual>
    <inertial>
      <mass value="4.0"/>
      <inertia ixx="0.02" iyy="0.06" izz="0.06" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>
  <joint name="shoulder_lift_joint" type="revolute">
    <parent link="shoulder_link"/>
    <child  link="upper_arm_link"/>
    <origin xyz="0 0 0.05" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit lower="-1.5707" upper="1.5707" velocity="2.0" effort="30.0"/>
  </joint>

  <!-- ============================================================
       forearm_link  (rotated around Y)
       ============================================================ -->
  <link name="forearm_link">
    <visual>
      <origin xyz="0.175 0 0" rpy="0 1.5707963 0"/>
      <geometry><cylinder radius="0.035" length="0.35"/></geometry>
      <material name="agros_orange"/>
    </visual>
    <inertial>
      <mass value="2.5"/>
      <inertia ixx="0.012" iyy="0.04" izz="0.04" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>
  <joint name="elbow_joint" type="revolute">
    <parent link="upper_arm_link"/>
    <child  link="forearm_link"/>
    <origin xyz="0.40 0 0" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit lower="-3.14159" upper="3.14159" velocity="2.0" effort="30.0"/>
  </joint>

  <!-- ============================================================
       wrist_1_link  (rotated around Y)
       ============================================================ -->
  <link name="wrist_1_link">
    <visual>
      <origin xyz="0.05 0 0" rpy="0 1.5707963 0"/>
      <geometry><cylinder radius="0.032" length="0.10"/></geometry>
      <material name="agros_blue"/>
    </visual>
    <inertial>
      <mass value="0.8"/>
      <inertia ixx="0.001" iyy="0.001" izz="0.001" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>
  <joint name="wrist_1_joint" type="revolute">
    <parent link="forearm_link"/>
    <child  link="wrist_1_link"/>
    <origin xyz="0.35 0 0" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit lower="-3.14159" upper="3.14159" velocity="2.0" effort="15.0"/>
  </joint>

  <!-- ============================================================
       wrist_2_link  (rotated around Z)
       ============================================================ -->
  <link name="wrist_2_link">
    <visual>
      <origin xyz="0 0 0.04" rpy="0 0 0"/>
      <geometry><cylinder radius="0.030" length="0.08"/></geometry>
      <material name="agros_orange"/>
    </visual>
    <inertial>
      <mass value="0.6"/>
      <inertia ixx="0.0008" iyy="0.0008" izz="0.0008" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>
  <joint name="wrist_2_joint" type="revolute">
    <parent link="wrist_1_link"/>
    <child  link="wrist_2_link"/>
    <origin xyz="0.10 0 0" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="-3.14159" upper="3.14159" velocity="2.0" effort="15.0"/>
  </joint>

  <!-- ============================================================
       wrist_3_link  (rotated around Y)
       ============================================================ -->
  <link name="wrist_3_link">
    <visual>
      <origin xyz="0.03 0 0" rpy="0 1.5707963 0"/>
      <geometry><cylinder radius="0.026" length="0.06"/></geometry>
      <material name="agros_blue"/>
    </visual>
    <inertial>
      <mass value="0.4"/>
      <inertia ixx="0.0004" iyy="0.0004" izz="0.0004" ixy="0" ixz="0" iyz="0"/>
    </inertial>
  </link>
  <joint name="wrist_3_joint" type="revolute">
    <parent link="wrist_2_link"/>
    <child  link="wrist_3_link"/>
    <origin xyz="0 0 0.08" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit lower="-3.14159" upper="3.14159" velocity="2.0" effort="10.0"/>
  </joint>

  <!-- ============================================================
       tool0 (end-effector frame). Tipped with a small red sphere so
       the user can see "this is what an IK solver would target."
       ============================================================ -->
  <link name="tool0">
    <visual>
      <geometry><sphere radius="0.015"/></geometry>
      <material name="agros_red"/>
    </visual>
  </link>
  <joint name="tool0_joint" type="fixed">
    <parent link="wrist_3_link"/>
    <child  link="tool0"/>
    <origin xyz="0.06 0 0" rpy="0 0 0"/>
  </joint>

</robot>
