17 KiB
17 KiB
URDF/Orocos KDL 机器人算法接口文档
本文档说明当前机器人运动学模块对外暴露的业务接口，包括功能、参数、返回值和单位约定。接口定义以 src/api/KinematicsWebAPI.RobotCommands.cpp 当前实现为准。
1. 通用调用格式
所有业务命令都通过统一 JSON 请求进入 KinematicsWebAPI::func。
{
"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 | 否 | 命令参数。 |
统一响应外层格式如下：
{
"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 | 当前时间字符串。 |
示例：
{
"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 | 当前时间字符串。 |
失败时返回：
{
"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 失败，常见原因包括关节超限。 |
示例：
{
"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;...，单位为弧度。 |
返回值：
当前返回存在一层历史嵌套，结构如下：
{
"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 失败时，若已有成功结果则复用上一组结果，否则复用初始关节角，避免轨迹数组中断。
示例：
{
"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 失败，会复用上一组成功结果；若还没有成功结果，则复用初始关节角。
示例：
{
"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 雅可比矩阵。 |
示例：
{
"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 输入关节角。