<?xml version="1.0"?>
<!-- ============================================================
  ar4_arm.urdf · LateAI 站内原创教学模型
  用途：在 MIT Drake (pydrake) 中「真实模拟」Annin AR4 (MK5) 六轴机械臂
  ============================================================
  关节链来源：逐字段对应官方 annin_ar4_description/urdf/ar_macro.xacro（MK5）
    · <joint> 的 origin xyz / rpy 与官方 xacro 完全一致（rpy 由度换算为弧度）
    · <axis> 取官方关节旋转轴（定义于关节 frame 的局部系，保留官方符号）
    · 关节限位取官方值：
        J1 ±170°   J2 -42°~90°   J3 -89°~52°
        J4 ±180°   J5 ±105°      J6 ±180°
  几何说明：连杆外形用长方体近似（Drake 无需任何外部网格即可运行）；
            质量与惯量按 AR4 真实连杆尺度、铝件 + NEMA23/NEMA17 电机估算
            （演示动力学用），真机标定请按实物替换。
  网格版本：真实零件（18 个官方 STL 转出的 OBJ）见同目录 ar4_real.urdf
  坐标系：数据系 Z-up（与官方 xacro 一致）
  ============================================================ -->
<robot name="ar4_arm">

  <!-- world：把 base 焊接在世界原点，机械臂才不会整体自由落体 -->
  <link name="world"/>
  <joint name="world_joint" type="fixed">
    <parent link="world"/>
    <child link="base"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
  </joint>

  <!-- ===== 底座（固定到世界） ===== -->
  <link name="base">
    <visual>
      <origin xyz="0 0 0.046" rpy="0 0 0"/>
      <geometry><box size="0.10 0.10 0.092"/></geometry>
      <material name="base_color"><color rgba="0.812 0.812 0.812 1"/></material>
    </visual>
    <collision>
      <origin xyz="0 0 0.046" rpy="0 0 0"/>
      <geometry><box size="0.10 0.10 0.092"/></geometry>
    </collision>
    <inertial>
      <origin xyz="0 0 0.046" rpy="0 0 0"/>
      <mass value="1.20"/>
      <inertia ixx="0.001810" ixy="0" ixz="0" iyy="0.001810" iyz="0" izz="0.002000"/>
    </inertial>
  </link>

  <!-- J1：底座回转（官方 origin xyz="0 0 0.092" rpy="180 0 0"，绕 Z） -->
  <joint name="J1" type="revolute">
    <parent link="base"/>
    <child link="link1"/>
    <origin xyz="0 0 0.092" rpy="3.14159265 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="-2.96705973" upper="2.96705973" effort="20" velocity="1.8"/>
    <dynamics damping="0.10"/>
  </joint>

  <!-- link1：肩座（含 J1 减速箱） -->
  <link name="link1">
    <visual>
      <origin xyz="0 0 0.05" rpy="0 0 0"/>
      <geometry><box size="0.09 0.09 0.10"/></geometry>
      <material name="aluminum"><color rgba="0.812 0.812 0.812 1"/></material>
    </visual>
    <collision>
      <origin xyz="0 0 0.05" rpy="0 0 0"/>
      <geometry><box size="0.09 0.09 0.10"/></geometry>
    </collision>
    <inertial>
      <origin xyz="0 0 0.05" rpy="0 0 0"/>
      <mass value="0.90"/>
      <inertia ixx="0.001360" ixy="0" ixz="0" iyy="0.001360" iyz="0" izz="0.001215"/>
    </inertial>
  </link>

  <!-- J2：肩（官方 origin xyz="0 0.06415 -0.07778" rpy="90 0 -90"） -->
  <joint name="J2" type="revolute">
    <parent link="link1"/>
    <child link="link2"/>
    <origin xyz="0 0.06415 -0.07778" rpy="1.57079633 0 -1.57079633"/>
    <axis xyz="0 0 -1"/>
    <limit lower="-0.73303829" upper="1.57079633" effort="20" velocity="1.5"/>
    <dynamics damping="0.10"/>
  </joint>

  <!-- link2：大臂（官方 J3 相对本连杆 y=-0.305，故沿 -Y 伸出 0.30 m） -->
  <link name="link2">
    <visual>
      <origin xyz="0 -0.15 0" rpy="0 0 0"/>
      <geometry><box size="0.07 0.30 0.07"/></geometry>
      <material name="aluminum"><color rgba="0.812 0.812 0.812 1"/></material>
    </visual>
    <collision>
      <origin xyz="0 -0.15 0" rpy="0 0 0"/>
      <geometry><box size="0.07 0.30 0.07"/></geometry>
    </collision>
    <inertial>
      <origin xyz="0 -0.15 0" rpy="0 0 0"/>
      <mass value="1.10"/>
      <inertia ixx="0.008700" ixy="0" ixz="0" iyy="0.000898" iyz="0" izz="0.008700"/>
    </inertial>
  </link>

  <!-- J3：肘（官方 origin xyz="0 -0.305 0" rpy="0 0 180"） -->
  <joint name="J3" type="revolute">
    <parent link="link2"/>
    <child link="link3"/>
    <origin xyz="0 -0.305 0" rpy="0 0 3.14159265"/>
    <axis xyz="0 0 -1"/>
    <limit lower="-1.55334303" upper="0.90757121" effort="15" velocity="1.5"/>
    <dynamics damping="0.08"/>
  </joint>

  <!-- link3：小臂（官方 J4 与本连杆 frame 重合，故沿 -Y 伸出 0.28 m） -->
  <link name="link3">
    <visual>
      <origin xyz="0 -0.14 0" rpy="0 0 0"/>
      <geometry><box size="0.06 0.28 0.06"/></geometry>
      <material name="aluminum"><color rgba="0.812 0.812 0.812 1"/></material>
    </visual>
    <collision>
      <origin xyz="0 -0.14 0" rpy="0 0 0"/>
      <geometry><box size="0.06 0.28 0.06"/></geometry>
    </collision>
    <inertial>
      <origin xyz="0 -0.14 0" rpy="0 0 0"/>
      <mass value="0.80"/>
      <inertia ixx="0.005470" ixy="0" ixz="0" iyy="0.000480" iyz="0" izz="0.005470"/>
    </inertial>
  </link>

  <!-- J4：腕部回转（官方 origin xyz="0 0 0" rpy="90 0 -90"） -->
  <joint name="J4" type="revolute">
    <parent link="link3"/>
    <child link="link4"/>
    <origin xyz="0 0 0" rpy="1.57079633 0 -1.57079633"/>
    <axis xyz="0 0 -1"/>
    <limit lower="-3.14159265" upper="3.14159265" effort="8" velocity="2.0"/>
    <dynamics damping="0.06"/>
  </joint>

  <!-- link4：腕段一（官方 J5 相对本连杆 z=-0.22294，故沿 -Z 伸出 0.22 m） -->
  <link name="link4">
    <visual>
      <origin xyz="0 0 -0.11" rpy="0 0 0"/>
      <geometry><box size="0.06 0.06 0.22"/></geometry>
      <material name="aluminum"><color rgba="0.812 0.812 0.812 1"/></material>
    </visual>
    <collision>
      <origin xyz="0 0 -0.11" rpy="0 0 0"/>
      <geometry><box size="0.06 0.06 0.22"/></geometry>
    </collision>
    <inertial>
      <origin xyz="0 0 -0.11" rpy="0 0 0"/>
      <mass value="0.50"/>
      <inertia ixx="0.002166" ixy="0" ixz="0" iyy="0.002166" iyz="0" izz="0.000300"/>
    </inertial>
  </link>

  <!-- J5：腕部摆动（官方 origin xyz="0 0 -0.22294" rpy="180 0 -90"） -->
  <joint name="J5" type="revolute">
    <parent link="link4"/>
    <child link="link5"/>
    <origin xyz="0 0 -0.22294" rpy="3.14159265 0 -1.57079633"/>
    <axis xyz="1 0 0"/>
    <limit lower="-1.83259571" upper="1.83259571" effort="8" velocity="2.5"/>
    <dynamics damping="0.05"/>
  </joint>

  <!-- link5：腕段二（官方 J6 相对本连杆 z=+0.041，故沿 +Z 伸出 0.08 m） -->
  <link name="link5">
    <visual>
      <origin xyz="0 0 0.04" rpy="0 0 0"/>
      <geometry><box size="0.05 0.05 0.08"/></geometry>
      <material name="aluminum"><color rgba="0.812 0.812 0.812 1"/></material>
    </visual>
    <collision>
      <origin xyz="0 0 0.04" rpy="0 0 0"/>
      <geometry><box size="0.05 0.05 0.08"/></geometry>
    </collision>
    <inertial>
      <origin xyz="0 0 0.04" rpy="0 0 0"/>
      <mass value="0.35"/>
      <inertia ixx="0.000309" ixy="0" ixz="0" iyy="0.000309" iyz="0" izz="0.000146"/>
    </inertial>
  </link>

  <!-- J6：末端滚转（官方 origin xyz="0 0 0.041" rpy="0 0 0"） -->
  <joint name="J6" type="revolute">
    <parent link="link5"/>
    <child link="link6"/>
    <origin xyz="0 0 0.041" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="-3.14159265" upper="3.14159265" effort="5" velocity="3.0"/>
    <dynamics damping="0.04"/>
  </joint>

  <!-- link6：末端法兰 -->
  <link name="link6">
    <visual>
      <origin xyz="0 0 0.015" rpy="0 0 0"/>
      <geometry><box size="0.05 0.05 0.03"/></geometry>
      <material name="aluminum"><color rgba="0.812 0.812 0.812 1"/></material>
    </visual>
    <collision>
      <origin xyz="0 0 0.015" rpy="0 0 0"/>
      <geometry><box size="0.05 0.05 0.03"/></geometry>
    </collision>
    <inertial>
      <origin xyz="0 0 0.015" rpy="0 0 0"/>
      <mass value="0.20"/>
      <inertia ixx="0.0000567" ixy="0" ixz="0" iyy="0.0000567" iyz="0" izz="0.0000833"/>
    </inertial>
  </link>

  <!-- tool_link：末端执行器参考点（夹持杆，长 0.05 m） -->
  <joint name="tool_joint" type="fixed">
    <parent link="link6"/>
    <child link="tool_link"/>
    <origin xyz="0 0 0.03" rpy="0 0 0"/>
  </joint>

  <link name="tool_link">
    <visual>
      <origin xyz="0 0 0.025" rpy="0 0 0"/>
      <geometry><box size="0.03 0.03 0.05"/></geometry>
      <material name="end_effector"><color rgba="0.100 0.100 0.100 1"/></material>
    </visual>
    <collision>
      <origin xyz="0 0 0.025" rpy="0 0 0"/>
      <geometry><box size="0.03 0.03 0.05"/></geometry>
    </collision>
    <inertial>
      <origin xyz="0 0 0.025" rpy="0 0 0"/>
      <mass value="0.05"/>
      <inertia ixx="0.0000142" ixy="0" ixz="0" iyy="0.0000142" iyz="0" izz="0.0000075"/>
    </inertial>
  </link>

</robot>
