增加机器人奇异点检测

docs/robot_kinematics_capabilities.md

@@ -160,15 +160,60 @@
 
 ### 奇异点检测
 
-当前没有显式计算雅可比矩阵条件数或可操作度。
+模块已支持基于 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 约束
 
@@ -259,12 +304,11 @@
 
 如果继续按工业机器人实际使用场景增强，建议优先级如下：
 
-1. 增加奇异点检测和雅可比分析。
+1. 增加多 IK 解和构型选择。
-2. 增加多 IK 解和构型选择。
+2. 增加关节连续性和最短路径处理。
-3. 增加关节连续性和最短路径处理。
+3. 增加速度、加速度和轨迹时间参数化。
-4. 增加速度、加速度和轨迹时间参数化。
+4. 增加工具坐标系和工件坐标系管理。
-5. 增加工具坐标系和工件坐标系管理。
+5. 增加 PTP、LIN、CIRC 等工业运动指令。
-6. 增加 PTP、LIN、CIRC 等工业运动指令。
+6. 增加碰撞检测。
-7. 增加碰撞检测。
+7. 增加奇异类型分类，例如腕部、肩部、肘部奇异。
 8. 增加控制器通讯和实时执行相关能力。
-

include/Robot.h

@@ -6,6 +6,7 @@
 #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>
@@ -193,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

@@ -420,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>
@@ -492,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>
@@ -546,11 +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"),
@@ -731,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 = [
@@ -883,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();
@@ -1114,6 +1158,22 @@
         }
       }
 
+      // 更新奇异点风险指标。
+      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) {
         [
@@ -1121,6 +1181,7 @@
           elements.registerUrdfBtn,
           elements.resetBtn,
           elements.fkBtn,
+          elements.singularityBtn,
           elements.ikBtn,
           elements.targetCurrentBtn,
         ].forEach((button) => {
@@ -1188,6 +1249,7 @@
       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,19 @@ 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");
@@ -195,6 +208,25 @@ async function runJointLimitCase(module) {
   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);
@@ -414,6 +446,10 @@ const suite = [
     id: "kinematics_joint_limit_rejects_fk",
     type: "joint_limit",
   },
+  {
+    id: "kinematics_singularity_check",
+    type: "singularity_check",
+  },
   {
     id: "spc_basic_5x30",
     type: "static",
@@ -587,6 +623,8 @@ async function runCase(module, testCase) {
       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/Robot.cpp

@@ -11,6 +11,7 @@
 #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>
@@ -797,6 +798,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" ||

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