# 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 的 `` 中读取关节上下限。 当前检查范围： - 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. 增加控制器通讯和实时执行相关能力。