<?xml version="1.0"?>
<!-- ============================================================
  smallrobot_arm.urdf · LateAI 站内原创教学模型
  用途：在 MIT Drake (pydrake) 中「真实模拟」SmallRobotArm 式六轴机械臂
  说明：连杆长度按 SmallRobotArm 机械装配 PDF 的轴系布局取近似值（米）；
        质量与惯量为本站估算（演示动力学用），真机标定请按实物替换。
  构型：J1 绕 Z（底座回转）→ J2~J5 绕 Y（肩/肘/腕摆动）→ J6 绕 Z（末端滚转）
  ============================================================ -->
<robot name="smallrobot_arm">

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

  <!-- ===== 底座（固定到世界） ===== -->
  <link name="base_link">
    <visual>
      <origin xyz="0 0 0.05" rpy="0 0 0"/>
      <geometry><box size="0.10 0.10 0.10"/></geometry>
    </visual>
    <collision>
      <origin xyz="0 0 0.05" rpy="0 0 0"/>
      <geometry><box size="0.10 0.10 0.10"/></geometry>
    </collision>
    <inertial>
      <mass value="0.6"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
    </inertial>
  </link>

  <!-- J1：底座回转（绕 Z） -->
  <joint name="J1" type="revolute">
    <parent link="base_link"/>
    <child link="carrier_link"/>
    <origin xyz="0 0 0.10" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="-3.14159" upper="3.14159" effort="8" velocity="2.5"/>
    <dynamics damping="0.05"/>
  </joint>

  <!-- 立柱/载座：连接 J1 到肩关节 J2 -->
  <link name="carrier_link">
    <visual>
      <origin xyz="0 0 0.08" rpy="0 0 0"/>
      <geometry><box size="0.06 0.06 0.16"/></geometry>
    </visual>
    <collision>
      <origin xyz="0 0 0.08" rpy="0 0 0"/>
      <geometry><box size="0.06 0.06 0.16"/></geometry>
    </collision>
    <inertial>
      <origin xyz="0 0 0.08"/>
      <mass value="0.25"/>
      <inertia ixx="0.00061" ixy="0" ixz="0" iyy="0.00061" iyz="0" izz="0.00015"/>
    </inertial>
  </link>

  <!-- J2：肩（绕 Y） -->
  <joint name="J2" type="revolute">
    <parent link="carrier_link"/>
    <child link="upper_arm_link"/>
    <origin xyz="0 0 0.16" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit lower="-2.4435" upper="2.4435" effort="12" velocity="2.5"/>
    <dynamics damping="0.05"/>
  </joint>

  <!-- 上臂：长 0.22 m -->
  <link name="upper_arm_link">
    <visual>
      <origin xyz="0.11 0 0" rpy="0 0 0"/>
      <geometry><box size="0.22 0.06 0.06"/></geometry>
    </visual>
    <collision>
      <origin xyz="0.11 0 0" rpy="0 0 0"/>
      <geometry><box size="0.22 0.06 0.06"/></geometry>
    </collision>
    <inertial>
      <origin xyz="0.10 0 0"/>
      <mass value="0.5"/>
      <inertia ixx="0.00030" ixy="0" ixz="0" iyy="0.00217" iyz="0" izz="0.00217"/>
    </inertial>
  </link>

  <!-- J3：肘（绕 Y） -->
  <joint name="J3" type="revolute">
    <parent link="upper_arm_link"/>
    <child link="forearm_link"/>
    <origin xyz="0.22 0 0" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit lower="-2.4435" upper="2.4435" effort="8" velocity="2.5"/>
    <dynamics damping="0.05"/>
  </joint>

  <!-- 前臂：长 0.20 m -->
  <link name="forearm_link">
    <visual>
      <origin xyz="0.10 0 0" rpy="0 0 0"/>
      <geometry><box size="0.20 0.055 0.055"/></geometry>
    </visual>
    <collision>
      <origin xyz="0.10 0 0" rpy="0 0 0"/>
      <geometry><box size="0.20 0.055 0.055"/></geometry>
    </collision>
    <inertial>
      <origin xyz="0.09 0 0"/>
      <mass value="0.4"/>
      <inertia ixx="0.00020" ixy="0" ixz="0" iyy="0.00143" iyz="0" izz="0.00143"/>
    </inertial>
  </link>

  <!-- J4：腕部摆动 1（绕 Y） -->
  <joint name="J4" type="revolute">
    <parent link="forearm_link"/>
    <child link="wrist1_link"/>
    <origin xyz="0.20 0 0" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit lower="-2.9671" upper="2.9671" effort="5" velocity="3.0"/>
    <dynamics damping="0.04"/>
  </joint>

  <!-- 腕部 1：长 0.12 m -->
  <link name="wrist1_link">
    <visual>
      <origin xyz="0.06 0 0" rpy="0 0 0"/>
      <geometry><box size="0.12 0.05 0.05"/></geometry>
    </visual>
    <collision>
      <origin xyz="0.06 0 0" rpy="0 0 0"/>
      <geometry><box size="0.12 0.05 0.05"/></geometry>
    </collision>
    <inertial>
      <origin xyz="0.05 0 0"/>
      <mass value="0.4"/>
      <inertia ixx="0.00017" ixy="0" ixz="0" iyy="0.00056" iyz="0" izz="0.00056"/>
    </inertial>
  </link>

  <!-- J5：腕部摆动 2（绕 Y） -->
  <joint name="J5" type="revolute">
    <parent link="wrist1_link"/>
    <child link="wrist2_link"/>
    <origin xyz="0.12 0 0" rpy="0 0 0"/>
    <axis xyz="0 1 0"/>
    <limit lower="-2.4435" upper="2.4435" effort="3" velocity="3.5"/>
    <dynamics damping="0.03"/>
  </joint>

  <!-- 腕部 2：长 0.08 m -->
  <link name="wrist2_link">
    <visual>
      <origin xyz="0.04 0 0" rpy="0 0 0"/>
      <geometry><box size="0.08 0.045 0.045"/></geometry>
    </visual>
    <collision>
      <origin xyz="0.04 0 0" rpy="0 0 0"/>
      <geometry><box size="0.08 0.045 0.045"/></geometry>
    </collision>
    <inertial>
      <origin xyz="0.035 0 0"/>
      <mass value="0.1"/>
      <inertia ixx="0.00003" ixy="0" ixz="0" iyy="0.00007" iyz="0" izz="0.00007"/>
    </inertial>
  </link>

  <!-- J6：末端滚转（绕 Z） -->
  <joint name="J6" type="revolute">
    <parent link="wrist2_link"/>
    <child link="tool_link"/>
    <origin xyz="0.08 0 0" rpy="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit lower="-3.14159" upper="3.14159" effort="1.5" velocity="4.0"/>
    <dynamics damping="0.02"/>
  </joint>

  <!-- 末端执行器：夹持杆，长 0.10 m（DEF 末端 = tool_link 原点后的几何中心） -->
  <link name="tool_link">
    <visual>
      <origin xyz="0.05 0 0" rpy="0 0 0"/>
      <geometry><box size="0.10 0.04 0.04"/></geometry>
    </visual>
    <collision>
      <origin xyz="0.05 0 0" rpy="0 0 0"/>
      <geometry><box size="0.10 0.04 0.04"/></geometry>
    </collision>
    <inertial>
      <origin xyz="0.05 0 0"/>
      <mass value="0.05"/>
      <inertia ixx="0.00001" ixy="0" ixz="0" iyy="0.00005" iyz="0" izz="0.00005"/>
    </inertial>
  </link>

</robot>
