自动推导URDF运动学链

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