启用机器人关节上下限检查

include/Robot.h

@@ -5,6 +5,7 @@
 #include <kdl/chainfksolverpos_recursive.hpp>
 #include <kdl/chainiksolverpos_lma.hpp>
 #include <kdl/chainiksolverpos_nr.hpp>
+#include <kdl/chainiksolverpos_nr_jl.hpp>
 #include <kdl/chainiksolvervel_pinv.hpp>
 #include <kdl/frames.hpp>
 #include <kdl/jntarray.hpp>
@@ -22,9 +23,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;
@@ -39,6 +44,12 @@ private:
     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:
     /**