# URDF/Orocos KDL 工业机器人运动学能力说明

本文档说明当前 URDF/Orocos KDL 机器人运动学模块已经支持的能力，以及距离完整工业级机器人控制器还需要补齐的能力。

## 当前已支持能力

### 多机器人 URDF 注册

模块支持通过 `Cmd_InitRobot` 动态注册 URDF，并用 `robot_uuid` 管理不同机器人实例。当前也支持查询机器人、列出机器人和删除机器人。

相关能力：

- `Cmd_InitRobot`：注册或更新机器人 URDF。
- `Cmd_GetRobot`：查询指定机器人实例。
- `Cmd_ListRobots`：列出当前机器人实例。
- `Cmd_RemoveRobot`：删除机器人实例。

### 自动推导运动学链

初始化时不再固定依赖 `base`、`tool0`、`base_link` 等特定 link 名称。模块会基于 KDL Tree 自动推导 6 轴串联运动学链。

当前策略：

- 优先兼容旧命名链路 `base -> tool0`。
- 如果旧链路不存在，则从 URDF 的真实根节点出发，扫描叶子 link。
- 选择一条正好包含 6 个活动关节的串联链。

### 正运动学 FK

模块支持输入 6 个关节角，计算 TCP 位姿。

输出内容：

- TCP 位置：`x, y, z`
- TCP 姿态：四元数 `qx, qy, qz, qw`

对应接口：

- `Cmd_Kinematics_forward_pose_str`

### 全关节位姿 FK

模块支持计算每个关节节点的位姿，并按前端需要的 `OPERATION.frames.objStates` 格式输出。

该能力适用于：

- three.js 机器人显示。
- 关节动画回放。
- 数字孪生场景姿态同步。
- 简单离线仿真预览。

对应接口：

- `Cmd_Kinematics_forward_all_joints`

### 逆运动学 IK

模块支持输入目标 TCP 位姿和初始关节角，计算对应关节解。

当前主路径使用 KDL LMA 求解器，另保留 NR 求解接口。

对应接口：

- `Cmd_Kinematics_inverse_pose_str`
- `Cmd_Kinematics_inverse_pose_str_2PSteps`
- `Cmd_Kinematics_inverse_pose_str_NoDifference`

### 姿态轨迹插补和逐点 IK

模块支持对两个姿态点之间进行插补，然后逐点执行 IK，生成一串关节解。

当前支持两种方式：

- 自动估算插补步数。
- 由调用方指定插补步数。

适用场景：

- 前端拖动目标位姿后的简单路径预览。
- 离线验证 TCP 从起点到终点的可达性。
- 生成用于动画显示的关节序列。

### 不插补的多点 IK

模块支持直接对输入的多个目标姿态点逐点逆解，不在相邻点之间插补。

适用场景：

- 已有外部轨迹点。
- 只需要验证离散点位是否可达。
- 调试 IK 解算稳定性。

对应接口：

- `Cmd_Kinematics_inverse_pose_str_NoDifference`

### 关节上下限检查

模块已支持从 URDF 的 `<limit lower="..." upper="...">` 中读取关节上下限。

当前检查范围：

- FK 输入关节值。
- 全关节 FK 输入关节值。
- IK 初始关节值。
- IK 求解结果。

如果关节值超出 URDF 定义范围，计算会返回失败，不会继续输出不可用结果。

### three.js 前端测试页面

当前已有 three.js 测试页面，用于验证 URDF 注册、机器人显示、FK、IK 和姿态回放。

页面能力包括：

- 加载并注册 URDF。
- 显示 3D 机器人模型。
- 测试 FK。
- 测试 IK。
- 应用 IK 结果到关节状态。
- 查看 JSON 请求和响应。

## 当前定位

当前模块更接近“工业机器人运动学仿真和前端验证内核”。

它已经具备工业机器人常见的基础运动学能力：

- URDF 模型注册。
- 运动学链自动推导。
- FK。
- 全关节 FK。
- IK。
- 简单轨迹插补。
- 关节上下限约束。
- 前端 3D 可视化测试。

这些能力适合用于：

- 机器人模型可用性检查。
- 前端 3D 调试。
- 数字孪生姿态同步。
- 离线点位可达性验证。
- 简单路径和动画预览。

## 尚未支持的完整工业级能力

以下能力目前尚未完整支持，如果要接近真实工业机器人控制器，需要继续补齐。

### 碰撞检测

当前不处理碰撞相关内容。

尚未支持：

- 机器人自碰撞检测。
- 机器人与环境碰撞检测。
- 工具与工件碰撞检测。
- 基于 URDF collision 几何的碰撞模型构建。

### 奇异点检测

模块已支持基于 TCP 雅可比矩阵的奇异点检测。

当前支持：

- 计算 6x6 TCP 雅可比矩阵。
- 计算雅可比矩阵奇异值。
- 输出最小奇异值、最大奇异值、条件数和 manipulability。
- 输出 rank。
- 根据阈值给出 `normal`、`warning`、`singular` 风险等级。
- 在 three.js 测试页面中通过“奇异点”按钮直接查看当前关节姿态风险。

对应接口：

- `Cmd_Kinematics_check_singularity`

请求示例：

```json
{
  "msg": "singularity check",
  "req_code": "CHECK_SINGULARITY_001",
  "req_from": "client",
  "req_cmd": "Cmd_Kinematics_check_singularity",
  "req_param": {
    "robot_uuid": "abb_irb120_3_58",
    "joints_str": "0.15,-0.25,0.35,0.1,-0.2,0.3"
  }
}
```

可选阈值参数：

```json
{
  "singular_threshold": 0.0001,
  "warning_threshold": 0.01,
  "condition_threshold": 1000000,
  "condition_warning_threshold": 10000
}
```

响应中的关键字段：

- `risk_level`：`normal`、`warning` 或 `singular`。
- `is_singular`：是否已判定为奇异。
- `is_near_singular`：是否接近奇异。
- `rank`：雅可比矩阵秩。
- `singular_values`：奇异值数组。
- `min_singular_value`：最小奇异值。
- `condition_number`：条件数。
- `manipulability`：可操作度指标。
- `jacobian`：6x6 TCP 雅可比矩阵。

注意：当前检测给出数值风险等级，尚未进一步分类为腕部奇异、肩部奇异或肘部奇异。

### 速度、加速度和 jerk 约束

当前主要约束位置上下限，尚未完整处理工业轨迹中的速度、加速度和 jerk 约束。

尚未支持：

- 关节速度限制。
- 关节加速度限制。
- 笛卡尔速度限制。
- 笛卡尔加速度限制。
- jerk 限制。
- 时间最优轨迹规划。

### 多 IK 解管理

工业机器人通常存在多个 IK 分支。当前模块没有显式管理多解分支。

尚未支持：

- 肘上/肘下选择。
- 腕翻转/不翻转选择。
- 肩部左右构型选择。
- 最接近当前姿态的解选择。
- 解连续性筛选。

### 轨迹连续性和最短路径

当前插补和 IK 可以生成关节序列，但还不是完整工业级轨迹规划。

尚未支持：

- 关节空间最短路径选择。
- 关节跨越 `+-pi` 时的连续性处理。
- 多点轨迹平滑。
- 速度连续和加速度连续约束。
- 轨迹失败点定位和恢复。

### 工具坐标系和工件坐标系

当前主要使用 URDF 链路末端作为 TCP，尚未形成完整坐标系管理能力。

尚未支持：

- 工具坐标系 TCP 管理。
- 工件坐标系/用户坐标系管理。
- 基坐标、世界坐标、工具坐标之间的统一转换。
- 多 TCP 切换。

### 工业机器人指令语义

当前支持基础运动学计算，但还没有完整工业机器人指令层。

尚未支持：

- PTP 指令。
- LIN 指令。
- CIRC 指令。
- Blend/Zone 过渡。
- 等待、IO、夹具动作与轨迹同步。
- 程序段级离线编程。

### 动力学和负载

当前模块不处理动力学。

尚未支持：

- 质量和惯量参与计算。
- 关节力矩计算。
- 重力补偿。
- 负载模型。
- 动态可达性判断。

### 控制器通讯和实时控制

当前模块是 WASM 运动学计算内核，不是实时控制器。

尚未支持：

- 与真实机器人控制器通讯。
- 实时伺服控制。
- 关节反馈闭环。
- 安全互锁。
- 急停、安全区、限位开关等控制器级安全能力。

## 建议后续增强顺序

如果继续按工业机器人实际使用场景增强，建议优先级如下：

1. 增加多 IK 解和构型选择。
2. 增加关节连续性和最短路径处理。
3. 增加速度、加速度和轨迹时间参数化。
4. 增加工具坐标系和工件坐标系管理。
5. 增加 PTP、LIN、CIRC 等工业运动指令。
6. 增加碰撞检测。
7. 增加奇异类型分类，例如腕部、肩部、肘部奇异。
8. 增加控制器通讯和实时执行相关能力。
