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

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:
     /**

scripts/run_wasm_tests.js

@@ -187,6 +187,14 @@ async function runRenamedUrdfChainCase(module) {
   };
 }
 
+// 发送超过 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;
+}
+
 function assertStaticCase(response, assertions) {
   for (const assertion of assertions) {
     const actual = getByPath(response, assertion.path);
@@ -402,6 +410,10 @@ const suite = [
     id: "kinematics_renamed_urdf_chain",
     type: "renamed_urdf_chain",
   },
+  {
+    id: "kinematics_joint_limit_rejects_fk",
+    type: "joint_limit",
+  },
   {
     id: "spc_basic_5x30",
     type: "static",
@@ -573,6 +585,8 @@ async function runCase(module, testCase) {
       return runRoundtripPathCase(module, testCase);
     case "renamed_urdf_chain":
       return runRenamedUrdfChainCase(module, testCase);
+    case "joint_limit":
+      return runJointLimitCase(module, testCase);
     default:
       throw new Error(`Unknown test type: ${testCase.type}`);
   }

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,6 +11,7 @@
 #include <kdl/velocityprofile_trap.hpp>
 #include <kdl/trajectory_segment.hpp>
 #include <kdl/rotational_interpolation_sa.hpp>
+#include <limits>
 #include <regex>
 #include <vector>
 
@@ -18,7 +20,7 @@ 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)
 {
     // 构造函数中显式初始化全部求解器指针。
 }
@@ -26,7 +28,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);
 }
@@ -118,6 +121,116 @@ bool Robot::selectKinematicChain(const KDL::Tree &tree, KDL::Chain &selectedChai
     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)
+        {
+            const std::string jointName = i < activeJointNames.size() ? activeJointNames[i] : ("joint_" + std::to_string(i + 1));
+            std::cerr << context << " joint limit violation: " << jointName
+                      << " value=" << value
+                      << ", range=[" << lower << ", " << upper << "]" << std::endl;
+            return false;
+        }
+    }
+
+    return true;
+}
+
+// 校验 vector 关节值是否处于 URDF 上下限内。
+bool Robot::validateJointLimits(const std::vector<double> &joints, const std::string &context) const
+{
+    if (joints.size() != 6)
+    {
+        std::cerr << context << " joint count invalid: " << joints.size() << std::endl;
+        return false;
+    }
+
+    return validateJointLimits(joints.data(), context);
+}
+
 // 根据 URDF 初始化机器人，并提取关节与 child link 的 UUID 映射。
 bool Robot::initRobot(const std::string &urdfString)
 {
@@ -133,6 +246,8 @@ bool Robot::initRobot(const std::string &urdfString)
         ikSolverLMA = nullptr;
         m_initialized = false;
         kinematicChain = KDL::Chain();
+        activeJointNames.clear();
+        m_hasJointLimits = false;
 
         KDL::Tree tree;
 
@@ -157,6 +272,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))
         {
@@ -166,7 +287,7 @@ 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;
@@ -405,6 +526,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]);
@@ -428,7 +554,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
         {
@@ -454,6 +580,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);
 
@@ -480,7 +611,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
         {
@@ -505,6 +636,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++)
@@ -550,6 +686,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);
@@ -603,6 +744,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++)