自动推导URDF运动学链

include/Robot.h

@@ -8,6 +8,7 @@
 #include <kdl/chainiksolvervel_pinv.hpp>
 #include <kdl/frames.hpp>
 #include <kdl/jntarray.hpp>
+#include <kdl/tree.hpp>
 
 #include <string>
 #include <unordered_map>
@@ -34,6 +35,10 @@ private:
                            int minSteps, int maxSteps,
                            double positionResolution, double orientationResolution);
     bool parseJointChildLinkUuidsFromUrdf(const std::string &urdfString);
+    // 自动收集 KDL Tree 中没有子节点的末端 link 名称。
+    std::vector<std::string> collectLeafSegmentNames(const KDL::Tree &tree) const;
+    // 自动选择可用于正逆运动学的 6 轴串联链，避免固定依赖 base/tool0 命名。
+    bool selectKinematicChain(const KDL::Tree &tree, KDL::Chain &selectedChain) const;
 
 public:
     /**

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

src/Robot.cpp

@@ -11,6 +11,7 @@
 #include <kdl/trajectory_segment.hpp>
 #include <kdl/rotational_interpolation_sa.hpp>
 #include <regex>
+#include <vector>
 
 using namespace KDL;
 
@@ -38,11 +39,101 @@ Robot::~Robot()
     delete ikSolverLMA;
 }
 
+// 自动收集 KDL Tree 中的叶子节点，用作自动推导运动学末端候选。
+std::vector<std::string> Robot::collectLeafSegmentNames(const KDL::Tree &tree) const
+{
+    std::vector<std::string> 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<std::string> 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;
         }