屏蔽算法运行时冗余日志

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

docs/robot_kinematics_capabilities.md

@@ -0,0 +1,314 @@
+# 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 几何的碰撞模型构建。
+
+### 奇异点检测
+
+模块已支持基于 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. 增加控制器通讯和实时执行相关能力。

docs/urdf.xml

@@ -0,0 +1,309 @@
+<robot
+	xmlns:xacro="http://ros.org/wiki/xacro" name="ABB_IRB_120" uuid="8bc48ac0-6570-4232-aa91-3367771c1069">
+	<link name="base_link" uuid="9c88ec70-a337-4d16-94c5-92ccff72c1a5">
+		<inertial>
+			<mass value="1.0"/>
+			<origin rpy="0 0 0" xyz="0 0 0"/>
+			<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+		</inertial>
+		<visual>
+			<origin rpy="0 0 0" xyz="0 0 0"/>
+			<geometry>
+				<mesh filename="package://abb_irb120_support/meshes/irb120_3_58/visual/link_1.stl"/>
+			</geometry>
+			<material name="">
+				<color rgba="0.7372549 0.3490196 0.1607843 1"/>
+			</material>
+		</visual>
+		<collision>
+			<origin rpy="0 0 0" xyz="0 0 0"/>
+			<geometry>
+				<mesh filename="package://abb_irb120_support/meshes/irb120_3_58/collision/link_1.stl"/>
+			</geometry>
+			<material name="">
+				<color rgba="1 1 0 1"/>
+			</material>
+		</collision>
+	</link>
+	<link name="link_1" uuid="09059ca4-6c99-4a5f-a474-8ba964e612c4">
+		<inertial>
+			<mass value="1.0"/>
+			<origin rpy="0 0 0" xyz="0 0 0"/>
+			<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+		</inertial>
+		<visual>
+			<origin rpy="0 0 0" xyz="0 0 0"/>
+			<geometry>
+				<mesh filename="package://abb_irb120_support/meshes/irb120_3_58/visual/link_2.stl"/>
+			</geometry>
+			<material name="">
+				<color rgba="0.7372549 0.3490196 0.1607843 1"/>
+			</material>
+		</visual>
+		<collision>
+			<origin rpy="0 0 0" xyz="0 0 0"/>
+			<geometry>
+				<mesh filename="package://abb_irb120_support/meshes/irb120_3_58/collision/link_2.stl"/>
+			</geometry>
+			<material name="">
+				<color rgba="1 1 0 1"/>
+			</material>
+		</collision>
+	</link>
+	<link name="link_2" uuid="3f38944a-9e19-4aba-a46e-2cc2d2611b91">
+		<inertial>
+			<mass value="1.0"/>
+			<origin rpy="0 0 0" xyz="0 0 0"/>
+			<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+		</inertial>
+		<visual>
+			<origin rpy="0 0 0" xyz="0 0 0"/>
+			<geometry>
+				<mesh filename="package://abb_irb120_support/meshes/irb120_3_58/visual/link_3.stl"/>
+			</geometry>
+			<material name="">
+				<color rgba="0.7372549 0.3490196 0.1607843 1"/>
+			</material>
+		</visual>
+		<collision>
+			<origin rpy="0 0 0" xyz="0 0 0"/>
+			<geometry>
+				<mesh filename="package://abb_irb120_support/meshes/irb120_3_58/collision/link_3.stl"/>
+			</geometry>
+			<material name="">
+				<color rgba="1 1 0 1"/>
+			</material>
+		</collision>
+	</link>
+	<link name="link_3" uuid="801f8e99-88d8-4a92-b848-3c573c5e6ddb">
+		<inertial>
+			<mass value="1.0"/>
+			<origin rpy="0 0 0" xyz="0 0 0"/>
+			<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+		</inertial>
+		<visual>
+			<origin rpy="0 0 0" xyz="0 0 0"/>
+			<geometry>
+				<mesh filename="package://abb_irb120_support/meshes/irb120_3_58/visual/link_4.stl"/>
+			</geometry>
+			<material name="">
+				<color rgba="0.7372549 0.3490196 0.1607843 1"/>
+			</material>
+		</visual>
+		<collision>
+			<origin rpy="0 0 0" xyz="0 0 0"/>
+			<geometry>
+				<mesh filename="package://abb_irb120_support/meshes/irb120_3_58/collision/link_4.stl"/>
+			</geometry>
+			<material name="">
+				<color rgba="1 1 0 1"/>
+			</material>
+		</collision>
+	</link>
+	<link name="link_4" uuid="c0146e6e-b9c7-4ada-9e96-b7bb141f6feb">
+		<inertial>
+			<mass value="1.0"/>
+			<origin rpy="0 0 0" xyz="0 0 0"/>
+			<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+		</inertial>
+		<visual>
+			<origin rpy="0 0 0" xyz="0 0 0"/>
+			<geometry>
+				<mesh filename="package://abb_irb120_support/meshes/irb120_3_58/visual/link_5.stl"/>
+			</geometry>
+			<material name="">
+				<color rgba="0.7372549 0.3490196 0.1607843 1"/>
+			</material>
+		</visual>
+		<collision>
+			<origin rpy="0 0 0" xyz="0 0 0"/>
+			<geometry>
+				<mesh filename="package://abb_irb120_support/meshes/irb120_3_58/collision/link_5.stl"/>
+			</geometry>
+			<material name="">
+				<color rgba="1 1 0 1"/>
+			</material>
+		</collision>
+	</link>
+	<link name="link_5" uuid="5a650225-e5cf-44ae-b2fc-aad215668177">
+		<inertial>
+			<mass value="1.0"/>
+			<origin rpy="0 0 0" xyz="0 0 0"/>
+			<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+		</inertial>
+		<visual>
+			<origin rpy="0 0 0" xyz="0 0 0"/>
+			<geometry>
+				<mesh filename="package://abb_irb120_support/meshes/irb120_3_58/visual/link_6.stl"/>
+			</geometry>
+			<material name="">
+				<color rgba="0.7372549 0.3490196 0.1607843 1"/>
+			</material>
+		</visual>
+		<collision>
+			<origin rpy="0 0 0" xyz="0 0 0"/>
+			<geometry>
+				<mesh filename="package://abb_irb120_support/meshes/irb120_3_58/collision/link_6.stl"/>
+			</geometry>
+			<material name="">
+				<color rgba="1 1 0 1"/>
+			</material>
+		</collision>
+	</link>
+	<link name="link_6" uuid="4d20b9a3-23a5-48d8-bed6-b50439da66c0">
+		<inertial>
+			<mass value="1.0"/>
+			<origin rpy="0 0 0" xyz="0 0 0"/>
+			<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
+		</inertial>
+		<visual>
+			<origin rpy="0 0 0" xyz="0 0 0"/>
+			<geometry>
+				<mesh filename="package://abb_irb120_support/meshes/irb120_3_58/visual/link_7.stl"/>
+			</geometry>
+			<material name="">
+				<color rgba="0.7372549 0.3490196 0.1607843 1"/>
+			</material>
+		</visual>
+		<collision>
+			<origin rpy="0 0 0" xyz="0 0 0"/>
+			<geometry>
+				<mesh filename="package://abb_irb120_support/meshes/irb120_3_58/collision/link_7.stl"/>
+			</geometry>
+			<material name="">
+				<color rgba="1 1 0 1"/>
+			</material>
+		</collision>
+	</link>
+	<joint name="joint_1" innerId="D0F7B07F-0438-4392-B775-42512D10BBD4" type="revolute">
+		<origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
+		<parent link="base_link" uuid="9c88ec70-a337-4d16-94c5-92ccff72c1a5"/>
+		<child link="link_1" uuid="09059ca4-6c99-4a5f-a474-8ba964e612c4"/>
+		<axis xyz="0 0 1"/>
+		<limit effort="0" lower="-6.283185" upper="6.283185" velocity="1.570796"/>
+		<dynamics damping="0.0" friction="0.0"/>
+	</joint>
+	<joint name="joint_2" innerId="3D80EDAC-92C7-492C-91FD-8108A298E175" type="revolute">
+		<origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.290000"/>
+		<parent link="link_1" uuid="09059ca4-6c99-4a5f-a474-8ba964e612c4"/>
+		<child link="link_2" uuid="3f38944a-9e19-4aba-a46e-2cc2d2611b91"/>
+		<axis xyz="1 0 0"/>
+		<limit effort="0" lower="-6.283185" upper="6.283185" velocity="1.570796"/>
+		<dynamics damping="0.0" friction="0.0"/>
+	</joint>
+	<joint name="joint_3" innerId="0A098978-9EAC-4F7B-B8DB-A1FFF640B6FF" type="revolute">
+		<origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.270000"/>
+		<parent link="link_2" uuid="3f38944a-9e19-4aba-a46e-2cc2d2611b91"/>
+		<child link="link_3" uuid="801f8e99-88d8-4a92-b848-3c573c5e6ddb"/>
+		<axis xyz="1 0 0"/>
+		<limit effort="0" lower="-6.283185" upper="6.283185" velocity="1.570796"/>
+		<dynamics damping="0.0" friction="0.0"/>
+	</joint>
+	<joint name="joint_4" innerId="823DDDE0-DBFA-4F5E-B3A6-9A87C06C8DB9" type="revolute">
+		<origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 -0.134000 0.070000"/>
+		<parent link="link_3" uuid="801f8e99-88d8-4a92-b848-3c573c5e6ddb"/>
+		<child link="link_4" uuid="c0146e6e-b9c7-4ada-9e96-b7bb141f6feb"/>
+		<axis xyz="0 1 0"/>
+		<limit effort="0" lower="-6.283185" upper="6.283185" velocity="1.570796"/>
+		<dynamics damping="0.0" friction="0.0"/>
+	</joint>
+	<joint name="joint_5" innerId="A3DD936A-C88D-4267-8926-1C29C144D52E" type="revolute">
+		<origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 -0.168000 0.000000"/>
+		<parent link="link_4" uuid="c0146e6e-b9c7-4ada-9e96-b7bb141f6feb"/>
+		<child link="link_5" uuid="5a650225-e5cf-44ae-b2fc-aad215668177"/>
+		<axis xyz="1 0 0"/>
+		<limit effort="0" lower="-6.283185" upper="6.283185" velocity="1.570796"/>
+		<dynamics damping="0.0" friction="0.0"/>
+	</joint>
+	<joint name="joint_6" innerId="85949D90-0C7B-463A-A18D-281A71FA4C19" type="revolute">
+		<origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 -0.072000 0.000000"/>
+		<parent link="link_5" uuid="5a650225-e5cf-44ae-b2fc-aad215668177"/>
+		<child link="link_6" uuid="4d20b9a3-23a5-48d8-bed6-b50439da66c0"/>
+		<axis xyz="0 1 0"/>
+		<limit effort="0" lower="-6.283185" upper="6.283185" velocity="1.570796"/>
+		<dynamics damping="0.0" friction="0.0"/>
+	</joint>
+	<link name="base" uuidBase="f25bddf0-6533-448a-a1f5-b1ebbabe7d3c"/>
+	<joint name="base_link-base" type="fixed">
+		<origin xyz="0.000000 0.000000 0.000000" rpy="0.000000 0.000000 0.000000"/>
+		<parent link="base"/>
+		<child link="base_link"/>
+	</joint>
+	<link name="flange"/>
+	<joint name="joint_6-flange" type="fixed">
+		<origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
+		<parent link="link_6"/>
+		<child link="flange"/>
+	</joint>
+	<link name="tool0" uuidTool="5272238a-a622-412b-aa83-3e748fe57798"/>
+	<joint name="link_6-tool0" type="fixed">
+		<origin rpy="0.000000 0.000000 0.000000" xyz="0.000000 0.000000 0.000000"/>
+		<parent link="flange"/>
+		<child link="tool0"/>
+	</joint>
+	<transmission name="joint_1">
+		<type>transmission_interface/SimpleTransmission</type>
+		<joint>
+			<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+		</joint>
+		<actuator name="joint_1_motor">
+			<hardwareInterface>1</hardwareInterface>
+			<mechanicalReduction/>
+		</actuator>
+	</transmission>
+	<transmission name="joint_2">
+		<type>transmission_interface/SimpleTransmission</type>
+		<joint>
+			<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+		</joint>
+		<actuator name="joint_2_motor">
+			<hardwareInterface>1</hardwareInterface>
+			<mechanicalReduction/>
+		</actuator>
+	</transmission>
+	<transmission name="joint_3">
+		<type>transmission_interface/SimpleTransmission</type>
+		<joint>
+			<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+		</joint>
+		<actuator name="joint_3_motor">
+			<hardwareInterface>1</hardwareInterface>
+			<mechanicalReduction/>
+		</actuator>
+	</transmission>
+	<transmission name="joint_4">
+		<type>transmission_interface/SimpleTransmission</type>
+		<joint>
+			<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+		</joint>
+		<actuator name="joint_4_motor">
+			<hardwareInterface>1</hardwareInterface>
+			<mechanicalReduction/>
+		</actuator>
+	</transmission>
+	<transmission name="joint_5">
+		<type>transmission_interface/SimpleTransmission</type>
+		<joint>
+			<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+		</joint>
+		<actuator name="joint_5_motor">
+			<hardwareInterface>1</hardwareInterface>
+			<mechanicalReduction/>
+		</actuator>
+	</transmission>
+	<transmission name="joint_6">
+		<type>transmission_interface/SimpleTransmission</type>
+		<joint>
+			<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
+		</joint>
+		<actuator name="joint_6_motor">
+			<hardwareInterface>1</hardwareInterface>
+			<mechanicalReduction/>
+		</actuator>
+	</transmission>
+	<gazebo>
+		<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
+			<robotNamespace>/</robotNamespace>
+		</plugin>
+	</gazebo>
+</robot>

include/Robot.h

@@ -5,9 +5,12 @@
 #include <kdl/chainfksolverpos_recursive.hpp>
 #include <kdl/chainiksolverpos_lma.hpp>
 #include <kdl/chainiksolverpos_nr.hpp>
+#include <kdl/chainiksolverpos_nr_jl.hpp>
+#include <kdl/chainjnttojacsolver.hpp>
 #include <kdl/chainiksolvervel_pinv.hpp>
 #include <kdl/frames.hpp>
 #include <kdl/jntarray.hpp>
+#include <kdl/tree.hpp>
 
 #include <string>
 #include <unordered_map>
@@ -21,9 +24,13 @@ private:
     KDL::Chain kinematicChain;
     KDL::ChainFkSolverPos_recursive *fkSolver;
     KDL::ChainIkSolverVel_pinv *ikVelSolver;
-    KDL::ChainIkSolverPos_NR *ikSolverNR;
+    KDL::ChainIkSolverPos_NR_JL *ikSolverNR;
     KDL::ChainIkSolverPos_LMA *ikSolverLMA;
     bool m_initialized;
+    KDL::JntArray jointLowerLimits;
+    KDL::JntArray jointUpperLimits;
+    std::vector<std::string> activeJointNames;
+    bool m_hasJointLimits;
 
     // 记录 joint 名称到 child link UUID 的映射，便于前端回写对象姿态。
     std::unordered_map<std::string, std::string> jointChildLinkUuidMap;
@@ -34,6 +41,16 @@ private:
                            int minSteps, int maxSteps,
                            double positionResolution, double orientationResolution);
     bool parseJointChildLinkUuidsFromUrdf(const std::string &urdfString);
+    // 自动收集 KDL Tree 中没有子节点的末端 link 名称。
+    std::vector<std::string> collectLeafSegmentNames(const KDL::Tree &tree) const;
+    // 自动选择可用于正逆运动学的 6 轴串联链，避免固定依赖 base/tool0 命名。
+    bool selectKinematicChain(const KDL::Tree &tree, KDL::Chain &selectedChain) const;
+    // 从 URDF 中提取当前运动学链的关节上下限。
+    bool parseJointLimitsFromUrdf(const std::string &urdfString);
+    // 检查关节数组是否处于 URDF 定义的上下限范围内。
+    bool validateJointLimits(const double joints[6], const std::string &context) const;
+    // 检查关节向量是否处于 URDF 定义的上下限范围内。
+    bool validateJointLimits(const std::vector<double> &joints, const std::string &context) const;
 
 public:
     /**
@@ -177,6 +194,21 @@ public:
     bool calculateFK_AllJointsforwardKinematics(const double joints[6], double jointPoses[42]);
     bool forwardKinematics(const double joints[6], double jointPoses[42]);
 
+    /**
+     * @brief 检查当前关节姿态是否接近奇异点。
+     * @param joints_str 输入关节角，格式为 `j1,j2,j3,j4,j5,j6`。
+     * @param singularThreshold 最小奇异值低于该阈值时判定为奇异。
+     * @param warningThreshold 最小奇异值低于该阈值时判定为接近奇异。
+     * @param conditionThreshold 条件数超过该阈值时判定为奇异。
+     * @param conditionWarningThreshold 条件数超过该阈值时判定为接近奇异。
+     * @return 奇异点检测结果 JSON。
+     */
+    json checkSingularity(const std::string &joints_str,
+                          double singularThreshold = 1e-4,
+                          double warningThreshold = 1e-2,
+                          double conditionThreshold = 1e6,
+                          double conditionWarningThreshold = 1e4);
+
     /**
      * @brief 获取当前 KDL 运动学链。
      */

public/robot_kinematics_three_test.html

@@ -27,7 +27,7 @@
       html,
       body {
         width: 100%;
-        min-height: 100%;
+        height: 100%;
       }
 
       body {
@@ -39,6 +39,7 @@
           sans-serif;
         color: var(--text);
         background: var(--bg);
+        overflow: hidden;
       }
 
       button,
@@ -129,17 +130,24 @@
       .app {
         display: grid;
         grid-template-columns: minmax(336px, 420px) minmax(0, 1fr);
-        min-height: 100dvh;
+        height: 100dvh;
+        min-height: 0;
+        overflow: hidden;
       }
 
       .side {
         display: flex;
         flex-direction: column;
         gap: 16px;
+        min-height: 0;
+        height: 100dvh;
+        overflow-x: hidden;
+        overflow-y: auto;
         padding: 18px;
         border-right: 1px solid var(--line);
         background: var(--surface);
         box-shadow: var(--shadow);
+        overscroll-behavior: contain;
         z-index: 2;
       }
 
@@ -233,7 +241,8 @@
       .scene-wrap {
         position: relative;
         min-width: 0;
-        min-height: 100dvh;
+        min-height: 0;
+        height: 100dvh;
         overflow: hidden;
         background: #dfe7ec;
       }
@@ -242,7 +251,6 @@
         display: block;
         width: 100%;
         height: 100%;
-        min-height: 100dvh;
       }
 
       .scene-toolbar {
@@ -318,11 +326,21 @@
       }
 
       @media (max-width: 940px) {
+        body {
+          overflow: auto;
+        }
+
         .app {
           grid-template-columns: 1fr;
+          height: auto;
+          min-height: 100dvh;
+          overflow: visible;
         }
 
         .side {
+          height: auto;
+          max-height: none;
+          overflow: visible;
           border-right: 0;
           border-bottom: 1px solid var(--line);
           box-shadow: none;
@@ -330,7 +348,8 @@
 
         .scene-wrap,
         #scene {
-          min-height: 58dvh;
+          height: 58dvh;
+          min-height: 360px;
         }
       }
 
@@ -382,9 +401,16 @@
               <label for="robot-uuid">robot_uuid</label>
               <input id="robot-uuid" value="abb_irb120_3_58" />
             </div>
+            <div class="field full">
+              <label for="urdf-file">URDF 文件</label>
+              <input id="urdf-file" type="file" accept=".xml,.urdf" />
+            </div>
           </div>
           <div class="actions">
             <button id="init-btn" type="button">初始化</button>
+            <button id="register-urdf-btn" class="secondary" type="button">
+              注册 URDF
+            </button>
             <button id="reset-btn" class="secondary" type="button">零位</button>
           </div>
         </section>
@@ -394,6 +420,9 @@
           <div id="joint-controls"></div>
           <div class="actions">
             <button id="fk-btn" type="button">正解</button>
+            <button id="singularity-btn" class="secondary" type="button">
+              奇异点
+            </button>
             <button id="target-current-btn" class="secondary" type="button">
               取当前位姿
             </button>
@@ -466,6 +495,10 @@
             <span class="metric-label">关节数</span>
             <span class="metric-value" id="joint-count-value">0</span>
           </div>
+          <div class="metric">
+            <span class="metric-label">奇异风险</span>
+            <span class="metric-value" id="singularity-value">-</span>
+          </div>
           <div class="metric">
             <span class="metric-label">three.js</span>
             <span class="metric-value">0.184.0</span>
@@ -490,6 +523,8 @@
       import { OrbitControls } from "three/addons/controls/OrbitControls.js";
       import SmartWasmAPI from "./wasm/smart_api_wrapper.js";
 
+      THREE.Object3D.DEFAULT_UP.set(0, 0, 1);
+
       const api = new SmartWasmAPI();
       const jointCount = 6;
       const state = {
@@ -505,6 +540,7 @@
         wasmStatus: document.getElementById("wasm-status"),
         solverStatus: document.getElementById("solver-status"),
         robotUuid: document.getElementById("robot-uuid"),
+        urdfFile: document.getElementById("urdf-file"),
         jointControls: document.getElementById("joint-controls"),
         requestJson: document.getElementById("request-json"),
         responseJson: document.getElementById("response-json"),
@@ -517,10 +553,13 @@
         targetQw: document.getElementById("target-qw"),
         tcpValue: document.getElementById("tcp-value"),
         jointCountValue: document.getElementById("joint-count-value"),
+        singularityValue: document.getElementById("singularity-value"),
         toast: document.getElementById("toast"),
         initBtn: document.getElementById("init-btn"),
+        registerUrdfBtn: document.getElementById("register-urdf-btn"),
         resetBtn: document.getElementById("reset-btn"),
         fkBtn: document.getElementById("fk-btn"),
+        singularityBtn: document.getElementById("singularity-btn"),
         ikBtn: document.getElementById("ik-btn"),
         applyIkBtn: document.getElementById("apply-ik-btn"),
         targetCurrentBtn: document.getElementById("target-current-btn"),
@@ -530,7 +569,8 @@
       scene.background = new THREE.Color(0xdfe7ec);
 
       const camera = new THREE.PerspectiveCamera(45, 1, 0.01, 80);
-      camera.position.set(1.1, -1.75, 1.05);
+      camera.up.set(0, 0, 1);
+      camera.position.set(1.18, -1.68, 0.92);
 
       const renderer = new THREE.WebGLRenderer({
         canvas: elements.canvas,
@@ -542,7 +582,24 @@
 
       const controls = new OrbitControls(camera, renderer.domElement);
       controls.enableDamping = true;
+      controls.screenSpacePanning = false;
       controls.target.set(0.18, 0, 0.34);
+      controls.update();
+
+      window.__robotKinematicsDebug = {
+        // 获取当前相机状态，用于验证按钮操作不会改变视角。
+        getCameraState() {
+          return {
+            position: camera.position.toArray(),
+            up: camera.up.toArray(),
+            target: controls.target.toArray(),
+          };
+        },
+        // 获取 three.js 默认上方向，用于验证场景为 Z 轴向上。
+        getDefaultUp() {
+          return THREE.Object3D.DEFAULT_UP.toArray();
+        },
+      };
 
       const robotGroup = new THREE.Group();
       const targetGroup = new THREE.Group();
@@ -683,6 +740,20 @@
         };
       }
 
+      // 生成奇异点检测请求。
+      function buildSingularityRequest() {
+        return {
+          msg: "threejs singularity check",
+          req_code: `THREE_SINGULARITY_${Date.now()}`,
+          req_from: "threejs_test",
+          req_cmd: "Cmd_Kinematics_check_singularity",
+          req_param: {
+            robot_uuid: elements.robotUuid.value.trim(),
+            joints_str: buildJointString(),
+          },
+        };
+      }
+
       // 生成逆解请求。
       function buildInverseRequest() {
         const pose = [
@@ -736,6 +807,67 @@
         }
       }
 
+      // 读取本地 URDF 文件并注册为当前 robot_uuid。
+      async function registerUrdfFromFile() {
+        await ensureInitialized();
+        const file = elements.urdfFile.files?.[0];
+        const robotUuid = elements.robotUuid.value.trim();
+
+        if (!file) {
+          showToast("请选择 URDF 文件");
+          return;
+        }
+
+        if (!robotUuid) {
+          showToast("请先填写 robot_uuid");
+          return;
+        }
+
+        setBusy(true, "正在注册 URDF");
+        try {
+          const urdfText = await file.text();
+          const request = {
+            msg: "threejs init robot from urdf",
+            req_code: `THREE_INIT_URDF_${Date.now()}`,
+            req_from: "threejs_test",
+            req_cmd: "Cmd_InitRobot",
+            req_param: {
+              robot_uuid: robotUuid,
+              force_update: true,
+              urdf_base64: encodeUtf8Base64(urdfText),
+            },
+          };
+
+          renderJson(request, null);
+          const response = await api.processBusinessRequest(request);
+          renderJson(request, response);
+
+          if (response.success && response.res_data?.success) {
+            state.latestIkJoints = null;
+            elements.applyIkBtn.disabled = true;
+            setStatus("URDF 注册完成", true);
+            await runForwardKinematics();
+          } else {
+            setStatus("URDF 注册失败", false);
+            showToast(response.res_data?.message || response.msg || "URDF 注册失败");
+          }
+        } catch (error) {
+          showError(error);
+        } finally {
+          setBusy(false);
+        }
+      }
+
+      // 将 URDF 文本编码成接口需要的 UTF-8 Base64 字符串。
+      function encodeUtf8Base64(value) {
+        const bytes = new TextEncoder().encode(value);
+        let binary = "";
+        bytes.forEach((byte) => {
+          binary += String.fromCharCode(byte);
+        });
+        return btoa(binary);
+      }
+
       // 确保调用算法前已经完成初始化。
       async function ensureInitialized() {
         if (!state.initialized) {
@@ -774,6 +906,27 @@
         }
       }
 
+      // 执行奇异点检测并显示风险等级。
+      async function runSingularityCheck() {
+        await ensureInitialized();
+        setBusy(true, "奇异点检测中");
+
+        const request = buildSingularityRequest();
+        try {
+          renderJson(request, null);
+          const response = await api.processBusinessRequest(request);
+          renderJson(request, response);
+          updateSingularityMetric(response?.res_data);
+
+          const ok = response.success && response.res_data?.success;
+          setStatus(ok ? "奇异点检测完成" : "奇异点检测失败", Boolean(ok));
+        } catch (error) {
+          showError(error);
+        } finally {
+          setBusy(false);
+        }
+      }
+
       // 执行逆解并保存待应用的关节解。
       async function runInverseKinematics() {
         await ensureInitialized();
@@ -906,7 +1059,6 @@
             .join(", ");
         }
 
-        fitCamera(points);
       }
 
       // 创建两个关节点之间的连杆圆柱。
@@ -930,23 +1082,6 @@
         return mesh;
       }
 
-      // 根据当前骨架范围调整相机目标。
-      function fitCamera(points) {
-        if (!points.length) {
-          return;
-        }
-
-        const box = new THREE.Box3().setFromPoints(points);
-        const center = box.getCenter(new THREE.Vector3());
-        const size = box.getSize(new THREE.Vector3()).length();
-        controls.target.copy(center);
-        if (size > 0) {
-          camera.near = 0.01;
-          camera.far = Math.max(20, size * 18);
-          camera.updateProjectionMatrix();
-        }
-      }
-
       // 清空三维分组。
       function clearGroup(group) {
         while (group.children.length > 0) {
@@ -1002,7 +1137,6 @@
           targetMaterial,
         );
         marker.position.fromArray(targetPose.position);
-        marker.rotation.x = Math.PI / 2;
         targetGroup.add(marker);
 
         const axes = new THREE.AxesHelper(0.18);
@@ -1024,12 +1158,30 @@
         }
       }
 
+      // 更新奇异点风险指标。
+      function updateSingularityMetric(result) {
+        if (!result?.success) {
+          elements.singularityValue.textContent = "-";
+          return;
+        }
+
+        const riskText = {
+          normal: "正常",
+          warning: "接近",
+          singular: "奇异",
+        }[result.risk_level] ?? result.risk_level;
+        const minSv = Number(result.min_singular_value);
+        elements.singularityValue.textContent = `${riskText} / ${Number.isFinite(minSv) ? minSv.toExponential(2) : "-"}`;
+      }
+
       // 设置按钮忙碌状态。
       function setBusy(isBusy, label) {
         [
           elements.initBtn,
+          elements.registerUrdfBtn,
           elements.resetBtn,
           elements.fkBtn,
+          elements.singularityBtn,
           elements.ikBtn,
           elements.targetCurrentBtn,
         ].forEach((button) => {
@@ -1094,8 +1246,10 @@
       }
 
       elements.initBtn.addEventListener("click", initializeWasm);
+      elements.registerUrdfBtn.addEventListener("click", registerUrdfFromFile);
       elements.resetBtn.addEventListener("click", resetJoints);
       elements.fkBtn.addEventListener("click", runForwardKinematics);
+      elements.singularityBtn.addEventListener("click", runSingularityCheck);
       elements.ikBtn.addEventListener("click", runInverseKinematics);
       elements.applyIkBtn.addEventListener("click", applyInverseSolution);
       elements.targetCurrentBtn.addEventListener("click", updateTcpTargetFromCurrent);

scripts/build.ps1

@@ -133,6 +133,7 @@ $compileResult = emcc `
     -s USE_PTHREADS=0 `
     -O3 `
     -Wno-deprecated-literal-operator `
+    -Wno-deprecated-declarations `
     -o public/wasm/smart_math.js 2>&1
 
 $endTime = Get-Date

scripts/run_wasm_tests.js

@@ -142,6 +142,91 @@ function buildInverseRequest(command, robotUuid, poseStr, qInitStr, extra = {})
   };
 }
 
+function buildSingularityRequest(robotUuid, joints) {
+  return {
+    msg: "singularity check",
+    req_code: "AUTO_SINGULARITY",
+    req_from: "wasm_test",
+    req_cmd: "Cmd_Kinematics_check_singularity",
+    req_param: {
+      robot_uuid: robotUuid,
+      joints_str: jointArrayToString(joints),
+    },
+  };
+}
+
+// 构造 link 名称被整体替换的 URDF，用于验证初始化能自动推导运动学链。
+function buildRenamedUrdfFromDocs() {
+  const urdfPath = path.join(rootDir, "docs", "urdf.xml");
+  return fs
+    .readFileSync(urdfPath, "utf8")
+    .replaceAll("base_link-base", "renamed_root_link-renamed_world")
+    .replaceAll("base_link", "renamed_root_link")
+    .replaceAll("tool0", "renamed_tcp")
+    .replaceAll('link name="base"', 'link name="renamed_world"')
+    .replaceAll('parent link="base"', 'parent link="renamed_world"');
+}
+
+// 注册改名后的 URDF 并执行一次 FK，确认底层不再依赖固定 base/tool0。
+async function runRenamedUrdfChainCase(module) {
+  const robotUuid = "renamed_chain_robot";
+  const urdfBase64 = Buffer.from(buildRenamedUrdfFromDocs(), "utf8").toString("base64");
+
+  const initResponse = callBusinessApi(module, {
+    msg: "init renamed urdf",
+    req_code: "AUTO_RENAMED_INIT",
+    req_from: "wasm_test",
+    req_cmd: "Cmd_InitRobot",
+    req_param: {
+      robot_uuid: robotUuid,
+      urdf_base64: urdfBase64,
+      force_update: true,
+    },
+  });
+
+  ensure(initResponse.success === true, "renamed URDF init request failed");
+  ensure(getByPath(initResponse, "res_data.success") === true, "renamed URDF init result is not successful");
+
+  const forwardResponse = callBusinessApi(module, buildForwardRequest(robotUuid, [0, 0, 0, 0, 0, 0]));
+  ensure(forwardResponse.success === true, "renamed URDF forward request failed");
+  ensure(getByPath(forwardResponse, "res_data.success") === true, "renamed URDF forward result is not successful");
+  ensure(Array.isArray(getByPath(forwardResponse, "res_data.position")), "renamed URDF forward position is missing");
+
+  return {
+    details: {
+      position: getByPath(forwardResponse, "res_data.position"),
+      orientation: getByPath(forwardResponse, "res_data.orientation"),
+    },
+  };
+}
+
+// 发送超过 URDF 关节上下限的 FK 请求，确认底层会拒绝计算。
+async function runJointLimitCase(module) {
+  const response = callBusinessApi(module, buildForwardRequest("abb_irb120_3_58", [7, 0, 0, 0, 0, 0]));
+  ensure(response.success === false, "joint limit case should fail at response level");
+  ensure(String(getByPath(response, "res_data.error")).includes("Forward kinematics calculation failed"), "joint limit error is missing");
+  return response;
+}
+
+// 检查奇异点分析接口会返回完整的雅可比 SVD 指标。
+async function runSingularityCheckCase(module) {
+  const response = callBusinessApi(module, buildSingularityRequest("abb_irb120_3_58", [0.15, -0.25, 0.35, 0.1, -0.2, 0.3]));
+  ensure(response.success === true, "singularity check request failed");
+  ensure(getByPath(response, "res_data.success") === true, "singularity check result is not successful");
+  ensure(Array.isArray(getByPath(response, "res_data.singular_values")), "singular values are missing");
+  ensure(getByPath(response, "res_data.singular_values").length === 6, "singular value count should be 6");
+  ensure(typeof getByPath(response, "res_data.condition_number") === "number", "condition number is missing");
+  ensure(typeof getByPath(response, "res_data.risk_level") === "string", "risk level is missing");
+  return {
+    details: {
+      riskLevel: getByPath(response, "res_data.risk_level"),
+      rank: getByPath(response, "res_data.rank"),
+      minSingularValue: getByPath(response, "res_data.min_singular_value"),
+      conditionNumber: getByPath(response, "res_data.condition_number"),
+    },
+  };
+}
+
 function assertStaticCase(response, assertions) {
   for (const assertion of assertions) {
     const actual = getByPath(response, assertion.path);
@@ -353,6 +438,18 @@ const suite = [
       { path: "res_data.OPERATION.OPERATION.frames", lengthEquals: 2 },
     ],
   },
+  {
+    id: "kinematics_renamed_urdf_chain",
+    type: "renamed_urdf_chain",
+  },
+  {
+    id: "kinematics_joint_limit_rejects_fk",
+    type: "joint_limit",
+  },
+  {
+    id: "kinematics_singularity_check",
+    type: "singularity_check",
+  },
   {
     id: "spc_basic_5x30",
     type: "static",
@@ -522,6 +619,12 @@ async function runCase(module, testCase) {
       return runRoundtripSingleCase(module, testCase);
     case "roundtrip_path":
       return runRoundtripPathCase(module, testCase);
+    case "renamed_urdf_chain":
+      return runRenamedUrdfChainCase(module, testCase);
+    case "joint_limit":
+      return runJointLimitCase(module, testCase);
+    case "singularity_check":
+      return runSingularityCheckCase(module, testCase);
     default:
       throw new Error(`Unknown test type: ${testCase.type}`);
   }

src/QuadrupedRobotSimulation/KinematicsHelper.cpp

@@ -84,9 +84,6 @@ void KinematicsHelper::SimRobot()
 
 json KinematicsHelper::QuadrupedRobot_CalculateAllPointsFromMotorAngles(const std::string &jsonInput)
 {
-
-    std::cout << "[DEBUG] json KinematicsHelper::QuadrupedRobot_CalculateAllPointsFromMotorAngles(const std::string &jsonInput) " << std::endl;
-
     std::string jsonStr = "{}";
     auto manager = RobotGaitDataManagerFromJson(jsonInput);
     ReverseKinematicsCalculator simulation(manager);
@@ -96,9 +93,6 @@ json KinematicsHelper::QuadrupedRobot_CalculateAllPointsFromMotorAngles(const st
 }
 json KinematicsHelper::QuadrupedRobot_PerformForwardKinematics(const std::string &jsonInput)
 {
-
-    std::cout << "[DEBUG] json KinematicsHelper::QuadrupedRobot_PerformForwardKinematics(const std::string &jsonInput) " << std::endl;
-
     auto manager = RobotGaitDataManagerFromJson(jsonInput);
     QuadrupedRobotConfiguration config(manager->GetGaitInfo(), manager->GetSystemParameters());
     QuadrupedRobotSimulation simulation(config);

src/QuadrupedRobotSimulation/KinematicsReverse.cpp

@@ -972,8 +972,6 @@ std::string ReverseKinematicsCalculator::CalculateAllPointsFromMotorAnglesJsonSt
 
     if (paramDict.empty())
     {
-        std::cout << "[DEBUG] 没有Param数据 json KinematicsHelper::QuadrupedRobot_CalculateAllPointsFromMotorAngles(const std::string &jsonInput) " << std::endl;
-
         return "没有Param数据";
     }
     // debugPrintParamDict(paramDict);
@@ -991,14 +989,6 @@ std::string ReverseKinematicsCalculator::CalculateAllPointsFromMotorAnglesJsonSt
         auto points_dict = CalculateAllPointsFromMotorAnglesReverse(
             thigh_angle_deg, shank_angle_deg, ankle_angle_deg, leg_type);
 
-        // std::cout << "计算得到的点数量: " << points_dict.size() << "\n";
-        // std::cout << "包含的点: ";
-        for (const auto &p : points_dict)
-        {
-            std::cout << p.first << " ";
-        }
-        // std::cout << "\n";
-
         // 改为：
         auto modelID = _manager->GetModelIDObject(); // 使用新方法
 

src/QuadrupedRobotSimulation/KinematicsSimulation.cpp

@@ -886,11 +886,6 @@ void QuadrupedRobotSimulation::CalculateMotorData()
     int phase_RF = static_cast<int>(timeRF * n_frames);
     int phase_RH = static_cast<int>(timeRH * n_frames);
 
-    std::cout << "步态类型: " << gait_type << std::endl;
-    std::cout << "总帧数: " << n_frames << std::endl;
-    std::cout << "相位偏移（帧）: LF=0, LH=" << phase_LH
-              << ", RF=" << phase_RF << ", RH=" << phase_RH << std::endl;
-
     // 1. 计算左前腿基准数据
     motor_data_LF_raw = CalculateIncrementsAndVelocities();
 

src/Robot.cpp

@@ -3,6 +3,7 @@
 #include <stdexcept>
 #include <kdl_parser/kdl_parser.hpp>
 #include <kdl/tree.hpp>
+#include <urdf_parser/urdf_parser.h>
 
 // #include <cmath>
 #include <algorithm>
@@ -10,14 +11,17 @@
 #include <kdl/velocityprofile_trap.hpp>
 #include <kdl/trajectory_segment.hpp>
 #include <kdl/rotational_interpolation_sa.hpp>
+#include <Eigen/SVD>
+#include <limits>
 #include <regex>
+#include <vector>
 
 using namespace KDL;
 
 using namespace std;
 
 Robot::Robot() : m_initialized(false), fkSolver(nullptr), ikVelSolver(nullptr),
-                 ikSolverNR(nullptr), ikSolverLMA(nullptr)
+                 ikSolverNR(nullptr), ikSolverLMA(nullptr), m_hasJointLimits(false)
 {
     // 构造函数中显式初始化全部求解器指针。
 }
@@ -25,7 +29,8 @@ Robot::Robot(const std::string &urdfString) : m_initialized(false),
                                               fkSolver(nullptr),
                                               ikVelSolver(nullptr),
                                               ikSolverNR(nullptr),
-                                              ikSolverLMA(nullptr)
+                                              ikSolverLMA(nullptr),
+                                              m_hasJointLimits(false)
 {
     initRobot(urdfString);
 }
@@ -38,11 +43,203 @@ Robot::~Robot()
     delete ikSolverLMA;
 }
 
+// 自动收集 KDL Tree 中的叶子节点，用作自动推导运动学末端候选。
+std::vector<std::string> Robot::collectLeafSegmentNames(const KDL::Tree &tree) const
+{
+    std::vector<std::string> leafSegmentNames;
+    const auto &segments = tree.getSegments();
+
+    for (const auto &segmentPair : segments)
+    {
+        if (GetTreeElementChildren(segmentPair.second).empty())
+        {
+            leafSegmentNames.push_back(segmentPair.first);
+        }
+    }
+
+    return leafSegmentNames;
+}
+
+// 自动选择运动学链：优先兼容旧命名，失败后从 URDF 根节点推导 6 轴叶子链。
+bool Robot::selectKinematicChain(const KDL::Tree &tree, KDL::Chain &selectedChain) const
+{
+    KDL::Chain legacyChain;
+    if (tree.getChain("base", "tool0", legacyChain) && legacyChain.getNrOfJoints() == 6)
+    {
+        selectedChain = legacyChain;
+        return true;
+    }
+
+    const auto &segments = tree.getSegments();
+    auto rootSegment = tree.getRootSegment();
+    if (rootSegment == segments.end())
+    {
+        std::cerr << "Failed to infer kinematic chain: tree root segment not found" << std::endl;
+        return false;
+    }
+
+    const std::string rootName = rootSegment->first;
+    const std::vector<std::string> leafSegmentNames = collectLeafSegmentNames(tree);
+    bool foundChain = false;
+    std::string selectedTipName;
+    unsigned int selectedSegmentCount = 0;
+
+    for (const auto &tipName : leafSegmentNames)
+    {
+        KDL::Chain candidateChain;
+        if (!tree.getChain(rootName, tipName, candidateChain))
+        {
+            continue;
+        }
+
+        if (candidateChain.getNrOfJoints() != 6)
+        {
+            continue;
+        }
+
+        // 多个 6 轴叶子链并存时，优先选择段数最多的完整工具链。
+        if (!foundChain || candidateChain.getNrOfSegments() > selectedSegmentCount)
+        {
+            selectedChain = candidateChain;
+            selectedTipName = tipName;
+            selectedSegmentCount = candidateChain.getNrOfSegments();
+            foundChain = true;
+        }
+    }
+
+    if (!foundChain)
+    {
+        std::cerr << "Failed to infer a 6-joint kinematic chain from root segment: "
+                  << rootName << std::endl;
+        return false;
+    }
+
+    return true;
+}
+
+// 从 URDF 模型中读取当前活动关节的 lower/upper 限位。
+bool Robot::parseJointLimitsFromUrdf(const std::string &urdfString)
+{
+    activeJointNames.clear();
+    jointLowerLimits = KDL::JntArray(kinematicChain.getNrOfJoints());
+    jointUpperLimits = KDL::JntArray(kinematicChain.getNrOfJoints());
+    m_hasJointLimits = false;
+
+    urdf::ModelInterfaceSharedPtr robotModel = urdf::parseURDF(urdfString);
+    if (!robotModel)
+    {
+        std::cerr << "Failed to parse URDF model for joint limits" << std::endl;
+        return false;
+    }
+
+    unsigned int jointIndex = 0;
+    for (unsigned int segmentIndex = 0; segmentIndex < kinematicChain.getNrOfSegments(); segmentIndex++)
+    {
+        const KDL::Joint &joint = kinematicChain.getSegment(segmentIndex).getJoint();
+        if (joint.getType() == KDL::Joint::Fixed)
+        {
+            continue;
+        }
+
+        const std::string &jointName = joint.getName();
+        urdf::JointConstSharedPtr urdfJoint = robotModel->getJoint(jointName);
+        if (!urdfJoint)
+        {
+            std::cerr << "Joint limit missing: joint not found in URDF: " << jointName << std::endl;
+            return false;
+        }
+
+        if (urdfJoint->type == urdf::Joint::CONTINUOUS)
+        {
+            jointLowerLimits(jointIndex) = -std::numeric_limits<double>::infinity();
+            jointUpperLimits(jointIndex) = std::numeric_limits<double>::infinity();
+        }
+        else if (urdfJoint->limits)
+        {
+            jointLowerLimits(jointIndex) = urdfJoint->limits->lower;
+            jointUpperLimits(jointIndex) = urdfJoint->limits->upper;
+        }
+        else
+        {
+            std::cerr << "Joint limit missing: <limit> not found for joint: " << jointName << std::endl;
+            return false;
+        }
+
+        if (jointLowerLimits(jointIndex) > jointUpperLimits(jointIndex))
+        {
+            std::cerr << "Joint limit invalid for joint " << jointName
+                      << ": lower is greater than upper" << std::endl;
+            return false;
+        }
+
+        activeJointNames.push_back(jointName);
+        jointIndex++;
+    }
+
+    if (jointIndex != kinematicChain.getNrOfJoints())
+    {
+        std::cerr << "Joint limit count mismatch, expected "
+                  << kinematicChain.getNrOfJoints() << ", got " << jointIndex << std::endl;
+        return false;
+    }
+
+    m_hasJointLimits = true;
+    return true;
+}
+
+// 校验固定数组关节值是否处于 URDF 上下限内。
+bool Robot::validateJointLimits(const double joints[6], const std::string &context) const
+{
+    if (!m_hasJointLimits)
+    {
+        return true;
+    }
+
+    constexpr double tolerance = 1e-9;
+    for (unsigned int i = 0; i < jointLowerLimits.rows(); i++)
+    {
+        const double value = joints[i];
+        const double lower = jointLowerLimits(i);
+        const double upper = jointUpperLimits(i);
+
+        if (value < lower - tolerance || value > upper + tolerance)
+        {
+            return false;
+        }
+    }
+
+    return true;
+}
+
+// 校验 vector 关节值是否处于 URDF 上下限内。
+bool Robot::validateJointLimits(const std::vector<double> &joints, const std::string &context) const
+{
+    if (joints.size() != 6)
+    {
+        return false;
+    }
+
+    return validateJointLimits(joints.data(), context);
+}
+
 // 根据 URDF 初始化机器人，并提取关节与 child link 的 UUID 映射。
 bool Robot::initRobot(const std::string &urdfString)
 {
     try
     {
+        delete fkSolver;
+        delete ikVelSolver;
+        delete ikSolverNR;
+        delete ikSolverLMA;
+        fkSolver = nullptr;
+        ikVelSolver = nullptr;
+        ikSolverNR = nullptr;
+        ikSolverLMA = nullptr;
+        m_initialized = false;
+        kinematicChain = KDL::Chain();
+        activeJointNames.clear();
+        m_hasJointLimits = false;
+
         KDL::Tree tree;
 
         // 从 URDF 字符串解析 KDL Tree。
@@ -52,10 +249,9 @@ bool Robot::initRobot(const std::string &urdfString)
             return false;
         }
 
-        // 提取 base 到 tool0 的运动学链。
+        // 自动选择可用运动学链，避免固定依赖 base/tool0 命名。
-        if (!tree.getChain("base", "tool0", kinematicChain))
+        if (!selectKinematicChain(tree, kinematicChain))
         {
-            std::cerr << "Failed to get chain from base to tool0" << std::endl;
             return false;
         }
 
@@ -67,6 +263,12 @@ bool Robot::initRobot(const std::string &urdfString)
             return false;
         }
 
+        // 从 URDF 提取关节上下限，供 FK 输入和 IK 输出统一校验。
+        if (!parseJointLimitsFromUrdf(urdfString))
+        {
+            return false;
+        }
+
         // 解析 joint 到 child link UUID 的映射。
         if (!parseJointChildLinkUuidsFromUrdf(urdfString))
         {
@@ -76,12 +278,10 @@ bool Robot::initRobot(const std::string &urdfString)
         // 初始化 FK / IK 求解器。
         fkSolver = new KDL::ChainFkSolverPos_recursive(kinematicChain);
         ikVelSolver = new KDL::ChainIkSolverVel_pinv(kinematicChain);
-        ikSolverNR = new KDL::ChainIkSolverPos_NR(kinematicChain, *fkSolver, *ikVelSolver, 100, 1e-6);
+        ikSolverNR = new KDL::ChainIkSolverPos_NR_JL(kinematicChain, jointLowerLimits, jointUpperLimits, *fkSolver, *ikVelSolver, 100, 1e-6);
 
         // LMA 求解器按调用时动态构造，便于传入不同收敛参数。
         m_initialized = true;
-        std::cout << "Robot initialized successfully with "
-                  << kinematicChain.getNrOfJoints() << " joints" << std::endl;
         numberOfJoints = getNumberOfJoints();
         return true;
     }
@@ -197,9 +397,6 @@ bool Robot::parseJointChildLinkUuidsFromUrdf(const std::string &urdfString)
         std::smatch matches;
         std::string::const_iterator searchStart(urdfString.cbegin());
 
-        // 输出解析过程，便于排查 URDF 结构问题。
-        std::cout << "Parsing URDF for joint child link UUIDs..." << std::endl;
-
         bool foundAny = false;
         while (std::regex_search(searchStart, urdfString.cend(), matches, jointRegex))
         {
@@ -210,10 +407,6 @@ bool Robot::parseJointChildLinkUuidsFromUrdf(const std::string &urdfString)
                 std::string uuid = matches[3].str();
                 jointChildLinkUuidMap[jointName] = uuid;
 
-                // 输出匹配结果。
-                std::cout << "Found joint: " << jointName
-                          << " -> Child link: " << childLinkName
-                          << " -> UUID: " << uuid << std::endl;
                 foundAny = true;
             }
             searchStart = matches.suffix().first;
@@ -222,8 +415,6 @@ bool Robot::parseJointChildLinkUuidsFromUrdf(const std::string &urdfString)
         // 如果首轮没有命中，则退化为更宽松的解析方式。
         if (!foundAny)
         {
-            std::cout << "Trying alternative parsing method..." << std::endl;
-
             // 方案二：分别匹配 joint 名称和 child link。
             std::regex jointNameRegex(R"(<joint\s+name=\"([^\"]+)\")");
             std::regex childLinkRegex(R"(<child\s+link=\"([^\"]+)\"\s+uuid=\"([^\"]+)\")");
@@ -259,9 +450,6 @@ bool Robot::parseJointChildLinkUuidsFromUrdf(const std::string &urdfString)
                                 std::string uuid = childMatches[2].str();
                                 jointChildLinkUuidMap[jointName] = uuid;
 
-                                std::cout << "Found joint: " << jointName
-                                          << " -> Child link: " << childLinkName
-                                          << " -> UUID: " << uuid << std::endl;
                                 foundAny = true;
                             }
                         }
@@ -277,15 +465,9 @@ bool Robot::parseJointChildLinkUuidsFromUrdf(const std::string &urdfString)
         {
             std::cerr << "No joint child link UUIDs found in URDF" << std::endl;
 
-            // 仅打印前 500 个字符，避免日志过长。
-            std::cout << "First 500 characters of URDF:" << std::endl;
-            std::cout << urdfString.substr(0, 500) << std::endl;
-
             return false;
         }
 
-        std::cout << "Successfully parsed " << jointChildLinkUuidMap.size()
-                  << " joint child link UUIDs" << std::endl;
         return true;
     }
     catch (const std::exception &e)
@@ -315,6 +497,11 @@ bool Robot::calculateIK_NR(const double pose[7], const double iniJ[6], double re
 
     try
     {
+        if (!validateJointLimits(iniJ, "IK initial joints"))
+        {
+            return false;
+        }
+
         // 将输入位姿转换为 KDL Frame。
         KDL::Vector position(pose[0], pose[1], pose[2]);
         KDL::Rotation rotation = KDL::Rotation::Quaternion(pose[3], pose[4], pose[5], pose[6]);
@@ -338,7 +525,7 @@ bool Robot::calculateIK_NR(const double pose[7], const double iniJ[6], double re
             {
                 resultJoints[i] = result(i);
             }
-            return true;
+            return validateJointLimits(resultJoints, "IK result joints");
         }
         else
         {
@@ -364,6 +551,11 @@ bool Robot::calculateIK_LMA(const double pose[7], const double iniJ[6], double r
 
     try
     {
+        if (!validateJointLimits(iniJ, "IK initial joints"))
+        {
+            return false;
+        }
+
         // 每次调用时动态构造 LMA 求解器，便于传入不同参数。
         KDL::ChainIkSolverPos_LMA ikSolverLMA(kinematicChain, eps, maxiter, eps_joints);
 
@@ -390,7 +582,7 @@ bool Robot::calculateIK_LMA(const double pose[7], const double iniJ[6], double r
             {
                 resultJoints[i] = result(i);
             }
-            return true;
+            return validateJointLimits(resultJoints, "IK result joints");
         }
         else
         {
@@ -415,6 +607,11 @@ bool Robot::calculateFK_TCP(const double joints[6], double tcpPose[7])
 
     try
     {
+        if (!validateJointLimits(joints, "FK input joints"))
+        {
+            return false;
+        }
+
         // 将关节数组转换为 KDL 关节对象。
         KDL::JntArray jointArray(6);
         for (int i = 0; i < 6; i++)
@@ -460,6 +657,11 @@ bool Robot::calculateFK_TCP(const double joints[6], double tcpPose[7])
 // const double joints[6], double jointPoses[42]
 bool Robot::forwardKinematics(const double inJoint[6], double outFrame[42])
 {
+    if (!validateJointLimits(inJoint, "FK all joints input"))
+    {
+        return false;
+    }
+
     Frame F_result;
 
     ChainFkSolverPos_recursive fkSolver(kinematicChain);
@@ -513,6 +715,11 @@ bool Robot::calculateFK_AllJointsforwardKinematics(const double joints[6], doubl
 
     try
     {
+        if (!validateJointLimits(joints, "FK all joints input"))
+        {
+            return false;
+        }
+
         // 将关节数组转换为 KDL 关节对象。
         KDL::JntArray jointArray(6);
         for (int i = 0; i < 6; i++)
@@ -561,6 +768,122 @@ bool Robot::calculateFK_AllJointsforwardKinematics(const double joints[6], doubl
     }
 }
 
+// 基于 TCP 雅可比矩阵的奇异值分析，判断当前姿态是否接近奇异点。
+json Robot::checkSingularity(const std::string &joints_str,
+                             double singularThreshold,
+                             double warningThreshold,
+                             double conditionThreshold,
+                             double conditionWarningThreshold)
+{
+    try
+    {
+        if (!m_initialized)
+        {
+            return json{{"success", false}, {"error", "Robot not initialized"}};
+        }
+
+        std::vector<double> joints = parseJointString(joints_str);
+        if (joints.size() != 6)
+        {
+            return json{{"success", false}, {"error", "Invalid joints format"}};
+        }
+
+        if (!validateJointLimits(joints, "Singularity check input joints"))
+        {
+            return json{{"success", false}, {"error", "Joint value out of limits"}};
+        }
+
+        KDL::JntArray jointArray(6);
+        for (int i = 0; i < 6; i++)
+        {
+            jointArray(i) = joints[i];
+        }
+
+        KDL::Jacobian jacobian(kinematicChain.getNrOfJoints());
+        KDL::ChainJntToJacSolver jacSolver(kinematicChain);
+        const int status = jacSolver.JntToJac(jointArray, jacobian);
+        if (status < 0)
+        {
+            return json{{"success", false}, {"error", "Jacobian calculation failed"}, {"status", status}};
+        }
+
+        Eigen::JacobiSVD<Eigen::MatrixXd> svd(jacobian.data, Eigen::ComputeThinU | Eigen::ComputeThinV);
+        const auto singularValues = svd.singularValues();
+
+        json singularValuesJson = json::array();
+        double minSingularValue = std::numeric_limits<double>::infinity();
+        double maxSingularValue = 0.0;
+        double manipulability = 1.0;
+        int rank = 0;
+
+        for (int i = 0; i < singularValues.size(); i++)
+        {
+            const double value = singularValues(i);
+            singularValuesJson.push_back(value);
+            minSingularValue = std::min(minSingularValue, value);
+            maxSingularValue = std::max(maxSingularValue, value);
+            manipulability *= value;
+            if (value > singularThreshold)
+            {
+                rank++;
+            }
+        }
+
+        if (!std::isfinite(minSingularValue))
+        {
+            minSingularValue = 0.0;
+        }
+
+        const double conditionNumber = minSingularValue <= 0.0
+                                           ? std::numeric_limits<double>::infinity()
+                                           : maxSingularValue / minSingularValue;
+        const bool isSingular = minSingularValue <= singularThreshold ||
+                                conditionNumber >= conditionThreshold ||
+                                rank < static_cast<int>(kinematicChain.getNrOfJoints());
+        const bool isNearSingular = !isSingular &&
+                                    (minSingularValue <= warningThreshold ||
+                                     conditionNumber >= conditionWarningThreshold);
+        const std::string riskLevel = isSingular ? "singular" : (isNearSingular ? "warning" : "normal");
+
+        json jacobianJson = json::array();
+        for (int row = 0; row < jacobian.data.rows(); row++)
+        {
+            json rowJson = json::array();
+            for (int col = 0; col < jacobian.data.cols(); col++)
+            {
+                rowJson.push_back(jacobian.data(row, col));
+            }
+            jacobianJson.push_back(rowJson);
+        }
+
+        return {
+            {"success", true},
+            {"is_singular", isSingular},
+            {"is_near_singular", isNearSingular},
+            {"risk_level", riskLevel},
+            {"rank", rank},
+            {"joint_count", kinematicChain.getNrOfJoints()},
+            {"min_singular_value", minSingularValue},
+            {"max_singular_value", maxSingularValue},
+            {"condition_number", conditionNumber},
+            {"manipulability", manipulability},
+            {"singular_values", singularValuesJson},
+            {"thresholds", {
+                               {"singular", singularThreshold},
+                               {"warning", warningThreshold},
+                               {"condition", conditionThreshold},
+                               {"condition_warning", conditionWarningThreshold},
+                           }},
+            {"joints", joints},
+            {"jacobian", jacobianJson},
+        };
+    }
+    catch (const std::exception &e)
+    {
+        return json{{"success", false}, {"error", "Failed to check singularity: " + std::string(e.what())}};
+    }
+}
+
 /**
  * @brief 计算四元数之间的夹角差，用于轨迹步数估算。
  */

src/api/KinematicsWebAPI.Core.cpp

@@ -15,6 +15,7 @@ bool isRobotCommand(const std::string &req_cmd)
            req_cmd == "Cmd_Kinematics_inverse_pose_str_NoDifference" ||
            req_cmd == "Cmd_Kinematics_forward_pose_str" ||
            req_cmd == "Cmd_Kinematics_forward_all_joints" ||
+           req_cmd == "Cmd_Kinematics_check_singularity" ||
            req_cmd == "Cmd_InitRobot" ||
            req_cmd == "Cmd_GetRobot" ||
            req_cmd == "Cmd_RemoveRobot" ||
@@ -149,8 +150,6 @@ json KinematicsWebAPI::createUnknownCommandResponse(const std::string &req_cmd,
 
 void KinematicsWebAPI::log(const std::string &message)
 {
-    std::cout << "[" << getCurrentTimestamp() << "] " << message << std::endl;
-
     if (onLog)
     {
         onLog("[" + getCurrentTimestamp() + "] " + message);

src/api/KinematicsWebAPI.QuadrupedCommands.cpp

@@ -7,13 +7,11 @@ json KinematicsWebAPI::handleQuadrupedCommand(const std::string &req_cmd, const
 {
     if (req_cmd == "Cmd_QuadrupedRobot_CalculateAllPointsFromMotorAngles")
     {
-        std::cout << "[DEBUG] Cmd_QuadrupedRobot_CalculateAllPointsFromMotorAngles: 开始计算点位" << std::endl;
         return KinematicsHelper::QuadrupedRobot_CalculateAllPointsFromMotorAngles(req_param.dump());
     }
 
     if (req_cmd == "Cmd_QuadrupedRobot_PerformForwardKinematics")
     {
-        std::cout << "[DEBUG] Cmd_QuadrupedRobot_PerformForwardKinematics: 开始执行正运动学" << std::endl;
         return KinematicsHelper::QuadrupedRobot_PerformForwardKinematics(req_param.dump());
     }
 

src/api/KinematicsWebAPI.RobotCommands.cpp

@@ -175,6 +175,39 @@ json KinematicsWebAPI::handleRobotCommand(const std::string &req_cmd, const json
             res_data = {{"error", "Failed to calculate forward kinematics for all joints: " + std::string(e.what())}};
         }
     }
+    else if (req_cmd == "Cmd_Kinematics_check_singularity")
+    {
+        try
+        {
+            log("Handling Cmd_Kinematics_check_singularity command");
+
+            std::string joints_str = req_param.value("joints_str", req_param.value("q_init_str", "0,0,0,0,0,0"));
+            std::string robot_uuid = req_param.value("robot_uuid", "default");
+            double singular_threshold = req_param.value("singular_threshold", 1e-4);
+            double warning_threshold = req_param.value("warning_threshold", 1e-2);
+            double condition_threshold = req_param.value("condition_threshold", 1e6);
+            double condition_warning_threshold = req_param.value("condition_warning_threshold", 1e4);
+
+            auto robot = RobotManager::getRobot(robot_uuid);
+            if (!robot || !robot->isInitialized())
+            {
+                res_data = {{"error", "Robot not found or not initialized"}};
+            }
+            else
+            {
+                res_data = robot->checkSingularity(
+                    joints_str,
+                    singular_threshold,
+                    warning_threshold,
+                    condition_threshold,
+                    condition_warning_threshold);
+            }
+        }
+        catch (const std::exception &e)
+        {
+            res_data = {{"error", "Failed to check singularity: " + std::string(e.what())}};
+        }
+    }
     else if (req_cmd == "Cmd_InitRobot")
     {
         try