From e26b04e915f5ce58a88b266839c4acb433fdc06c Mon Sep 17 00:00:00 2001 From: zhangshun <453905631@qq.com> Date: Tue, 16 Jun 2026 12:41:36 +0800 Subject: [PATCH] =?UTF-8?q?=E5=90=AF=E7=94=A8=E6=9C=BA=E5=99=A8=E4=BA=BA?= =?UTF-8?q?=E5=85=B3=E8=8A=82=E4=B8=8A=E4=B8=8B=E9=99=90=E6=A3=80=E6=9F=A5?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- include/Robot.h | 13 +++- scripts/run_wasm_tests.js | 14 ++++ src/Robot.cpp | 156 ++++++++++++++++++++++++++++++++++++-- 3 files changed, 177 insertions(+), 6 deletions(-) diff --git a/include/Robot.h b/include/Robot.h index f717cbb..fe60379 100644 --- a/include/Robot.h +++ b/include/Robot.h @@ -5,6 +5,7 @@ #include #include #include +#include #include #include #include @@ -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 activeJointNames; + bool m_hasJointLimits; // 记录 joint 名称到 child link UUID 的映射，便于前端回写对象姿态。 std::unordered_map jointChildLinkUuidMap; @@ -39,6 +44,12 @@ private: std::vector 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 &joints, const std::string &context) const; public: /** diff --git a/scripts/run_wasm_tests.js b/scripts/run_wasm_tests.js index 9f07344..a6436fa 100644 --- a/scripts/run_wasm_tests.js +++ b/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}`); } diff --git a/src/Robot.cpp b/src/Robot.cpp index 50767a6..8f2804d 100644 --- a/src/Robot.cpp +++ b/src/Robot.cpp @@ -3,6 +3,7 @@ #include #include #include +#include // #include #include @@ -10,6 +11,7 @@ #include #include #include +#include #include #include @@ -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::infinity(); + jointUpperLimits(jointIndex) = std::numeric_limits::infinity(); + } + else if (urdfJoint->limits) + { + jointLowerLimits(jointIndex) = urdfJoint->limits->lower; + jointUpperLimits(jointIndex) = urdfJoint->limits->upper; + } + else + { + std::cerr << "Joint limit missing: 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 &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++)