# 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 输入关节角。