增加机器人奇异点检测

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 运动学链。
      */