增加机器人奇异点检测

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