从零造一个腱绳驱动灵巧手——仿真器篇2：运动学树——让一只散装的手“活“过来

一、从一堆 STL 到一只手

上一篇我们搭好了骨架。现在面对一个很现实的问题：

桌面上躺着 一堆 STL 零件文件——指尖、指节、掌骨、轴承座、T 形联轴器……它们在建模软件里是一个装配体，但导出 STL 时已经"散架"了，各自带着自己的坐标系。

怎么把它们装回一只会动的手？

答案是一棵运动学树。

二、树节点：Joint + Link = 一个零件的全部

先看数据结构——整个仿真器的"宪法"：

```cpp
// core/types.hpp
struct TreeNode {
int id = -1;
int parent = -1; // -1 = 根；-2 = 已删除
Joint joint;
Link link;
};
```

每个节点 = 一条 Joint（关节）+ 一个 Link（连杆）。

• Joint 描述"我怎么动"：类型（焊死 or 绕轴转）、固定变换、转轴方向、限位

• Link 描述"我长什么样"：STL 路径、几何偏移、缩放、质量参数

手动摆位姿 = 一条 Fixed Joint

这里有个精妙的设计决策：

```cpp
struct Joint {
JointType type = JointType::Fixed; // ← 手动摆位姿 = Fixed
Transform origin = Transform::Identity(); // ← 摆到哪就在 origin 里
Vec3 axis = Vec3::UnitZ(); // Fixed 不用转轴
// …
};
```

手动拖动/旋转一个零件，不是修改什么"pose 字段"，而是存一条 Fixed Joint。

为什么这样设计？因为设计模式和仿真模式必须共用同一棵树。如果摆位姿用一个 pose 字段、运动学用另一个 joint 字段，两边迟早打架。统一成 Joint，一切语义归一：

三、树的操作：增删改查，每个都有坑

3.1 新增节点

```cpp
int Tree::addNode(const Joint& joint, const Link& link, int parent_id) {
TreeNode n;
n.id = static_cast<int>(nodes_.size()); // id = 数组下标
n.parent = parent_id;
n.joint = joint;
n.link = link;
nodes_.push_back(n);
states_.push_back(JointState{});
return n.id;
}
```

id 就是数组下标——简单粗暴，O(1) 访问。

3.2 删除节点——"墓碑"策略

```cpp
bool Tree::removeNode(int id) {
for (const auto& n : nodes_)
if (n.parent == id) return false; // 还有子节点？不行！
nodes_[id].parent = -2; // 墓碑：id 保持稳定
return true;
}
```

为什么不用 vector::erase？因为id 会变。如果删了 id=3，后面所有 id 都往前挪一位，那么之前存在 JSON 里的 parent=7 就指错人了。

所以用 -2 标记"已删除"，id 永远不回收。aliveNodes() 遍历时自动跳过墓碑。

踩坑记录：一开始我用 tree_.size()+1 给新节点命名，删除后 size 不减小，

假设我新增了一个节点，但是删掉了，再新增一个节点，编号就从 53 跳到 54、55……明明节点为空，但是编号被占用。后来改用 aliveNodes().size()+1 + 去重，才连续了。

3.3 换父节点——防环是刚需

```cpp
bool Tree::setParent(int id, int parent_id) {
if (id == parent_id) return false;
// 防环：parent 的祖先链里不能出现自己
for (int p = parent_id; p >= 0; ) {
if (p == id) return false;
const TreeNode* np = node(p);
if (!np || np->parent == -2) break;
p = np->parent;
}
nodes_[id].parent = parent_id;
return true;
}
```

如果允许环，FK 递推会死循环。防环检查是树操作的底线。

四、正向运动学：链式累乘

FK 公式（URDF 语义，机器人学标准）：

```
T_link = T_parent · joint.origin · Rot(axis, q)
```

三个因子的含义：

• T_parent：爸在哪

• joint.origin：我出生时相对爸的偏移

• Rot(axis, q)：我绕自己的轴转了多少度

代码实现：

```cpp
Transform linkPose(const Tree& tree, int node_id) {
// 从自己一路爬到根
const std::vector<int> chain = tree.chainToRoot(node_id);

Transform T = Transform::Identity();
// 从根侧开始累乘
for (auto it = chain.rbegin(); it != chain.rend(); ++it) {
const TreeNode* n = tree.node(*it);
if (!n || n->parent == -2) break;
T = T * (n->joint.origin * jointMotion(*n, nodeQ(tree, n->id)));
}
return T;
}
```

其中"绕轴转"就是 Eigen 的 AngleAxisd：

```cpp
Transform jointMotion(const TreeNode& n, double q) {
if (n.joint.type != JointType::Revolute)
return Transform::Identity(); // Fixed 不转
Transform R = Transform::Identity();
R.linear() = Eigen::AngleAxisd(q, n.joint.axis).toRotationMatrix();
return R;
}

红线提醒：限位（limit）不在 FK 里截断。FK 只管忠实计算矩阵，q 的合法性由上层（UI 输入、积分器）保证。这保持了函数的单一职责。

验证 FK：不要靠肉眼

```cpp
// _fk_test.cpp —— 纯数值验证
// 建一条 3 节点链，设 q=90°，检查末端位姿
auto poses = hand_core::forwardKinematics(tree);
double diff = (poses.back().matrix() – expected.matrix()).cwiseAbs().maxCoeff();
std::printf("max diff = %e\\n", diff); // 期望 1e-15 量级
```

一行 printf，比开窗口截图放大看快 1000 倍。

五、插轴：让静态零件变成活动关节

这是整个项目最"魔术"的一步。在 3D 里点击一个圆柱面（销轴/孔壁），系统自动：

关键的写回公式：

```cpp
// 插轴前：零件显示位姿 D = linkPose(node) · visual_origin
// 插轴后：joint.origin = Tp^-1 · [Rf|point]，axis = Rf^T · worldDir
// visual_origin = W^-1 · D（吸收偏移，显示不变）
const Transform W = …; // [Rf|point]
n->joint.type = JointType::Revolute;
n->joint.origin = Tp.inverse() * W;
n->joint.axis = Rf.transpose() * dir;
n->link.visual_origin = W.inverse() * D;
```

子树锚定——一个隐藏的坑：

插轴时 link 坐标系会搬到轴点。如果这个节点下面还挂着子节点（比如指尖挂在指节下），子节点的 origin 必须同步换算：

```cpp
const Transform Lnew = linkPose(tree, nodeId); // 新 link 位姿
const Transform Lfix = Lnew.inverse() * Lold; // 修复因子
for (int cid : tree.children(nodeId)) {
cn->joint.origin = Lfix * cn->joint.origin; // 子件跟着搬
}
```

否则——"插轴后子件集体飞走了"。我第一次没做这步，查了两小时才发现是子节点 origin 没跟着换算。

六、Mimic 联动：一个滑条驱动两个关节

T 形联轴器是一个经典难题：两个输入件（inner/outer）都固定在支架上，互不干扰，但共同约束一个输出件。串联树表达不了并联约束。

解法是 mimic 联动（URDF <mimic> 标准语义）：

```cpp
struct JointMimic {
int joint_id = -1; // 跟随谁
double multiplier = 1.0; // q_从 = m · q_主 + c
double offset = 0.0;
};
```

搭树：

```
base（支架）
├─ outer_1 [revolute] ← 驱动端：只俯仰
└─ inner_1 [revolute] ← 只左右
└─ pitch_link [revolute] ← 虚拟节点（无几何），mimic 跟随 outer_1
└─ 联轴器 [fixed] → 手
```

拖 outer 的滑条 → pitch_link 自动同步 → 手俯仰，inner 不受影响。完美。

实现要点：

```cpp
bool Tree::applyQWithMimic(int id, double q) {
int src = id; double srcQ = q;
if (mimic.valid()) {
src = mimic.joint_id;
srcQ = (q – mimic.offset) / mimic.multiplier; // 反推驱动端
}
states_[src].q = srcQ; // 写驱动端
// 广播所有从动节点
for (auto& n : nodes_) {
if (n.joint.mimic.joint_id == src)
states_[n.id].q = n.joint.mimic.multiplier * srcQ + n.joint.mimic.offset;
}
}
```

一级联动防环：从动节点的跟随目标自身不得再有 mimic，否则 A→B→A 死循环。

但是最后在建模里面改善了这个，用了一个笨办法解决了，这个部分的代码就注释掉，没有启用了哈哈哈哈哈

七、数值测试：先把核心跑绿

每次改完运动学相关代码，第一件事是跑测试，第一篇里面，每个部分我哦们都是独立拆开的，方便调试，这就为我们打好了测试调试的基础：

```powershell
cd hand_sim/build
.\\_fk_test.exe # 正向运动学 ✓
.\\_axis_subtree_test.exe # 插轴子树锚定 ✓
.\\_mimic_test.exe # 关节联动 ✓
.\\_hinge_test.exe # 铰链验证 ✓
```

测试全绿才继续。红灯就停。

下一篇讲 VTK 渲染与 3D 交互——怎么把 Eigen 算出的矩阵变成屏幕上的像素，怎么在 3D 里点选零件、拖拽 Gizmo、实时旋转手指，但是csdn不知道为啥发帖额度还有限制，无语了，那就等过几天空闲了再同步吧