补充机器人算法接口文档

docs/robot_kinematics_api.md

@@ -0,0 +1,496 @@
+# URDF/Orocos KDL 机器人算法接口文档
+
+本文档说明当前机器人运动学模块对外暴露的业务接口，包括功能、参数、返回值和单位约定。接口定义以 `src/api/KinematicsWebAPI.RobotCommands.cpp` 当前实现为准。
+
+## 1. 通用调用格式
+
+所有业务命令都通过统一 JSON 请求进入 `KinematicsWebAPI::func`。
+
+```json
+{
+  "msg": "request message",
+  "req_code": "REQ_001",
+  "req_from": "client",
+  "req_cmd": "Cmd_Name",
+  "req_param": {}
+}
+```
+
+| 字段 | 类型 | 必填 | 说明 |
+| --- | --- | --- | --- |
+| `msg` | string | 否 | 调用说明。失败时如果业务层没有更具体错误，可能回传为响应 `msg`。 |
+| `req_code` | string | 否 | 调用方请求编号，响应中原样返回。 |
+| `req_from` | string | 否 | 调用来源，响应中原样返回。 |
+| `req_cmd` | string | 是 | 命令名称。 |
+| `req_param` | object | 否 | 命令参数。 |
+
+统一响应外层格式如下：
+
+```json
+{
+  "success": true,
+  "code": 0,
+  "msg": "request message",
+  "req_code": "REQ_001",
+  "req_from": "client",
+  "req_cmd": "Cmd_Name",
+  "timestamp": 1792137600,
+  "res_data": {}
+}
+```
+
+| 字段 | 类型 | 说明 |
+| --- | --- | --- |
+| `success` | boolean | 外层调用是否成功。业务层返回 `success:false` 或 `error` 时为 `false`。 |
+| `code` | number | `0` 表示成功，`1000` 表示业务失败，`400` 表示 JSON 格式错误，`500` 表示处理异常。 |
+| `msg` | string | 响应消息。失败时优先取业务层 `message` 或 `error`。 |
+| `res_data` | object | 业务命令返回体。本文后续“返回值”均指 `res_data` 内部结构。 |
+
+## 2. 单位和格式约定
+
+| 数据 | 格式 | 单位 |
+| --- | --- | --- |
+| 关节角 | `j1,j2,j3,j4,j5,j6` | 弧度 rad |
+| 关节角序列 | `j1,j2,j3,j4,j5,j6;j1,j2,j3,j4,j5,j6` | 弧度 rad |
+| TCP 位姿 | `x,y,z,qx,qy,qz,qw` | 位置为米 m，姿态为四元数 |
+| TCP 位姿序列 | `x,y,z,qx,qy,qz,qw;x,y,z,qx,qy,qz,qw` | 位置为米 m，姿态为四元数 |
+| URDF 关节上下限 | URDF `<limit lower upper>` | 旋转关节为弧度 rad |
+| `objStates` 位姿 | `tx,ty,tz,qx,qy,qz,qw` | 位置为米 m，姿态为四元数 |
+
+注意：WASM/KDL 侧返回的 IK 关节结果是弧度。如果前端工艺数据使用角度，需要在前端显式做 `rad -> deg` 转换。
+
+## 3. 机器人管理接口
+
+### 3.1 `Cmd_InitRobot`
+
+功能：注册或更新机器人 URDF，并初始化 KDL Tree、6 轴运动学链、FK/IK 求解器、关节上下限和关节 child link 映射。
+
+参数：
+
+| 字段 | 类型 | 必填 | 默认值 | 说明 |
+| --- | --- | --- | --- | --- |
+| `robot_uuid` | string | 否 | `""` | 机器人实例 ID。为空时内部使用 URDF 内容哈希作为 ID。 |
+| `urdf_base64` | string | 是 | `""` | URDF 文件内容的 base64 字符串。 |
+| `force_update` | boolean | 否 | `true` | 是否强制更新。为 `false` 且内容未变化时跳过更新。 |
+
+返回值：
+
+| 字段 | 类型 | 说明 |
+| --- | --- | --- |
+| `success` | boolean | 初始化是否成功。 |
+| `message` | string | 初始化结果说明。 |
+| `timestamp` | string | 当前时间字符串。 |
+
+示例：
+
+```json
+{
+  "req_cmd": "Cmd_InitRobot",
+  "req_param": {
+    "robot_uuid": "abb_irb120_3_58",
+    "urdf_base64": "PD94bWwgdmVyc2lvbj0iMS4wIj8+...",
+    "force_update": true
+  }
+}
+```
+
+### 3.2 `Cmd_GetRobot`
+
+功能：查询指定机器人实例是否存在、是否已初始化以及关节数量。
+
+参数：
+
+| 字段 | 类型 | 必填 | 默认值 | 说明 |
+| --- | --- | --- | --- | --- |
+| `uuid` | string | 是 | `""` | 机器人实例 ID。注意该接口当前字段名为 `uuid`，不是 `robot_uuid`。 |
+
+返回值：
+
+| 字段 | 类型 | 说明 |
+| --- | --- | --- |
+| `success` | boolean | 查询是否成功。 |
+| `uuid` | string | 机器人实例 ID。 |
+| `initialized` | boolean | 机器人是否已初始化。 |
+| `joints_count` | number | 当前运动学链关节数量。 |
+| `timestamp` | string | 当前时间字符串。 |
+
+失败时返回：
+
+```json
+{
+  "error": "Robot not found"
+}
+```
+
+### 3.3 `Cmd_RemoveRobot`
+
+功能：删除指定机器人实例和对应 URDF 哈希记录。
+
+参数：
+
+| 字段 | 类型 | 必填 | 默认值 | 说明 |
+| --- | --- | --- | --- | --- |
+| `uuid` | string | 是 | `""` | 机器人实例 ID。注意该接口当前字段名为 `uuid`，不是 `robot_uuid`。 |
+
+返回值：
+
+| 字段 | 类型 | 说明 |
+| --- | --- | --- |
+| `success` | boolean | 删除是否成功。 |
+| `message` | string | 删除结果说明。 |
+| `timestamp` | string | 当前时间字符串。 |
+
+### 3.4 `Cmd_ListRobots`
+
+功能：列出当前已注册的机器人实例。
+
+参数：
+
+| 字段 | 类型 | 必填 | 默认值 | 说明 |
+| --- | --- | --- | --- | --- |
+| `detail` | boolean | 否 | `false` | 是否返回关节数量、校验状态和 URDF 哈希摘要。 |
+
+返回值：
+
+| 字段 | 类型 | 说明 |
+| --- | --- | --- |
+| `success` | boolean | 是否查询成功。 |
+| `robots` | object | 机器人字典。键为机器人 ID，值为实例说明或详细信息字符串。 |
+| `count` | number | 当前机器人数量。 |
+| `timestamp` | string | 当前时间字符串。 |
+
+## 4. 正运动学接口
+
+### 4.1 `Cmd_Kinematics_forward_pose_str`
+
+功能：输入 6 个关节角，计算 TCP 位姿。
+
+参数：
+
+| 字段 | 类型 | 必填 | 默认值 | 说明 |
+| --- | --- | --- | --- | --- |
+| `robot_uuid` | string | 否 | `"default"` | 机器人实例 ID。 |
+| `q_init_str` | string | 否 | `"0,0,0,0,0,0"` | 6 个关节角，单位为弧度。该字段名为历史命名，实际表示 FK 输入关节。 |
+
+返回值：
+
+| 字段 | 类型 | 说明 |
+| --- | --- | --- |
+| `position` | number[] | TCP 位置 `[x,y,z]`，单位为米。 |
+| `orientation` | number[] | TCP 四元数 `[qx,qy,qz,qw]`。 |
+| `joints` | number[] | 输入关节角，单位为弧度。 |
+| `success` | boolean | 正解是否成功。 |
+
+失败时常见错误：
+
+| 错误 | 说明 |
+| --- | --- |
+| `Robot not found or not initialized` | 机器人未注册或初始化失败。 |
+| `Invalid joints format` | 关节字符串不是 6 个数值。 |
+| `Forward kinematics calculation failed` | FK 失败，常见原因包括关节超限。 |
+
+示例：
+
+```json
+{
+  "req_cmd": "Cmd_Kinematics_forward_pose_str",
+  "req_param": {
+    "robot_uuid": "abb_irb120_3_58",
+    "q_init_str": "0.15,-0.25,0.35,0.1,-0.2,0.3"
+  }
+}
+```
+
+### 4.2 `Cmd_Kinematics_forward_all_joints`
+
+功能：输入一帧或多帧 6 轴关节角，计算每一帧中各关节节点位姿，并按前端 `OPERATION.frames.objStates` 格式输出。
+
+参数：
+
+| 字段 | 类型 | 必填 | 默认值 | 说明 |
+| --- | --- | --- | --- | --- |
+| `robot_uuid` | string | 否 | `"default"` | 机器人实例 ID。 |
+| `joints_str` | string | 否 | `"0,0,0,0,0,0"` | 关节角序列，格式为 `j1,j2,j3,j4,j5,j6;...`，单位为弧度。 |
+
+返回值：
+
+当前返回存在一层历史嵌套，结构如下：
+
+```json
+{
+  "success": true,
+  "OPERATION": {
+    "success": true,
+    "count": 2,
+    "OPERATION": {
+      "frames": [
+        {
+          "time": "0.020000",
+          "objStates": [
+            {
+              "i": "child_link_uuid",
+              "tx": 0,
+              "ty": 0,
+              "tz": 0,
+              "qx": 0,
+              "qy": 0,
+              "qz": 0,
+              "qw": 1
+            }
+          ]
+        }
+      ]
+    }
+  }
+}
+```
+
+| 字段 | 类型 | 说明 |
+| --- | --- | --- |
+| `OPERATION.success` | boolean | 批量 FK 是否成功。 |
+| `OPERATION.count` | number | 输入关节帧数量。 |
+| `OPERATION.OPERATION.frames` | array | 前端播放帧。 |
+| `frames[].time` | string | 当前实现固定为 `"0.020000"`。 |
+| `frames[].objStates` | array | 每个关节 child link 对应的对象位姿。 |
+| `objStates[].i` | string | URDF 中解析出的 child link UUID。 |
+| `objStates[].tx/ty/tz` | number | link 位置，单位为米。 |
+| `objStates[].qx/qy/qz/qw` | number | link 姿态四元数。 |
+
+失败时常见错误：
+
+| 错误 | 说明 |
+| --- | --- |
+| `Empty joints input` | 未解析到有效关节帧。 |
+| `每组关节角都必须包含 6 个值` | 某一帧关节数量不是 6。 |
+| `Forward kinematics calculation for all joints failed` | FK 失败，常见原因包括关节超限。 |
+
+## 5. 逆运动学接口
+
+### 5.1 `Cmd_Kinematics_inverse_pose_str`
+
+功能：输入一个或多个 TCP 位姿，执行逆运动学。单点时直接求解；多点时对相邻位姿自动估算步数并插补，再逐点求 IK。
+
+参数：
+
+| 字段 | 类型 | 必填 | 默认值 | 说明 |
+| --- | --- | --- | --- | --- |
+| `robot_uuid` | string | 否 | `"default"` | 机器人实例 ID。 |
+| `pose_str` | string | 是 | `""` | 位姿或位姿序列，格式为 `x,y,z,qx,qy,qz,qw;...`。位置单位为米。 |
+| `q_init_str` | string | 否 | `"0,0,0,0,0,0"` | IK 初始关节角，单位为弧度。用于影响多解选择和求解连续性。 |
+
+返回值：
+
+| 字段 | 类型 | 说明 |
+| --- | --- | --- |
+| `joints` | number[][] | 逆解关节序列，每组 6 个关节角，单位为弧度。 |
+| `success` | boolean | IK 是否成功。 |
+
+实现说明：
+
+- 单点输入返回 1 组关节解。
+- 多点输入会对相邻点自动插补，自动步数范围当前为 10 到 100。
+- 逐点 IK 成功后，会把上一点结果作为下一点初值。
+- 逐点 IK 失败时，若已有成功结果则复用上一组结果，否则复用初始关节角，避免轨迹数组中断。
+
+示例：
+
+```json
+{
+  "req_cmd": "Cmd_Kinematics_inverse_pose_str",
+  "req_param": {
+    "robot_uuid": "abb_irb120_3_58",
+    "pose_str": "0.374,0,0.63,0,0,0,1",
+    "q_init_str": "0,0,0,0,0,0"
+  }
+}
+```
+
+### 5.2 `Cmd_Kinematics_inverse_pose_str_2PSteps`
+
+功能：输入两个 TCP 位姿，按调用方指定步数进行插补，并对插补点逐点求 IK。主要用于 MoveL 直线路径的离散关节解生成。
+
+参数：
+
+| 字段 | 类型 | 必填 | 默认值 | 说明 |
+| --- | --- | --- | --- | --- |
+| `robot_uuid` | string | 否 | `"default"` | 机器人实例 ID。 |
+| `pose_str` | string | 是 | `""` | 起点和终点位姿，格式为 `pose1;pose2`。如果只传 1 个点，则直接求单点 IK。 |
+| `q_init_str` | string | 否 | `"0,0,0,0,0,0"` | IK 初始关节角，单位为弧度。 |
+| `steps_str` | string | 是 | `"default"` | 插补点数量字符串，例如 `"30"`。当前实现会调用 `stoi` 转整数，不能传非数字。 |
+
+返回值：
+
+| 字段 | 类型 | 说明 |
+| --- | --- | --- |
+| `joints` | number[][] | 逆解关节序列，每组 6 个关节角，单位为弧度。 |
+| `success` | boolean | IK 是否成功。 |
+
+实现说明：
+
+- 返回值是弧度，不是角度。
+- 插补过程中每一点 IK 成功后，会更新下一点的初值。
+- 如果某个插补点 IK 失败，会复用上一组成功结果；若还没有成功结果，则复用初始关节角。
+
+示例：
+
+```json
+{
+  "req_cmd": "Cmd_Kinematics_inverse_pose_str_2PSteps",
+  "req_param": {
+    "robot_uuid": "abb_irb120_3_58",
+    "pose_str": "0.374,0,0.63,0,0,0,1;0.4626,0.2209,0.334,0.148283,0.435718,0.079062,0.884258",
+    "q_init_str": "0,0,0,0,0,0",
+    "steps_str": "30"
+  }
+}
+```
+
+### 5.3 `Cmd_Kinematics_inverse_pose_str_NoDifference`
+
+功能：直接对输入位姿点执行 IK，不做相邻点插补。适用于调用方已经生成了离散轨迹点，只需要逐点求关节解的场景。
+
+参数：
+
+| 字段 | 类型 | 必填 | 默认值 | 说明 |
+| --- | --- | --- | --- | --- |
+| `robot_uuid` | string | 否 | `"default"` | 机器人实例 ID。 |
+| `pose_str` | string | 是 | `""` | 位姿或位姿序列，格式为 `x,y,z,qx,qy,qz,qw;...`。 |
+| `q_init_str` | string | 否 | `"0,0,0,0,0,0"` | IK 初始关节角，单位为弧度。 |
+
+返回值：
+
+| 字段 | 类型 | 说明 |
+| --- | --- | --- |
+| `joints` | number[][] | 逆解关节序列，每组 6 个关节角，单位为弧度。 |
+| `success` | boolean | IK 是否成功。 |
+
+实现说明：
+
+- 单点输入返回 1 组关节解。
+- 多点输入不插补，逐点求解，并用上一点成功结果作为下一点初值。
+- 当前多点实现循环到 `posePoints.size() - 1`，最后一个输入位姿不会被求解；如果业务需要完整多点结果，建议后续修正。
+
+## 6. 奇异点检测接口
+
+### 6.1 `Cmd_Kinematics_check_singularity`
+
+功能：基于当前关节姿态计算 TCP 雅可比矩阵，对奇异值、条件数和可操作度进行评估，返回 `normal`、`warning` 或 `singular` 风险等级。
+
+参数：
+
+| 字段 | 类型 | 必填 | 默认值 | 说明 |
+| --- | --- | --- | --- | --- |
+| `robot_uuid` | string | 否 | `"default"` | 机器人实例 ID。 |
+| `joints_str` | string | 否 | `q_init_str` 或 `"0,0,0,0,0,0"` | 6 个关节角，单位为弧度。 |
+| `q_init_str` | string | 否 | `"0,0,0,0,0,0"` | 兼容字段。未传 `joints_str` 时使用。 |
+| `singular_threshold` | number | 否 | `1e-4` | 最小奇异值低于或等于该阈值时判定为奇异。 |
+| `warning_threshold` | number | 否 | `1e-2` | 最小奇异值低于或等于该阈值时判定为接近奇异。 |
+| `condition_threshold` | number | 否 | `1e6` | 条件数大于或等于该阈值时判定为奇异。 |
+| `condition_warning_threshold` | number | 否 | `1e4` | 条件数大于或等于该阈值时判定为接近奇异。 |
+
+返回值：
+
+| 字段 | 类型 | 说明 |
+| --- | --- | --- |
+| `success` | boolean | 检测是否成功。 |
+| `is_singular` | boolean | 是否判定为奇异。 |
+| `is_near_singular` | boolean | 是否判定为接近奇异。 |
+| `risk_level` | string | 风险等级：`normal`、`warning`、`singular`。 |
+| `rank` | number | 雅可比矩阵秩。 |
+| `joint_count` | number | 运动学链关节数量。 |
+| `min_singular_value` | number | 最小奇异值。 |
+| `max_singular_value` | number | 最大奇异值。 |
+| `condition_number` | number | 条件数。最小奇异值为 0 时为无穷大。 |
+| `manipulability` | number | 可操作度指标，当前为全部奇异值乘积。 |
+| `singular_values` | number[] | 雅可比矩阵奇异值。 |
+| `thresholds` | object | 本次检测使用的阈值。 |
+| `joints` | number[] | 输入关节角，单位为弧度。 |
+| `jacobian` | number[][] | TCP 雅可比矩阵。 |
+
+示例：
+
+```json
+{
+  "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"
+  }
+}
+```
+
+风险判断逻辑：
+
+- `singular`：`min_singular_value <= singular_threshold`，或 `condition_number >= condition_threshold`，或 `rank < joint_count`。
+- `warning`：未达到 `singular`，但 `min_singular_value <= warning_threshold` 或 `condition_number >= condition_warning_threshold`。
+- `normal`：不满足以上风险条件。
+
+失败时常见错误：
+
+| 错误 | 说明 |
+| --- | --- |
+| `Invalid joints format` | 关节字符串不是 6 个数值。 |
+| `Joint value out of limits` | 输入关节超出 URDF 上下限。 |
+| `Jacobian calculation failed` | 雅可比矩阵计算失败。 |
+
+## 7. 非运动学占位接口
+
+以下命令目前也由机器人命令处理器识别，但不是机器人运动学算法接口，当前仅返回固定成功结构。
+
+### 7.1 `Cmd_SelectCraftTree`
+
+功能：占位接口，表示选择工艺树。
+
+参数：
+
+| 字段 | 类型 | 必填 | 默认值 | 说明 |
+| --- | --- | --- | --- | --- |
+| `tree_id` | string | 否 | `""` | 工艺树 ID。 |
+
+返回值：
+
+| 字段 | 类型 | 说明 |
+| --- | --- | --- |
+| `success` | boolean | 固定为 `true`。 |
+| `tree_id` | string | 输入工艺树 ID。 |
+| `message` | string | 固定成功消息。 |
+| `timestamp` | string | 当前时间字符串。 |
+
+### 7.2 `Cmd_AddOperationTree`
+
+功能：占位接口，表示新增操作树。
+
+参数：
+
+| 字段 | 类型 | 必填 | 默认值 | 说明 |
+| --- | --- | --- | --- | --- |
+| `name` | string | 否 | `""` | 操作树名称。 |
+| `operations` | array | 否 | `[]` | 操作列表。 |
+
+返回值：
+
+| 字段 | 类型 | 说明 |
+| --- | --- | --- |
+| `success` | boolean | 固定为 `true`。 |
+| `tree_name` | string | 输入操作树名称。 |
+| `operations_count` | number | 输入操作数量。 |
+| `message` | string | 固定成功消息。 |
+| `timestamp` | string | 当前时间字符串。 |
+
+## 8. 关节上下限和初始化约束
+
+初始化机器人时会执行以下检查和准备：
+
+- URDF 必须能解析为 KDL Tree。
+- 模块会自动推导一条 6 轴串联运动学链，不再固定依赖 `base_link`、`base` 或 `tool0` 等名称。
+- 当前只支持 6 轴机器人链，非 6 轴会初始化失败。
+- 会从 URDF `<limit lower="..." upper="...">` 读取关节上下限。
+- FK 输入、全关节 FK 输入、IK 初值、IK 结果、奇异点检测输入都会执行关节上下限检查。
+
+## 9. 常见接入建议
+
+- 前端 MoveJ 工艺通常使用角度数据做关节空间插值，调用 FK 前需要统一转为弧度。
+- 前端 MoveL 工艺应把 TCP 位姿传给 IK 接口，IK 返回弧度后如需存入角度工艺数据，需要转为角度。
+- 连续 MoveL 或 MoveJ 后接 MoveL 时，建议把上一段末尾关节作为下一段 `q_init_str`，保证 IK 多解选择更连续。
+- 奇异点检测只能发现风险，不会自动规避路径；规避需要结合多解选择、路径调整、姿态微调或工艺点重规划。
+- `Cmd_Kinematics_forward_pose_str` 的入参字段名 `q_init_str` 属于历史命名，实际含义是 FK 输入关节角。