# 60 · URDF 机器人模型(完全指南)

> **目标**:能读懂 URDF,能从零写一个 6-DoF 机械臂 URDF,理解 link / joint / xacro / 物理参数,会校验 + 可视化。

---

## 目录

- [1. URDF 是什么](#1-urdf-是什么)
- [2. URDF 三大元素](#2-urdf-三大元素)
- [3. 完整 6-DoF 机械臂示例](#3-完整-6-dof-机械臂示例)
- [4. xacro:模板化 URDF](#4-xacro模板化-urdf)
- [5. 坐标约定 REP-103 / REP-105](#5-坐标约定-rep-103--rep-105)
- [6. ros2_control 集成(加 transmission)](#6-ros2_control-集成加-transmission)
- [7. Gazebo 集成(加 gazebo 标签)](#7-gazebo-集成加-gazebo-标签)
- [8. 校验 + 可视化](#8-校验--可视化)
- [9. 工具链总结](#9-工具链总结)
- [10. MoveIt2 SRDF(规划组)](#10-moveit2-srdf规划组)
- [11. 在本仓库里跑](#11-在本仓库里跑)
- [12. 进阶:6-DoF 真机械臂 URDF 实战](#12-进阶6-dof-真机械臂-urdf-实战)

---

## 1. URDF 是什么

**URDF** (Unified Robot Description Format) = 用 XML 描述机器人:
- 有哪些**刚体段**(`<link>`)
- 刚体之间怎么连(`<joint>`)
- 视觉 / 碰撞几何(`<visual>` / `<collision>`)
- 物理参数(质量、惯性,`<inertial>`)

URDF 通过 `robot_state_publisher` 翻译成 TF tree,被 MoveIt2 / Gazebo / RViz 读取。

---

## 2. URDF 三大元素

### 2.1 `<link>` — 刚体段

| 子标签 | 用途 | 必填 |
|---|---|---|
| `<visual>` | 视觉几何(RViz / Gazebo 显示) | 推荐 |
| `<collision>` | 碰撞几何(物理仿真) | 推荐 |
| `<inertial>` | 质量 + 转动惯量(动力学仿真必填) | 仿真必填 |
| `<material>` | 视觉颜色 | 否 |

```xml
<link name="base_link">
  <visual>
    <origin xyz="0 0 0.05" rpy="0 0 0"/>
    <geometry><box size="0.1 0.1 0.1"/></geometry>
    <material name="gray">
      <color rgba="0.5 0.5 0.5 1.0"/>
    </material>
  </visual>
  <collision>
    <origin xyz="0 0 0.05"/>
    <geometry><box size="0.1 0.1 0.1"/></geometry>
  </collision>
  <inertial>
    <mass value="1.0"/>
    <inertia ixx="0.01" ixy="0" ixz="0" iyy="0.01" iyz="0" izz="0.01"/>
  </inertial>
</link>
```

#### 几何类型

```xml
<geometry>
  <box size="w h d"/>           <!-- 长方体 -->
  <sphere radius="r"/>           <!-- 球 -->
  <cylinder radius="r" length="L"/>  <!-- 圆柱 -->
  <mesh filename="package://my_pkg/meshes/arm.stl" scale="0.001 0.001 0.001"/>
</geometry>
```

#### 颜色 / 材质

```xml
<material name="gray">
  <color rgba="0.5 0.5 0.5 1.0"/>     <!-- RGBA,0~1 -->
  <texture filename="..."/>          <!-- 贴图(可选) -->
</material>
```

### 2.2 `<joint>` — 关节

| type | 含义 | DoF |
|---|---|---|
| `revolute` | 转动(有限位) | 1 |
| `continuous` | 转动(无限位) | 1 |
| `prismatic` | 滑动 | 1 |
| `fixed` | 固定连接 | 0 |
| `floating` | 6 DoF | 6 |
| `planar` | 平面 3 DoF | 3 |

```xml
<joint name="joint1" type="revolute">
  <parent link="base_link"/>
  <child  link="link1"/>
  <origin xyz="0 0 0.05" rpy="0 0 0"/>     <!-- 子 link 相对父 link -->
  <axis   xyz="0 0 1"/>                   <!-- 在 parent 系下定义 -->
  <limit  lower="-3.14" upper="3.14"      <!-- 关节限位 -->
          effort="100" velocity="1.0"/>   <!-- 力矩 / 速度上限 -->
  <dynamics damping="0.1" friction="0.0"/> <!-- 仿真摩擦 -->
</joint>
```

**重要约定**:
- `axis` 在 **parent link 系**下定义
- `origin` 是 **child 相对 parent** 的偏移
- 关节运动绕 `axis` 旋转(或沿 axis 平移)

### 2.3 `<robot>` 根
- `name`:机器人名字,在 ROS 工具中显示
- 必须包含所有 `<link>` 和 `<joint>`

### 2.4 最小 URDF 示例

```xml
<?xml version="1.0"?>
<robot name="hello_arm">
  <link name="base_link">
    <visual><geometry><box size="0.1 0.1 0.1"/></geometry></visual>
  </link>
  <joint name="joint1" type="revolute">
    <parent link="base_link"/>
    <child  link="link1"/>
    <origin xyz="0 0 0.05"/>
    <axis   xyz="0 0 1"/>
    <limit  lower="-3.14" upper="3.14" effort="1.0" velocity="1.0"/>
  </joint>
  <link name="link1">
    <visual><geometry><box size="0.05 0.05 0.1"/></geometry></visual>
  </link>
</robot>
```

---

## 3. 完整 6-DoF 机械臂示例

模拟 6-DoF 工业机械臂(类似 UR5 / xArm 6):

```xml
<?xml version="1.0"?>
<robot name="my_6dof_arm">
  <!-- 材料定义 -->
  <material name="white"><color rgba="1 1 1 1"/></material>
  <material name="red"><color rgba="0.8 0.2 0.2 1"/></material>

  <!-- 1) base_link:固定基座 -->
  <link name="base_link">
    <visual>
      <geometry><cylinder radius="0.05" length="0.05"/></geometry>
      <material name="white"/>
    </visual>
    <collision>
      <geometry><cylinder radius="0.05" length="0.05"/></geometry>
    </collision>
    <inertial>
      <mass value="1.0"/>
      <inertia ixx="0.005" ixy="0" ixz="0" iyy="0.005" iyz="0" izz="0.005"/>
    </inertial>
  </link>

  <!-- 2) joint1:底盘转动(绕 z 轴) -->
  <joint name="joint1" type="revolute">
    <parent link="base_link"/>
    <child  link="shoulder_link"/>
    <origin xyz="0 0 0.05"/>
    <axis   xyz="0 0 1"/>
    <limit  lower="-3.14" upper="3.14" effort="100" velocity="2.0"/>
  </joint>
  <link name="shoulder_link">
    <visual>
      <geometry><box size="0.05 0.05 0.05"/></geometry>
      <material name="red"/>
    </visual>
    <collision>
      <geometry><box size="0.05 0.05 0.05"/></geometry>
    </collision>
    <inertial>
      <mass value="0.3"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
    </inertial>
  </link>

  <!-- 3) joint2:肩部俯仰(绕 y 轴) -->
  <joint name="joint2" type="revolute">
    <parent link="shoulder_link"/>
    <child  link="upper_arm_link"/>
    <origin xyz="0 0 0.05"/>
    <axis   xyz="0 1 0"/>
    <limit  lower="-1.57" upper="1.57" effort="100" velocity="2.0"/>
  </joint>
  <link name="upper_arm_link">
    <visual>
      <origin xyz="0 0 0.1"/>
      <geometry><cylinder radius="0.025" length="0.2"/></geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0.1"/>
      <geometry><cylinder radius="0.025" length="0.2"/></geometry>
    </collision>
    <inertial>
      <mass value="0.5"/>
      <inertia ixx="0.002" ixy="0" ixz="0" iyy="0.002" iyz="0" izz="0.0005"/>
    </inertial>
  </link>

  <!-- 4) joint3:肘部俯仰 -->
  <joint name="joint3" type="revolute">
    <parent link="upper_arm_link"/>
    <child  link="forearm_link"/>
    <origin xyz="0 0 0.2"/>
    <axis   xyz="0 1 0"/>
    <limit  lower="-2.0" upper="2.0" effort="50" velocity="2.0"/>
  </joint>
  <link name="forearm_link">
    <visual>
      <origin xyz="0 0 0.075"/>
      <geometry><cylinder radius="0.02" length="0.15"/></geometry>
      <material name="white"/>
    </visual>
    <collision>
      <origin xyz="0 0 0.075"/>
      <geometry><cylinder radius="0.02" length="0.15"/></geometry>
    </collision>
    <inertial>
      <mass value="0.3"/>
      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.0003"/>
    </inertial>
  </link>

  <!-- 5) joint4:腕部旋转 -->
  <joint name="joint4" type="revolute">
    <parent link="forearm_link"/>
    <child  link="wrist_1_link"/>
    <origin xyz="0 0 0.15"/>
    <axis   xyz="1 0 0"/>
    <limit  lower="-3.14" upper="3.14" effort="20" velocity="2.0"/>
  </joint>
  <link name="wrist_1_link">
    <visual>
      <geometry><box size="0.04 0.04 0.04"/></geometry>
      <material name="red"/>
    </visual>
    <collision><geometry><box size="0.04 0.04 0.04"/></geometry></collision>
    <inertial>
      <mass value="0.1"/>
      <inertia ixx="0.0003" ixy="0" ixz="0" iyy="0.0003" iyz="0" izz="0.0003"/>
    </inertial>
  </link>

  <!-- 6) joint5:腕部俯仰 -->
  <joint name="joint5" type="revolute">
    <parent link="wrist_1_link"/>
    <child  link="wrist_2_link"/>
    <origin xyz="0 0 0.04"/>
    <axis   xyz="0 0 1"/>
    <limit  lower="-3.14" upper="3.14" effort="20" velocity="2.0"/>
  </joint>
  <link name="wrist_2_link">
    <visual><geometry><box size="0.03 0.03 0.04"/></geometry></visual>
    <collision><geometry><box size="0.03 0.03 0.04"/></geometry></collision>
    <inertial>
      <mass value="0.1"/>
      <inertia ixx="0.0002"/>
    </inertial>
  </link>

  <!-- 7) joint6:末端旋转 -->
  <joint name="joint6" type="revolute">
    <parent link="wrist_2_link"/>
    <child  link="end_effector_link"/>
    <origin xyz="0 0 0.04"/>
    <axis   xyz="1 0 0"/>
    <limit  lower="-3.14" upper="3.14" effort="10" velocity="2.0"/>
  </joint>
  <link name="end_effector_link">
    <visual>
      <geometry><cylinder radius="0.02" length="0.03"/></geometry>
      <material name="red"/>
    </visual>
    <collision>
      <geometry><cylinder radius="0.02" length="0.03"/></geometry>
    </collision>
    <inertial>
      <mass value="0.05"/>
      <inertia ixx="0.0001"/>
    </inertial>
  </link>

  <!-- 8) 末端夹爪(可选 gripper) -->
  <joint name="gripper_joint" type="fixed">
    <parent link="end_effector_link"/>
    <child  link="gripper_link"/>
    <origin xyz="0 0 0.025"/>
  </joint>
  <link name="gripper_link">
    <visual><geometry><box size="0.04 0.06 0.02"/></geometry></visual>
    <collision><geometry><box size="0.04 0.06 0.02"/></geometry></collision>
    <inertial>
      <mass value="0.05"/>
      <inertia ixx="0.0001"/>
    </inertial>
  </link>
</robot>
```

**约定**:
- x = 前进
- y = 左
- z = 上
- joint1 绕 z 轴,旋转"底盘朝向"
- joint2/joint3 绕 y 轴,"肩膀/肘部俯仰"
- joint4/joint5/joint6 腕部 3 DoF
- 总长度约 0.4 m,可伸展约 0.5 m

---

## 4. xacro:模板化 URDF

URDF 重复内容多(左右轮、对称连杆)。**xacro** 提供宏、变量、数学。

### 4.1 安装

xacro 通常自带 ROS2:
```bash
sudo apt install ros-humble-xacro
```

### 4.2 xacro 基础语法

```xml
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="arm">
  <!-- 变量 -->
  <xacro:property name="link_len" value="0.1"/>
  <xacro:property name="PI" value="3.14159"/>

  <!-- 宏 -->
  <xacro:macro name="make_link" params="name color *origin">
    <link name="${name}">
      <visual>
        <xacro:insert_block name="origin"/>
        <geometry><box size="0.05 0.05 ${link_len}"/></geometry>
      </visual>
    </link>
  </xacro:macro>

  <link name="base_link"/>
  <xacro:make_link name="link1" color="red">
    <origin xyz="0 0 0.05"/>
  </xacro:make_link>
  <xacro:make_link name="link2" color="green">
    <origin xyz="0 0 0.1"/>
  </xacro:make_link>

  <!-- 数学 -->
  <xacro:property name="mass" value="${1.0 * link_len}"/>
</robot>
```

### 4.3 编译 xacro → URDF

```bash
xacro arm.xacro > arm.urdf
# 或 ros2 launch 时直接传 xacro 输出:
ros2 run robot_state_publisher robot_state_publisher \
  --ros-args -p robot_description:="$(xacro /path/to/arm.xacro)"
```

---

## 5. 坐标约定 REP-103 / REP-105

ROS2 的标准约定,**违反会导致 MoveIt2 / Gazebo 算错**:

### 5.1 REP-103(单位 + 坐标轴方向)

- 长度单位:**米**(m)
- 角度单位:**弧度**(rad)
- 右手系:
  - **x** = 前进
  - **y** = 左
  - **z** = 上

### 5.2 REP-105(坐标系语义)

| frame | 含义 |
|---|---|
| `world` | 全局参考(通常不动) |
| `map` | SLAM 输出的地图 |
| `odom` | 里程计 / AMCL 定位 |
| `base_link` | 机器人底盘中心(刚体) |
| `base_footprint` | 底盘在地面的投影(2D 导航) |

关系:
- `world → map → odom → base_link` 是固定链
- `world → map` 由 SLAM 提供(变化)
- `map → odom` 由 AMCL / 里程计提供
- `odom → base_link` 由编码器 / IMU 提供

---

## 6. ros2_control 集成(加 transmission)

`ros2_control` 需要每个关节加 `<transmission>` 标签:

```xml
<joint name="joint1" type="revolute">
  <parent link="base_link"/>
  <child  link="shoulder_link"/>
  <axis   xyz="0 0 1"/>
  <limit  effort="100" velocity="2.0" lower="-3.14" upper="3.14"/>

  <!-- ros2_control 必填 -->
  <transmission name="trans_joint1">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint1">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor_joint1">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
</joint>
```

`<transmission>` 描述:
- 哪个 joint
- 用哪个 hardware interface(Position / Velocity / Effort)
- 减速比(`mechanicalReduction`)

---

## 7. Gazebo 集成(加 gazebo 标签)

Gazebo Sim / ros_gz 加载 URDF 时需要 `<gazebo>` 标签 + `<transmission>`(已经加)。

```xml
<gazebo>
  <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
    <parameters>$(find my_arm)/config/controllers.yaml</parameters>
  </plugin>
</gazebo>

<!-- 每个 link 加颜色(可选) -->
<gazebo reference="shoulder_link">
  <material>Gazebo/Red</material>
</gazebo>
```

---

## 8. 校验 + 可视化

### 8.1 校验

```bash
sudo apt install liburdfdom-tools

# 检查 URDF 合法性
check_urdf my_arm.urdf

# 输出 OK 即合法,会打印 link / joint 树结构
```

### 8.2 生成 PDF/PNG 图

```bash
sudo apt install liburdfdom-tools python3-urdfdom-py

urdf_to_graphiz my_arm.urdf    # 生成 .pdf
# 或
urdf_to_graphiz my_arm.urdf -o my_arm.pdf
```

### 8.3 RViz 可视化

```bash
ros2 launch urdf_tutorial display.launch.py
# 或自己写 launch:
ros2 run rviz2 rviz2
# 在 RViz:
#   Fixed Frame → base_link
#   Add → RobotModel    ← 显示 URDF
#   Add → TF           ← 显示 TF tree
```

### 8.4 joint_state_publisher_gui(手动调关节)

```bash
ros2 run joint_state_publisher_gui joint_state_publisher_gui
# 弹出 GUI,拖滑块调关节角,看机械臂在 RViz 里动
```

---

## 9. 工具链总结

```
xacro .xacro ───> .urdf (用 xacro 编译)
                │
                ├──> check_urdf (校验)
                ├──> urdf_to_graphiz (可视化)
                │
                ├──> robot_state_publisher
                │       │
                │       │ 读 URDF + /joint_states → 算 TF → /tf
                │       │
                │       └──> tf2::Buffer (其他节点订阅)
                │
                ├──> MoveIt2 (规划)
                │       │
                │       │ 读 URDF + SRDF → 算 IK / path
                │       │
                │       └──> trajectory_msgs/JointTrajectory
                │
                └──> Gazebo Sim (仿真)
                        │
                        │ 读 URDF + gazebo 标签 → 物理仿真
```

---

## 10. MoveIt2 SRDF(规划组)

URDF 描述"机器人长啥样",SRDF 加 MoveIt2 需要的规划信息:

- **虚拟关节**:base_link → world(让 MoveIt2 把臂放在世界系)
- **规划组**(Planning Group):哪些关节一起动
- **末端执行器**:夹爪
- **预设位姿**(Pose):home / ready / extended

### 10.1 用 MoveIt Setup Assistant 生成

```bash
ros2 launch moveit_setup_assistant setup_assistant.launch.py
```

GUI 里:
1. Load URDF
2. Add Virtual Joint(base_link → world)
3. Add Planning Group("arm",含 6 joint)
4. Add End Effector(gripper)
5. Add Poses(home / ready / up)
6. **Generate** SRDF + MoveIt config

### 10.2 SRDF 简例

```xml
<robot name="my_arm">
  <!-- virtual joint: world → base_link -->
  <virtual_joint name="world_to_base" type="fixed"
                parent_frame="world" child_link="base_link"/>

  <!-- planning group -->
  <group name="arm">
    <chain base_link="base_link" tip_link="gripper_link"/>
    <joint name="joint1"/>
    <joint name="joint2"/>
    ...
  </group>

  <!-- end effector -->
  <end_effector name="gripper" parent_link="end_effector_link"
               group="gripper"/>

  <!-- pose -->
  <group_state name="home" group="arm">
    <joint name="joint1" value="0"/>
    <joint name="joint2" value="0"/>
    ...
  </group_state>
</robot>
```

---

## 11. 在本仓库里跑

### 11.1 启 3 关节机械臂 demo

```bash
docker exec ros2_dev bash -lc "source /root/ros2_ws/install/setup.bash && ros2 launch bringup robot_launch.py"
```

节点(launch 重命名后是 `joint_state_publisher` / `tf2_listener`,executable 名带 `_cpp`):
- `joint_state_publisher_cpp` (本包,executable 名)
- `robot_state_publisher` (系统包)
- `tf2_listener_cpp` (本包,executable 名)

### 11.2 校验本仓库 URDF

```bash
docker exec ros2_dev bash -lc "check_urdf /root/ros2_ws/install/cpp_robot_tf2/share/cpp_robot_tf2/urdf/simple_arm.urdf"
```

### 11.3 源码
- URDF: [`src/cpp_robot_tf2/urdf/simple_arm.urdf`](../src/cpp_robot_tf2/urdf/simple_arm.urdf)
- launch: [`src/cpp_robot_tf2/launch/robot_tf2_launch.py`](../src/cpp_robot_tf2/launch/robot_tf2_launch.py)

### 11.4 端到端日志
[`docker/robot_e2e.log`](../docker/robot_e2e.log)

---

## 12. 进阶:6-DoF 真机械臂 URDF 实战

### 12.1 找开源 URDF

| 机械臂 | URDF 来源 |
|---|---|
| Franka Panda | `ros-planning/moveit_resources/panda` |
| UR5 / UR10 | `ros-industrial/universal_robot` |
| Ufactory xArm 6 | `xArm-Robotics/xarm_ros2` |
| Aloha | `tonyzhaozh/aerial_manipulation` |

### 12.2 URDF 适配本仓库

把开源 URDF 拷到 `src/<your_arm>/urdf/`:
```bash
cp -r <upstream>/franka_description/urdf src/my_arm/urdf/

# 加 transmission(ros2_control 必填)
# 已经在开源版本里有,但要确认 hardwareInterface 是 PositionJointInterface
```

### 12.3 加 ros2_control 配置

`src/my_arm/config/controllers.yaml`:
```yaml
controller_manager:
  ros__parameters:
    update_rate: 100
    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateController
    arm_controller:
      type: position_controllers/JointGroupPositionController

arm_controller:
  ros__parameters:
    joints:
      - joint1
      - joint2
      - joint3
      - joint4
      - joint5
      - joint6
```

### 12.4 加 launch

`src/my_arm/launch/arm_bringup.launch.py`:
```python
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import os

def generate_launch_description():
    pkg = get_package_share_directory('my_arm')
    urdf = os.path.join(pkg, 'urdf', 'arm.urdf')
    controllers = os.path.join(pkg, 'config', 'controllers.yaml')

    with open(urdf, 'r') as f: urdf_content = f.read()

    return LaunchDescription([
        # 1) robot_state_publisher
        Node(package='robot_state_publisher', executable='robot_state_publisher',
             parameters=[{'robot_description': urdf_content}]),
        # 2) ros2_control_node
        Node(package='controller_manager', executable='ros2_control_node',
             parameters=[{'robot_description': urdf_content,
                          'update_rate': 100}],
             output='screen'),
        # 3) spawn joint_state_broadcaster
        Node(package='controller_manager', executable='spawner',
             arguments=['joint_state_broadcaster', '-c', '/controller_manager']),
        # 4) spawn arm_controller
        Node(package='controller_manager', executable='spawner',
             arguments=['arm_controller', '-c', '/controller_manager']),
    ])
```

### 12.5 加 MoveIt2

```bash
sudo apt install ros-humble-moveit
# 用 MoveIt Setup Assistant 加载你的 URDF 生成 SRDF
ros2 launch moveit_setup_assistant setup_assistant.launch.py
```

详见 [`99-embodied-ai.md`](99-embodied-ai.md) 阶段 2。

---

## 接下来读

| 主题 | 文档 |
|---|---|
| TF 坐标变换 | [`50-tf2.md`](50-tf2.md) |
| ros2_control + MoveIt2 | [`99-embodied-ai.md`](99-embodied-ai.md) 阶段 2 |
| launch 文件 | [`70-launch.md`](70-launch.md) |
| 三机部署 | [`100-embedded-deployment.md`](100-embedded-deployment.md) |

---

---

## 📖 阅读路径导航

> 💡 这是仓库 `doc/` 下所有文档的推荐阅读顺序。[返回 README 总导航](../README.md#-23-篇文档怎么读)
>
> ⏱ **本文预计阅读时间**: 60 分钟
> 📍 **当前位置**: 第 17 / 24 篇

- ⏮ **上一篇**: [坐标变换](50-tf2.md)
- ⏭ **下一篇**: [launch 文件系统](70-launch.md)