From a100e44ac0378a33431f8e02d824a72d9e833251 Mon Sep 17 00:00:00 2001
From: zhangshun <453905631@qq.com>
Date: Tue, 16 Jun 2026 12:45:27 +0800
Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E6=9C=BA=E5=99=A8=E4=BA=BA?=
 =?UTF-8?q?=E8=BF=90=E5=8A=A8=E5=AD=A6=E8=83=BD=E5=8A=9B=E8=AF=B4=E6=98=8E?=
 =?UTF-8?q?=E6=96=87=E6=A1=A3?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 docs/robot_kinematics_capabilities.md | 270 ++++++++++++++++++++++++++
 1 file changed, 270 insertions(+)
 create mode 100644 docs/robot_kinematics_capabilities.md

diff --git a/docs/robot_kinematics_capabilities.md b/docs/robot_kinematics_capabilities.md
new file mode 100644
index 0000000..f781298
--- /dev/null
+++ b/docs/robot_kinematics_capabilities.md
@@ -0,0 +1,270 @@
+# 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 几何的碰撞模型构建。
+
+### 奇异点检测
+
+当前没有显式计算雅可比矩阵条件数或可操作度。
+
+尚未支持：
+
+- 奇异点预警。
+- 腕部奇异。
+- 肩部奇异。
+- 肘部奇异。
+- 接近奇异点时的速度放大风险提示。
+
+### 速度、加速度和 jerk 约束
+
+当前主要约束位置上下限，尚未完整处理工业轨迹中的速度、加速度和 jerk 约束。
+
+尚未支持：
+
+- 关节速度限制。
+- 关节加速度限制。
+- 笛卡尔速度限制。
+- 笛卡尔加速度限制。
+- jerk 限制。
+- 时间最优轨迹规划。
+
+### 多 IK 解管理
+
+工业机器人通常存在多个 IK 分支。当前模块没有显式管理多解分支。
+
+尚未支持：
+
+- 肘上/肘下选择。
+- 腕翻转/不翻转选择。
+- 肩部左右构型选择。
+- 最接近当前姿态的解选择。
+- 解连续性筛选。
+
+### 轨迹连续性和最短路径
+
+当前插补和 IK 可以生成关节序列，但还不是完整工业级轨迹规划。
+
+尚未支持：
+
+- 关节空间最短路径选择。
+- 关节跨越 `+-pi` 时的连续性处理。
+- 多点轨迹平滑。
+- 速度连续和加速度连续约束。
+- 轨迹失败点定位和恢复。
+
+### 工具坐标系和工件坐标系
+
+当前主要使用 URDF 链路末端作为 TCP，尚未形成完整坐标系管理能力。
+
+尚未支持：
+
+- 工具坐标系 TCP 管理。
+- 工件坐标系/用户坐标系管理。
+- 基坐标、世界坐标、工具坐标之间的统一转换。
+- 多 TCP 切换。
+
+### 工业机器人指令语义
+
+当前支持基础运动学计算，但还没有完整工业机器人指令层。
+
+尚未支持：
+
+- PTP 指令。
+- LIN 指令。
+- CIRC 指令。
+- Blend/Zone 过渡。
+- 等待、IO、夹具动作与轨迹同步。
+- 程序段级离线编程。
+
+### 动力学和负载
+
+当前模块不处理动力学。
+
+尚未支持：
+
+- 质量和惯量参与计算。
+- 关节力矩计算。
+- 重力补偿。
+- 负载模型。
+- 动态可达性判断。
+
+### 控制器通讯和实时控制
+
+当前模块是 WASM 运动学计算内核，不是实时控制器。
+
+尚未支持：
+
+- 与真实机器人控制器通讯。
+- 实时伺服控制。
+- 关节反馈闭环。
+- 安全互锁。
+- 急停、安全区、限位开关等控制器级安全能力。
+
+## 建议后续增强顺序
+
+如果继续按工业机器人实际使用场景增强，建议优先级如下：
+
+1. 增加奇异点检测和雅可比分析。
+2. 增加多 IK 解和构型选择。
+3. 增加关节连续性和最短路径处理。
+4. 增加速度、加速度和轨迹时间参数化。
+5. 增加工具坐标系和工件坐标系管理。
+6. 增加 PTP、LIN、CIRC 等工业运动指令。
+7. 增加碰撞检测。
+8. 增加控制器通讯和实时执行相关能力。
+