Pinocchio 浮动基专题 02：沿源码读懂 RNEA、CRBA 与 ABA

浮动基动力学最值得追问的不是“调用哪个函数”，而是：一个位置加四元数的根关节，为什么可以进入与机械臂相同的递归？基座的六维加速度又是在哪一步解出来的？

本篇沿着真实实现回答这些问题。先建立源码阅读地图，再把 RNEA、CRBA、ABA 中的缓存变量翻译成物理量，最后回到浮动基的欠驱动方程。

阅读路线：专题入口 → 01：模型与流形 → 本篇 → 03：接口、数值技巧与自检。原来的 ABA 入门笔记保留，适合先建立三趟递归的直觉。

本文固定研究 Pinocchio v3.8.0，提交为 655877b314baed68c7e2d4dd56b0a0200bb9f98e。这是教学基线，不是“最新版”的声明。除特别说明外，递归按 Convention::LOCAL、无 mimic 的树形刚体模型展开；不能把其他版本、WORLD 分支或接触约束求解器的实现混在一起理解。

1. 先分清：三个算法不是三个不同的机器人模型

对同一个 Model、同一套广义速度坐标，无外力时统一写为：

$M(q)a+h(q,v)=\tau$

这里 q 的存储长度为 nq；v、a、tau 的长度为 nv。浮动基的 a 是与所选速度坐标配套的广义加速度，不是把七个配置存储数逐个求二阶导。

算法	已知	求什么	最适合回答的问题
RNEA	`q, v, a`	`tau`	实现这个运动需要多大的广义力？
CRBA	`q`	`M`	各自由度的加速如何相互耦合？
ABA	`q, v, tau`	`a`	施加这些广义力后系统怎样加速？

这三个 API 在概念上互相校验，却不需要在内部互相调用。ABA 并不是“先调用 CRBA，再调用矩阵求逆”。参见固定版本的 RNEA、CRBA 和 ABA 接口声明。

为什么根关节不需要另一套算法？

算法向关节模型询问的是相对变换、运动子空间、关节速度和偏置项，而不是询问“你是不是机械臂”。

单轴转动关节的运动子空间是六行一列；free-flyer 的运动子空间是六行六列的恒等映射。外层仍然可以执行同一种“父速度传播 + 本关节速度”的计算，只是局部块尺寸不同。

注意区分两种编号：model.joints[0] 是 universe 哨兵，不是具有六个自由度的浮动基。通常通过 free-flyer 建立的根动力学关节编号为 1，它的父编号才是 0。具体索引仍应从模型查询。

2. 源码地图：从 API 到具体关节类型

不要一开始就逐行阅读一个很长的头文件。先找三层结构：

算法入口：检查尺寸、初始化、选择 LOCAL / WORLD 分支
    ↓
逐关节 Pass::run：访问当前 JointModel / JointData
    ↓
具体关节的 calc / calc_aba：利用该关节的结构计算小块

阅读入口	核心实现位置	建议搜索的名字
逆动力学	`algorithm/rnea.hxx`	`RneaForwardStep`、`RneaBackwardStep`
质量矩阵	`algorithm/crba.hxx`	`CrbaLocalConventionForwardStep`、`CrbaLocalConventionBackwardStep`
前向动力学	`algorithm/aba.hxx`	`AbaLocalConventionForwardStep1`、`AbaLocalConventionBackwardStep`、`AbaLocalConventionForwardStep2`
单关节类型分发	`multibody/visitor/joint-unary-visitor.hpp`	`JointUnaryVisitorBase`、`boost::apply_visitor`
浮动基专用计算	`multibody/joint/joint-free-flyer.hpp`	`calc`、`calc_aba`

上表路径均相对于 include/pinocchio/。.hpp 往往是接口入口，.hxx 包含模板实现，但不要把扩展名当成绝对规则：free-flyer 的实现本身就在 .hpp 中。

Visitor 在这里解决了什么问题？

机器人可以同时包含转动、移动、球形和浮动基关节。外层循环处理统一的关节容器；visitor 从 variant 中识别实际类型，调用模板化的 algo<JointModel>。随后 jmodel.calc(...) 落到具体关节实现。

因此，这既不是“所有东西都在编译期决定、没有运行时分发”，也不是“每个关节都通过一组虚函数进行动态矩阵运算”。它结合运行时的类型选择与编译期的局部尺寸、特化类型；例如一自由度关节可以利用标量结构，而 free-flyer 使用固定六维块。

相关实现见 JointUnaryVisitorBase。boost::fusion::vector 在这些 pass 中用于打包参数；它不是又一次动力学计算。

3. Model 与 Data：结构和计算现场分开

Model 保存父子关系、关节类型、安装变换和惯量等模型描述。Data 保存与该模型尺寸对应的中间结果和输出。复用 Data 可以减少重复分配，但它不是自动维护一致性的数据库。

变量	本文中的含义	容易误读的地方
`model.parents[i]`	父关节编号	不是父 frame 编号
`model.jointPlacements[i]`	关节的固定安装变换	不包含本次关节配置的全部影响
`jdata.M()`	本关节配置产生的相对变换	不一定是世界位姿
`data.liMi[i]`	组合后的父关节到当前关节位姿表示	`act` 与 `actInv` 的方向不能互换
`jdata.S()`	从该关节切空间速度到空间运动的映射	列数看 `nv`，不是 `nq`
`data.v[i]`	当前关节坐标系中的空间速度	不是整个机器人的广义速度向量
`data.h[i]`	空间动量	不等于方程中的非线性项 `h(q,v)`
`data.a_gf[i]`	引入重力初始化后的递归加速度量	不能直接当作 IMU 的经典线加速度
`data.f[i]`	当前阶段的空间力缓存	不同算法、不同 pass 的含义会变化

定义可对照 Model 和 Data。修改模型拓扑、自由度等结构后，应重新建立与之匹配的 Data；并行计算应使用独立的可写 Data。

一个值得养成的调试习惯

查看缓存时，同时写出“由哪个 API、用哪组输入、在哪个分支更新”。例如，读取 oMf 不能仅因为此前调用过某个动力学算法，就假设所有 frame 位姿已经更新。需要 frame 位姿时，明确执行对应的运动学与 frame 更新流程。

4. RNEA：先把运动传下去，再把力传上来

为方便阅读，定义 X_i 表示将父关节的空间运动变换到当前关节坐标系；它对应 liMi[i].actInv(...) 的作用。I_i 是当前刚体在当前关节系中的空间惯量，vJ_i 是关节自身贡献的空间速度。

这里用自写数学伪代码解释结构，不直接复制实现。

第一趟：根到叶，计算惯性力

对每个动力学关节：

$v_i=X_i v_{p(i)}+v_{J,i}$

$c_i=c_{J,i}+v_i\times v_{J,i}$

$a_i^{gf}=X_i a_{p(i)}^{gf}+c_i+S_i a_{q,i}$

$f_i=I_i a_i^{gf}+v_i\times^{*}(I_i v_i)$

a_{q,i} 是广义加速度向量中属于该关节的块，不要与六维空间加速度混淆。符号 × 是运动对运动的空间叉乘，×* 是运动对力的对偶作用；不能用普通三维叉乘替代整个六维运算。

源码首先调用 jmodel.calc(q,v) 更新关节数据，然后组成 liMi，再更新 v、a_gf、h 和 f。重力并不是每个连杆循环里再加一个 mass * g：入口将 universe 的 a_gf[0] 设成负的模型重力，由递归自动传播。RNEA 前向实现

第二趟：叶到根，累计子树作用力

每个关节把累计的空间力投影到自己的运动子空间：

$\tau_i=S_i^{\mathsf T}f_i$

然后把子树作用力变换到父关节系并相加。liMi.act(force) 不只是旋转力向量，还包含力矩因作用参考点变化产生的平移项。

在 v3.8.0 的实际实现中，关节力使用映射 selector 累加到 data.tau，入口也会先清零 tau。这是为了容纳 mimic 关节对被模仿关节广义力的贡献；普通无 mimic 模型的教学公式可以写成赋值，但解释真实源码时不能遗漏这个差别。

为什么 RNEA 最后还有 armature？

关节等效转子惯量产生的是与广义加速度成正比的项。该版本在递归后另外加入：

$\tau\leftarrow\tau+\operatorname{diag}(\text{armature})a$

它不是摩擦，也不是重力补偿。若已经用模型的 armature，就不要在控制器外部无意中再加一遍同样的惯量贡献。

5. 外力：索引、坐标系与作用点缺一不可

带 fext 的 RNEA 在每个关节自己的力缓存中减去对应外力，之后再进行反向累计。因此方程可以写成：

$M(q)a+h(q,v)=\tau+\sum_i J_{i,LOCAL}^{\mathsf T}f_{ext,i}^{LOCAL}$

这里的 fext 按关节编号组织，长度与 model.njoints 对应；不是按 frame 列表，也不是按电机数量。外力条目以所属关节的局部坐标系表达。外力接口说明

如果力施加在足端 frame，而该 frame 与所属关节原点存在偏移，应先把力旋量从足端变换到关节系。设 jMf 表示足端在关节系中的位姿，则应使用 jMf.act(f_at_foot)。只旋转三维力而不平移力矩，会漏掉力臂效应。

同一关节上有多个外力作用点时，应逐个变换后累计。特别注意：v3.8.0 的 ABA 即便采用 WORLD 内部递归，外部传入的 fext 仍按该接口的局部约定输入；WORLD 分支内部再用 oMi.act(...) 转换。不要因为看见 WORLD 就直接塞一个世界系力旋量。

6. CRBA：把子树看成暂时锁住的复合刚体

CRBA 的核心对象是复合刚体惯量。可以想象：把某个关节下方的各关节暂时锁住，将整棵子树对当前关节的惯性贡献合并起来。

在 LOCAL 实现中，Ycrb[i] 从单个连杆的惯量开始；叶到根递归时，子树惯量经过坐标变换加给父节点。与此同时，Fcrb 缓存由复合惯量作用于运动子空间得到的力列，再通过当前关节的 Sᵀ 得到质量矩阵块。

为什么有 `nvSubtree` 和整块矩阵操作？

源码不是对每个元素都重新做一次逆动力学，而是按关节对应的行块、子树对应的列块写入。data.nvSubtree[i] 描述该子树速度维度范围，配合关节索引可以一次处理多列。

这解释了为什么源码里反复出现 middleCols、block 和 jointCols：它们不只是书写风格，而是在利用树结构和块结构。CRBAChecker 也描述了紧凑树排序要求；自行手工构造分支模型时，不应任意打乱关节编号再假设这些连续块仍然成立。LOCAL 复合惯量、子树块与检查器实现

C++ 和 Python 的“上三角”必须分开说

v3.8.0 的 C++ CRBA 核心接口只保证计算质量矩阵的上三角部分。要把它交给普通全矩阵运算，先明确补全对称部分。例如：

data.M.setZero();
pinocchio::crba(model, data, q, pinocchio::Convention::LOCAL);
Eigen::MatrixXd M = data.M.selfadjointView<Eigen::Upper>();

但 这个版本的 Python pin.crba 已经在绑定层补全对称矩阵：crba_proxy 先清零 data.M，调用 C++ 核心，再执行 make_symmetric 后返回。不能把 C++ 的约定机械套到 Python 上。Python CRBA 绑定

M = pin.crba(model, data, q).copy()  # v3.8.0 返回完整对称矩阵

不要使用 M + M.T 当作通用补全方法，那会把对角线加倍。在未知三角填充状态时，防御性做法应明确取上三角，再只镜像严格上三角。

CRBA 最后也加入 model.armature 的对角贡献，所以与 RNEA 比较时必须使用同一个模型状态。

7. ABA：消去关节加速度，而不是构造整个逆矩阵

ABA 与 CRBA 的“子树惯量”有一个本质差别：CRBA 合并的是锁住内部关节时的复合刚体惯量；ABA 合并的则是考虑内部关节可运动后，经消元得到的铰接体惯量。

把这两个东西都简称“惯量往上加”，很容易看不懂 ABA 中为什么要减掉一项。

第一趟：为消元准备局部运动与偏置力

AbaLocalConventionForwardStep1 更新关节变换和速度，计算偏置加速度，并初始化：

缓存	第一趟结束时的意义
`Yaba[i]`	从该连杆空间惯量开始的六乘六矩阵
`a_gf[i]`	目前只装局部运动偏置，尚非最终加速度
`h[i]`	空间动量
`f[i]`	速度引起的偏置力；有外力时还会减去外力
`u`	入口先复制 `tau`，反向递归中再扣除偏置投影

同一个名字 a_gf 在第一趟和第三趟的含义不同，这是阅读原地缓存算法时必须接受的事实。调试时停在不同 pass 看它，得到的并不是同一个物理完成度的结果。

第二趟：叶到根，做小块 Schur 消元

设当前已累计子树贡献的铰接体惯量为 I_A，关节 armature 对角块为 A_i。定义：

$U_i=I_A S_i,\qquad D_i=S_i^{\mathsf T}U_i+A_i$

$u_i=\tau_i-S_i^{\mathsf T}p_{A,i}$

消去当前关节加速度后，向父节点传播的惯量变为：

$\bar I_A=I_A-U_iD_i^{-1}U_i^{\mathsf T}$

对应偏置力还要加入 bar I_A * c_i 和 U_i D_i^{-1} u_i。随后惯量和力一起变换到父坐标系，累计到父节点。

源码缓存	数学含义
`jdata.U()`	`I_A S`
`jdata.StU()`	含 armature 的局部 `D` 块
`jdata.Dinv()`	局部 `D` 的逆或相应求解结果缓存
`jdata.UDinv()`	反复复用的 `U D⁻¹`
`data.u`	全部关节剩余驱动力块组成的向量

LOCAL 反向 pass 把关节专用的小块处理委托给 jmodel.calc_aba(...)。该调用可能原地更新 Yaba[i]；之后向父节点传播时用的已经是消元后的惯量。ABA LOCAL 三个 pass

第三趟：根到叶，回代求广义加速度

先把父空间加速度传播到当前关节，并加上第一趟保留的偏置，记为 a_pre；然后求：

$a_{q,i}=D_i^{-1}u_i-(U_iD_i^{-1})^{\mathsf T}a_{pre,i}$

再用 S_i a_{q,i} 补上当前关节空间加速度。至此，data.ddq 中相应的 nv 维块得到结果。

这里通过保存的局部块完成回代，没有显式生成全机器人 M⁻¹。此外，v3.8.0 的 LOCAL 高层实现还会在三趟主要动力学递归之后，再反向累计力缓存。“ABA 三趟”是主要求解过程，不是说这个具体函数体里恰好只有三个循环。

8. Free-flyer 的 `calc_aba` 为什么特别简洁？

在本关节局部运动坐标中，free-flyer 的 S 为六维恒等映射，因此通用表达式立即简化为：

$U=I_A,\qquad D=I_A+A_b$

源码直接把输入惯量赋给 U 和 StU，向 StU 对角加入 armature，再调用 PerformStYSInversion，缓存 Dinv 和 UDinv。它没有必要真的做一次“六乘六矩阵乘恒等矩阵”。free-flyer 专用实现

`update_I` 为什么取决于有没有动力学父关节？

LOCAL 反向 pass 传入的条件是 parent > 0。对直接连到 universe 的根 free-flyer，仍然需要求局部块，供第三趟计算基座加速度；但没有需要接收消元惯量的动力学父节点，因此无需执行用于向父节点传播的原地惯量更新。

这不是“浮动基没有惯量”，而是“递归已到最上层，不必再做无用传播”。也不能把它理解为某种默认固定基约束：基座六维加速度照样参与求解。

`calc(q,v)` 与 `calc_aba(...)` 不是一回事

free-flyer 的 calc 从配置块读取平移和四元数，形成关节变换；从速度块取六个切空间分量形成关节运动。calc_aba 则处理铰接体惯量的局部消元。前者回答“关节现在怎样运动”，后者回答“给定子树惯性，怎样消去该关节加速度”。

四元数归一化和 nq=7, nv=6 的原因见 01：模型与流形。动力学 pass 不会把非法配置神奇地变成合法配置。

9. 六维基座广义力不等于六个基座电机

free-flyer 在算法中有六维运动子空间，所以 RNEA 返回的根关节广义力也是六维。但“可以表达任意六维力旋量”与“机器人具有六个独立执行器”是两回事。

对于一个根 free-flyer 加一组全驱动关节的常见模型，真实控制输入通常写成：

$M(q)a+h(q,v)=B\tau_{act}+J_c^{\mathsf T}\lambda$

$B\tau_{act}=\begin{bmatrix}0_6\\\tau_{act}\end{bmatrix}$

这里的分块假定 free-flyer 位于广义速度向量最前面，其他关节驱动也恰好按该顺序排列；一般执行器布局应使用明确的映射 B。

为什么随便指定加速度，RNEA 会给出非零的前六维？

因为你指定的运动可能需要外部支撑、推进或接触力。RNEA 返回的是“实现它需要的广义力”，并不保证你的机器人真的能产生这个广义力。

例如悬空且无推进的机器人，不能靠内部电机让整体质心抵抗重力悬停。对任意期望加速度调用 RNEA，再把前六维截掉，并不会自动使剩下的电机力矩实现原加速度。

相反，给 ABA 的广义力中基座驱动块为零，只表示没有直接施加该六维广义驱动力；不表示基座加速度为零。它仍会受到重力、外力以及与内部关节的动力学耦合影响。

这一执行器映射是机器人建模层面的解释，不是 Pinocchio 替你配置的电机控制器。实际接触条件还需要约束方程和约束求解，不能只向普通 ABA 输入一个期望加速度。

10. 真正值得学习的数值与实现技巧

局部小块求逆，不等于显式求整个质量矩阵的逆

“不要显式求逆”针对的常见坏用法是：为求 M a = rhs 先计算整个稠密 M.inverse()。ABA 内部保存低维 Dinv，是为了高效复用小块消元结果；这是不同层次的问题。

选用何种数值分解与标量类型有关。v3.8.0 的 PerformStYSInversion 对浮点标量先建立单位矩阵，再通过 LLT 分解求解得到局部逆；非浮点标量走另外的 inverse 路径。LLT 路径依赖局部块的正定性，不能理解成遇到奇异惯量也会自动稳健求解。小块求逆的真实实现

对应用代码，只需要解一个线性系统时，通常直接使用合适的分解求解；不要为了模仿内部缓存而构造整个逆矩阵。

`noalias()` 是承诺，不是稳定性补丁

源码里的 Eigen noalias() 声明目标与乘积操作数不重叠，从而减少不必要的临时对象。只有这个承诺确实成立时才能使用；把它加到真实别名表达式上可能得到错误结果。

类似地，jointCols、固定维度块和专用空间代数操作，是在利用已知结构。不要把一个结构明确的六维运动全都展开成动态大矩阵，再指望编译器自动发现所有同样的简化。

有限数值不等于模型物理正确

负质量、错误单位、近乎零的转动惯量，可能让某些求解块奇异或病态。惯量矩阵对称、能被数组容纳，并不代表它对应一个真实刚体；质心处主惯量还应满足物理一致性条件。

检查数值问题时，先查质量、长度单位、惯量表达坐标系、质心偏移和 armature，再检查条件数与残差。随意给质量矩阵加一个很大的对角项，虽然可能使程序继续运行，却改变了你正在研究的动力学。

复杂度要带上前提

在每个关节自由度有固定上界、无约束树结构等常见前提下，RNEA 和 ABA 的主要递归工作量随关节数线性增长；显式构建完整质量矩阵的 CRBA 在串联链等最坏情形需要二次工作量。一般稠密矩阵分解则是三次量级，但有结构的求解器不能一概按稠密复杂度描述。

接触约束、闭链、越来越大的复合关节块、导数计算和 Python 包装开销，都会改变实际任务的代价。v3.8.0 的 ABA 实现还明确检查不支持 mimic 关节；不能因为 RNEA/CRBA 中看见 mimic 分支，就推断所有算法都支持同一种模型。

从两条局部方程推导 Schur 消元

前面列出了 ABA 的结果，现在把被消去的未知量明确写出来。设子树对当前关节的空间力关系为：

$f=I_A a_{sp}+p_A,\qquad a_{sp}=a_{pre}+S a_q$

关节的广义力平衡包含 armature：

$\tau=S^{\mathsf T}f+A a_q$

代入前两式，整理出唯一需要局部求解的未知量：

$\underbrace{(S^{\mathsf T}I_A S+A)}_D a_q =\underbrace{\tau-S^{\mathsf T}p_A}_u-S^{\mathsf T}I_A a_{pre}$

因此：

$a_q=D^{-1}u-D^{-1}U^{\mathsf T}a_{pre}$

再把这个结果代回空间力关系：

$f=(I_A-UD^{-1}U^{\mathsf T})a_{pre}+p_A+UD^{-1}u$

这时就能看见减号的来源：子树内部允许关节加速，会释放部分原本在“锁住关节”条件下传给父节点的惯性约束。它不是凭经验把惯量调小，也不是用负惯量抵消错误。

因为 a_pre = X * a_parent + c，向父级传播的偏置项中还必须加入 Ibar * c。如果手写 ABA 只实现了 Ibar 而遗漏这一项，静止零速度的测试可能通过，一旦关节运动起来就会失败。

上述推导假设 D 可逆；实际浮点路径进一步使用正定块的 LLT。若出现负质量、错误惯量或冗余参数造成的退化，就不能把公式中的逆当作总能计算的操作。

用 free-flyer 加两个 RY 关节跟踪一遍

考虑一条最小但不平凡的链，两个转动关节的转轴都沿各自局部 Y 轴。下面只给符号与尺寸，不编造某组模型的数值打印结果。

universe [0]
  └─ free-flyer [1]：基座刚体
       └─ RY [2]：第一段连杆
            └─ RY [3]：第二段连杆

关节	`idx_q` / `nq`	`idx_v` / `nv`	本关节 `S` 尺寸	局部 `D` 尺寸
free-flyer，编号 1	0 / 7	0 / 6	6 × 6	6 × 6
RY，编号 2	7 / 1	6 / 1	6 × 1	1 × 1
RY，编号 3	8 / 1	7 / 1	6 × 1	1 × 1

所以整机 nq=9、nv=8。这是此串联模型的索引表；实际 URDF 或复合关节模型仍应调用索引查询，而不是照抄常数。

第一趟的空间运动传播为：

$v_1=v_b,\qquad v_2=X_2v_1+S_2v_6,\qquad v_3=X_3v_2+S_3v_7$

此处 v_6、v_7 指零起始广义速度索引处的标量，v_1,v_2,v_3 则是按关节编号标记的六维空间速度。变量命名混用很容易造成误解，因此实际调试输出最好写成 qvel[6] 和 spatial_v[joint=2]。

停在何处	`Yaba[1]`	`Yaba[2]`	`Yaba[3]`	已得到什么
第一趟之后	基座单体惯量	连杆 2 单体惯量	连杆 3 单体惯量	三个局部速度、偏置力与偏置加速度
处理关节 3 之后	尚未接收后代	已收到关节 3 消元后的贡献	已用于求标量 `D3` 与传播	`U3,Dinv3,UDinv3,u3`
处理关节 2 之后	已收到整个机械臂的消元贡献	已用于求标量 `D2` 与传播	此后不再反向修改	`U2,Dinv2,UDinv2,u2`
处理关节 1 之后	整机传到根部的铰接体惯量	保持中间结果	保持中间结果	六维 `D1` 分解与基座剩余力

这张表能解释一个常见困惑：为什么 free-flyer 的 calc_aba 只拿到一个六乘六矩阵，仍能考虑整条手臂？因为处理到根节点时，输入已经不只是基座刚体自身惯量，而是下游两次消元的累计结果。

第三趟按相反依赖顺序回代：

已知 universe 的重力初始化
  → 用根节点缓存解出 ddq[0:6]
  → 得到基座空间加速度，传播到关节 2
  → 用关节 2 的标量块解出 ddq[6]
  → 传播到关节 3，再解出 ddq[7]

不能先独立求出两个电机关节加速度，再假设基座不动。浮动基和关节的耦合已经进入根部铰接体惯量，也会通过第三趟的父空间加速度传回来。

把三趟写成便于插桩的教学伪代码

下面的符号伪代码故意不调用 Pinocchio 的内部模板类，方便把每个量与源码缓存对应。motion_to_child 和 force_to_parent 必须是对偶的一对坐标变换。

for i in [1, 2, 3]:
    更新关节的变换 T、子空间 S、局部速度 vJ、偏置 cJ
    V[i] = motion_to_child(T[i], V[parent[i]]) + vJ[i]
    bias_a[i] = cJ[i] + motion_cross(V[i], vJ[i])
    IA[i] = 单体空间惯量[i]
    pA[i] = force_cross(V[i], IA[i] * V[i]) - external[i]

for i in [3, 2, 1]:
    U[i] = IA[i] * S[i]
    D[i] = transpose(S[i]) * U[i] + armature_block[i]
    u[i] = tau_block[i] - transpose(S[i]) * pA[i]
    保存 D[i] 的局部求解表示与 U[i]
    if parent[i] != 0:
        reduced_I = IA[i] - U[i] * solve(D[i], transpose(U[i]))
        reduced_p = pA[i] + reduced_I * bias_a[i]
        reduced_p += U[i] * solve(D[i], u[i])
        把 reduced_I 与 reduced_p 变换到父关节系并累计

for i in [1, 2, 3]:
    inherited_a = motion_to_child(T[i], A[parent[i]]) + bias_a[i]
    joint_a[i] = solve(D[i], u[i] - transpose(U[i]) * inherited_a)
    A[i] = inherited_a + S[i] * joint_a[i]

实际 Pinocchio 会利用关节特化、固定块尺寸与 UDinv 复用，不会逐行按此伪代码创建临时矩阵。本段目标是暴露依赖关系，不是提供性能等价的替代实现。

再用 RNEA 与 CRBA 看同一条链

对 RNEA，给定 a[0:6]、a[6]、a[7] 后，运动顺序仍为 1→2→3；力按 3→2→1 累计。因此 tau[0:6] 反映整棵链对基座运动的要求，而不只是基座刚体自己的惯性力。

对 CRBA，整机质量矩阵块结构为：

$M=\begin{bmatrix} M_{bb}^{6\times6}&M_{b2}^{6\times1}&M_{b3}^{6\times1}\\ M_{2b}^{1\times6}&M_{22}&M_{23}\\ M_{3b}^{1\times6}&M_{32}&M_{33} \end{bmatrix}$

即使两个 RY 的轴方向相同，非对角块也一般不为零。轴方向相同不代表没有惯性耦合；连杆质量、质心位置、安装偏移与当前配置都会进入这些块。

一个有区分力的实验是：保持 q,v 不变，只把第二个关节期望加速度增加 delta，检查 RNEA 输出差值是否等于质量矩阵第 7 列乘以 delta。相比随机比较整向量，这能直接检查你是否弄对 idx_v 和浮动基前六列。

线性复杂度隐藏了哪些常数？

对该模型，两个 RY 的 D 是标量，free-flyer 的 D 是固定六乘六块。增加更多固定单自由度关节，并不会让根节点的块尺寸跟着机器人自由度一起增长，所以主要递归能保持线性增长。

但若某个自定义复合关节的局部自由度 k 自身不断增长，对它的稠密块分解代价可达到三次量级，不能再把“每关节常数开销”当作前提。输出全质量矩阵、全导数张量、接触 Delassus 矩阵的任务，也不是一次普通 ABA 的同一个复杂度问题。

源码阅读时建议分别统计：树遍历次数、每次遍历处理的块尺寸、是否产生整机稠密输出。这比只数 for 循环层数更接近实际代价。

11. 用恒等式确认自己真的读懂了

比“打印出的数看起来合理”更有价值的是构造能失败的检查。使用相同模型、配置、速度、armature 和外力约定，并为结果复制或使用不同 Data：

给定合法 q,v,a，检查 rnea(q,v,a) 与 M @ a + nonLinearEffects(q,v) 的残差。
用上一步广义力调用 ABA，检查是否回到原加速度。
设置一个已知局部外力，检查带外力 RNEA 相比无外力结果的变化是否为负的 J_LOCAL.T @ fext。
对悬空浮动基将直接驱动的基座力块置零，检查重力下基座仍会响应，而不是误认为它被固定。
改变一个关节的 armature，检查 RNEA 与 CRBA 的变化是否符合同一个惯量增量。

这些是应执行的自检条件，不是本文预先宣称通过的实验结果。完整可运行接口、自检脚本和验证状态放在 03：接口、数值技巧与自检，以该篇记录为准。

读到这里，再打开 aba.hxx 时，建议只追一条路径：aba → abaLocalConvention → 三个 LOCAL pass → JointModelFreeFlyerTpl::calc_aba。先弄清每个缓存何时被写、属于哪个坐标系、是否已被消元，再扩展到 WORLD、解析导数和接触约束。