diff --git a/include/Robot.h b/include/Robot.h index e94ccde..f717cbb 100644 --- a/include/Robot.h +++ b/include/Robot.h @@ -8,6 +8,7 @@ #include #include #include +#include #include #include @@ -34,6 +35,10 @@ private: int minSteps, int maxSteps, double positionResolution, double orientationResolution); bool parseJointChildLinkUuidsFromUrdf(const std::string &urdfString); + // 自动收集 KDL Tree 中没有子节点的末端 link 名称。 + std::vector collectLeafSegmentNames(const KDL::Tree &tree) const; + // 自动选择可用于正逆运动学的 6 轴串联链，避免固定依赖 base/tool0 命名。 + bool selectKinematicChain(const KDL::Tree &tree, KDL::Chain &selectedChain) const; public: /** diff --git a/scripts/run_wasm_tests.js b/scripts/run_wasm_tests.js index 7bf7db5..9f07344 100644 --- a/scripts/run_wasm_tests.js +++ b/scripts/run_wasm_tests.js @@ -142,6 +142,51 @@ function buildInverseRequest(command, robotUuid, poseStr, qInitStr, extra = {}) }; } +// 构造 link 名称被整体替换的 URDF，用于验证初始化能自动推导运动学链。 +function buildRenamedUrdfFromDocs() { + const urdfPath = path.join(rootDir, "docs", "urdf.xml"); + return fs + .readFileSync(urdfPath, "utf8") + .replaceAll("base_link-base", "renamed_root_link-renamed_world") + .replaceAll("base_link", "renamed_root_link") + .replaceAll("tool0", "renamed_tcp") + .replaceAll('link name="base"', 'link name="renamed_world"') + .replaceAll('parent link="base"', 'parent link="renamed_world"'); +} + +// 注册改名后的 URDF 并执行一次 FK，确认底层不再依赖固定 base/tool0。 +async function runRenamedUrdfChainCase(module) { + const robotUuid = "renamed_chain_robot"; + const urdfBase64 = Buffer.from(buildRenamedUrdfFromDocs(), "utf8").toString("base64"); + + const initResponse = callBusinessApi(module, { + msg: "init renamed urdf", + req_code: "AUTO_RENAMED_INIT", + req_from: "wasm_test", + req_cmd: "Cmd_InitRobot", + req_param: { + robot_uuid: robotUuid, + urdf_base64: urdfBase64, + force_update: true, + }, + }); + + ensure(initResponse.success === true, "renamed URDF init request failed"); + ensure(getByPath(initResponse, "res_data.success") === true, "renamed URDF init result is not successful"); + + const forwardResponse = callBusinessApi(module, buildForwardRequest(robotUuid, [0, 0, 0, 0, 0, 0])); + ensure(forwardResponse.success === true, "renamed URDF forward request failed"); + ensure(getByPath(forwardResponse, "res_data.success") === true, "renamed URDF forward result is not successful"); + ensure(Array.isArray(getByPath(forwardResponse, "res_data.position")), "renamed URDF forward position is missing"); + + return { + details: { + position: getByPath(forwardResponse, "res_data.position"), + orientation: getByPath(forwardResponse, "res_data.orientation"), + }, + }; +} + function assertStaticCase(response, assertions) { for (const assertion of assertions) { const actual = getByPath(response, assertion.path); @@ -353,6 +398,10 @@ const suite = [ { path: "res_data.OPERATION.OPERATION.frames", lengthEquals: 2 }, ], }, + { + id: "kinematics_renamed_urdf_chain", + type: "renamed_urdf_chain", + }, { id: "spc_basic_5x30", type: "static", @@ -522,6 +571,8 @@ async function runCase(module, testCase) { return runRoundtripSingleCase(module, testCase); case "roundtrip_path": return runRoundtripPathCase(module, testCase); + case "renamed_urdf_chain": + return runRenamedUrdfChainCase(module, testCase); default: throw new Error(`Unknown test type: ${testCase.type}`); } diff --git a/src/Robot.cpp b/src/Robot.cpp index f9aa2cf..50767a6 100644 --- a/src/Robot.cpp +++ b/src/Robot.cpp @@ -11,6 +11,7 @@ #include #include #include +#include using namespace KDL; @@ -38,11 +39,101 @@ Robot::~Robot() delete ikSolverLMA; } +// 自动收集 KDL Tree 中的叶子节点，用作自动推导运动学末端候选。 +std::vector Robot::collectLeafSegmentNames(const KDL::Tree &tree) const +{ + std::vector leafSegmentNames; + const auto &segments = tree.getSegments(); + + for (const auto &segmentPair : segments) + { + if (GetTreeElementChildren(segmentPair.second).empty()) + { + leafSegmentNames.push_back(segmentPair.first); + } + } + + return leafSegmentNames; +} + +// 自动选择运动学链：优先兼容旧命名，失败后从 URDF 根节点推导 6 轴叶子链。 +bool Robot::selectKinematicChain(const KDL::Tree &tree, KDL::Chain &selectedChain) const +{ + KDL::Chain legacyChain; + if (tree.getChain("base", "tool0", legacyChain) && legacyChain.getNrOfJoints() == 6) + { + selectedChain = legacyChain; + std::cout << "Selected kinematic chain by legacy names: base -> tool0" << std::endl; + return true; + } + + const auto &segments = tree.getSegments(); + auto rootSegment = tree.getRootSegment(); + if (rootSegment == segments.end()) + { + std::cerr << "Failed to infer kinematic chain: tree root segment not found" << std::endl; + return false; + } + + const std::string rootName = rootSegment->first; + const std::vector leafSegmentNames = collectLeafSegmentNames(tree); + bool foundChain = false; + std::string selectedTipName; + unsigned int selectedSegmentCount = 0; + + for (const auto &tipName : leafSegmentNames) + { + KDL::Chain candidateChain; + if (!tree.getChain(rootName, tipName, candidateChain)) + { + continue; + } + + if (candidateChain.getNrOfJoints() != 6) + { + continue; + } + + // 多个 6 轴叶子链并存时，优先选择段数最多的完整工具链。 + if (!foundChain || candidateChain.getNrOfSegments() > selectedSegmentCount) + { + selectedChain = candidateChain; + selectedTipName = tipName; + selectedSegmentCount = candidateChain.getNrOfSegments(); + foundChain = true; + } + } + + if (!foundChain) + { + std::cerr << "Failed to infer a 6-joint kinematic chain from root segment: " + << rootName << std::endl; + return false; + } + + std::cout << "Selected kinematic chain by inference: " << rootName + << " -> " << selectedTipName + << " (" << selectedChain.getNrOfJoints() << " joints, " + << selectedChain.getNrOfSegments() << " segments)" << std::endl; + return true; +} + // 根据 URDF 初始化机器人，并提取关节与 child link 的 UUID 映射。 bool Robot::initRobot(const std::string &urdfString) { try { + delete fkSolver; + delete ikVelSolver; + delete ikSolverNR; + delete ikSolverLMA; + fkSolver = nullptr; + ikVelSolver = nullptr; + ikSolverNR = nullptr; + ikSolverLMA = nullptr; + m_initialized = false; + kinematicChain = KDL::Chain(); + KDL::Tree tree; // 从 URDF 字符串解析 KDL Tree。 @@ -52,10 +143,9 @@ bool Robot::initRobot(const std::string &urdfString) return false; } - // 提取 base 到 tool0 的运动学链。 - if (!tree.getChain("base", "tool0", kinematicChain)) + // 自动选择可用运动学链，避免固定依赖 base/tool0 命名。 + if (!selectKinematicChain(tree, kinematicChain)) { - std::cerr << "Failed to get chain from base to tool0" << std::endl; return false; }