meswork/smart_wasm
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
zhangshun
2026-06-16 12:41:36 +08:00
parent c2d79a74b9
commit e26b04e915
3 changed files with 177 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
include/Robot.h
View File

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

14
scripts/run_wasm_tests.js
View File

@@ -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}`);
}

156
src/Robot.cpp
View File

@@ -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++)