树形刚体动力学：RNEA 与 CRBA

考虑由两个铰链连接的两根刚性杆：关节一将第一杆连接到固定基座，关节二将第二杆连接到第一杆末端。给定某一时刻的关节角、关节角速度和关节角加速度后，两根连杆在该时刻的瞬时运动状态便随之确定。根据给定的运动状态反求各关节电机需要提供的驱动力矩，这项计算称为逆动力学（Inverse Dynamics，ID）：

式中的 a 是指定的关节角加速度，描述各关节处的相对转动快慢变化；而每根连杆质心的三维空间加速度不仅取决于本关节的运动，还叠加了上游父连杆传来的牵连运动。因此在计算受力之前，必须先把从基座到各连杆的运动传递关系算清楚。

设两根连杆的长度均为 1\,\mathrm m、质量均为 1\,\mathrm{kg}，质心位于各杆中点。每根杆的截面尺寸为 0.1\,\mathrm m\times0.1\,\mathrm m，由长方体主轴转动惯量公式 J=m(\ell^2+b^2)/12（其中杆长 \ell=1\,\mathrm m，横向宽度 b=0.1\,\mathrm m）可得，单杆绕质心纵深轴的转动惯量为 J=101/1200\,\mathrm{kg\,m^2}，用于计算连杆产生角加速度所需的合力矩。

先考察两杆水平向右伸展并保持静止的状态，即 q=(\pi/2,0)^{\mathsf T}、v=a=0，取重力加速度大小 g=10\,\mathrm{m/s^2}。记两个关节中心的位置为 O_1,O_2，两杆质心的位置为 C_1,C_2，则它们在世界系下的坐标分别为

我们先单独隔离出第二杆分析其受力平衡。由于第二杆受到 10\,\mathrm N 向下的重力，关节二必须向上提供 10\,\mathrm N 的支撑力；同时，第二杆质心距关节二的水平力臂为 0.5\,\mathrm m，因此第二关节电机还必须提供 0.5\times10=5\,\mathrm{N\,m} 的支撑力矩。这组支撑力与支撑力矩共同构成了第二杆在连接点 O_2 处的完整受力需求。

根据作用力与反作用力定律，第二杆会对第一杆末端施加大小相等、方向相反的力与力矩。因此，关节一不仅要支撑第一杆自身的重量，还要一并承担第二杆从关节二传回的力与力矩。当把作用在关节二处的 10\,\mathrm N 向上支撑力平移到关节一处计矩时，由于两个关节相距 1\,\mathrm m，会额外产生一段附加力矩：

将这部分传回的力矩再加上第一杆自身重力所需的 5\,\mathrm{N\,m} 支撑力矩，便得到两个关节在水平静止状态下的驱动力矩：

实验中的默认状态正对应式（8.1）的读数。若将第二杆向内折回使其质心靠近关节一，关节一所需的支撑力矩会显著减小，但关节一需要提供的竖直总支撑力依然等于两杆重量之和。可见力臂决定了转动负载，而总质量决定了平动负载，因此在向基座回传受力时，必须把力与力矩同时保留下来。

概括起来，沿运动学树从根向叶推进时，每一步由父体运动确定子体运动；沿运动学树从叶向根回传时，每一步将子体的受力需求换算到父连接点并与父体需求累加。RNEA 正是将这两种方向相反的物理依赖组织成前后两遍遍历。下面先把静止支撑中的“受力需求”推广到一般加速运动状态。

当连杆开始加速运动时，关节除了平衡重力外，还要提供改变连杆线动量与角动量的惯性力。先考察单根刚性杆：设其与父体的连接点为 O，质心为 C，从连接点指向质心的位移向量为 r。已知连接点的线加速度 a_O、刚体角速度 \omega 和角加速度 \alpha，则质心的线加速度为

式（8.2）右侧的三项分别来自连接点的牵连平动、刚体角速度变化引起的切向加速度，以及刚体旋转产生的向心加速度，这正是第 5 章刚体两点运动关系在质心处的直接应用。求出质心加速度后，在质心处写出刚体的 Newton–Euler 方程，就能把运动状态转化为连接点处的受力需求。

记重力加速度向量为 g_W=(0,0,-g)^{\mathsf T}，并设除重力与连接处作用力外无其他外力。将连接点处必须对刚体提供的合力记为 f，关于连接点 O 的合力矩记为 t，则由质心平动与转动方程可得

式中 J_C 为刚体关于质心的三维转动惯量矩阵（此处沿世界坐标轴表示）。力矩公式的前两项 J_C\alpha+\omega\times(J_C\omega) 是改变刚体绕质心角动量所需的质心合力矩，最后一项 r\times f 则将合力 f 的计矩参考点从质心换算到连接点 O。因此，加速运动刚体在连接点处的受力需求依然由“合力与关于连接点的合力矩”两部分组成。

同一两连杆的运动输入

回到上一节水平向右伸展的两连杆构型，保持 q=(\pi/2,0)^{\mathsf T}，现赋予关节角速度 v=(1,2)^{\mathsf T}\,\mathrm{rad/s} 与关节角加速度 a=(2,1)^{\mathsf T}\,\mathrm{rad/s^2}。由于两根关节转轴平行且均指向负纵深方向，第二杆相对于世界系的绝对角速度与角加速度可直接由两关节分量相加得到：

因为基座固定，第一关节中心满足 a_{O_1}=0。第二关节中心 O_2 位于第一杆末端，先由式（8.2）算出 a_{O_2}，再依次算出两杆质心的线加速度：

此时两个质心都在向上加速，因此关节需要提供比静止时更大的竖向支撑力；与此同时，连接处还必须提供向左的水平拉力以维持两杆旋转所需的向心加速度。将质心加速度与角运动量代入式（8.3），可得每根连杆自身的受力需求：

把第二杆的需求换到关节一

由于两根连杆的受力需求都沿世界坐标轴表示，将它们在关节一处合并前只需统一计矩参考点。把第二杆的受力从关节二平移到关节一，合力矩会多出叉乘项 d\times f_2。记整条两连杆子树在关节一处所需的总合力与总力矩为 f_\Sigma,t_\Sigma，则有

代入 d=(1,0,0)^{\mathsf T} 与 f_2=(-5.5,0,13.5)^{\mathsf T}，移点附加力矩 d\times f_2 的纵深分量为 -13.5\,\mathrm{N\,m}。再沿关节正转轴 (0,-1,0)^{\mathsf T} 投影力矩分量，便得到两个关节的驱动力矩：

对比前两节可以看到，静止算例与加速运动算例遵循完全相同的计算顺序：先从基座向外确定每根连杆的运动，再由单杆 Newton–Euler 方程计算本体受力需求，最后从末端向内换算参考点并累加下游子树需求。一般运动学树只是增加了节点与分支数量，并允许各连杆处于不同姿态。下一节引入六维空间向量，将这些反复出现的三维换点与旋转运算收拢为统一的矩阵操作。

前两节始终同时处理一组角量和一组线量：角速度与连接点速度、合力矩与合力。把每组的两个三维向量上下排列，便得到六维空间向量。每个刚体选择自己的坐标系，原点位于与父体相连的连接点，坐标轴固定在该刚体上。

第五章的式（5.3）、式（5.4）分别规定运动与力的变换方向；式（5.7）给出单体所需空间力。下面把这些关系用于当前两连杆，并保持前面算出的普通点加速度作为核对。

V 记录该刚体相对世界的运动，只是分量沿刚体自身的轴表示。u 为连接点速度，t 关于同一个连接点计矩。B 继续表示刚体自身需要的受力；后面用 F 表示已经包含所有后代的受力。

空间加速度与普通点加速度

刚体坐标轴本身在转动。在这套轴中，对一个向量的分量求导，需要减去坐标轴转动带来的变化。连接点速度的分量导数因此为 a_O-\omega\times u；角速度的对应修正为 \omega\times\omega=0。本章的空间加速度定义为

下标 \rm lin 表示取六维数组的后三项。例如，一根杆以恒定角速度转动时，连接点的随体速度分量可能恒定，普通点加速度仍含向心项。式（8.5）的恢复关系把两种描述联系起来。重力将在后面作为一个统一的加速度修正加入。

空间惯量把速度映射为动量

在随体坐标中，质心位置 r 和质心惯量 J_C 都是常量。质心速度为 u+\omega\times r，线动量为 p=m(u+\omega\times r)。关于连接点的角动量则为 h=J_C\omega+r\times p。把这两式按 \omega,u 收集系数，得到空间惯量：

[r]_\times x=r\times x，\mathbf1 是三阶单位矩阵。右下块计算平动动量，非对角块记录质心偏离连接点后的平动与转动联系；左上块包含熟悉的平行轴贡献。三维质心惯量用 J_C，六维连接点惯量用 I，二者的参考点和矩阵大小由此区分。

单体 Newton–Euler 方程的空间形式

空间力叉乘表示一个运动量对力或动量分量的运输作用。对 V=(\omega,u)^{\mathsf T}，它逐项计算

刚体坐标中的惯量保持不变。惯量乘加速度给出分量变化，以上运输项补上坐标转动与参考点运动的影响。扣除已知外力 E 后，本体需求为

这里 E 也按“关于连接点的力矩、合力”排列，并沿同一套随体坐标轴表示。若全部外力都显式放入 E，其中也应包括重力。后面的 RNEA 采用根加速度计入重力，届时 E 只保留其他已知外力。

至此，单个刚体的受力已经可以由 V,A,I 求出。机构中的下一项工作，是由父体运动建立子体的 V,A。

第二杆的绝对运动由两部分相加：父杆在第二连接点处带来的运动，以及第二关节允许的相对运动。相加前，还要把父杆的分量转到第二杆的坐标轴。于是每条父子边都执行“换参考点、换坐标轴、加入关节运动”三个操作。

下标 p 表示当前节点的父体。父原点到子原点的位移 d_i 在父坐标中表示；R_i 把子坐标分量变为父坐标分量。先暂时锁住本关节，从父运动得到子原点处的运动：

改变计量点不改变角速度；线速度多出 \omega_p\times d_i。外侧的 R_i^{\mathsf T} 再把两个向量改写到子坐标轴。这组运算可以合写成第五章的运动变换矩阵：

关节子空间 S_i 把一个关节速度变成六维相对速度。转轴经过子原点的铰链与滑动关节分别使用

其中 \boldsymbol n_i 在子坐标系中表示。先运输父速度，再加入关节运动：

这里 v_i 为单个关节的标量速度；V_i 为整根刚体的六维绝对速度；S_i 规定该关节允许的运动方向。例如，父体静止、铰链轴为 (0,-1,0)^{\mathsf T}，关节速度为一时，S_i v_i=(0,-1,0,0,0,0)^{\mathsf T}。

接着对式（8.8）求导，便得到加速度递推。空间加速度的线分量采用式（8.5）的定义。对刚体自身的坐标分量求时间导数时，坐标轴旋转的修正已经包含在定义中：

运动叉乘用于处理两组运动量。若 V=(\omega,u)^{\mathsf T}、W=(\eta,w)^{\mathsf T}，其分量定义为

它的输入、输出都是运动型向量；前节的 \times^* 则作用于力或动量型向量。以下的速度乘积项用这一分量定义直接计算。

关节运动使 X_i 随时间变化。固定轴关节满足运输恒等式，因而得到

式（8.9）把子加速度分成父加速度、本关节加速度和速度乘积三项。前两项随指定关节加速度线性改变，最后一项由当前速度决定。父体转动着带动一个滑块时，即使关节速度数值恒定，也会出现横向加速度；下面用这个例子核对速度乘积项。若采用随配置变化的子空间，直接求导还会出现 \dot S_i v_i；其形式应从所选关节参数化计算。

作为检查，考虑父体绕竖轴转动、滑块沿父体横轴移动的情形。角速度为 \Omega、相对滑速为 u 时，普通点加速度应含 2\Omega u\boldsymbol e_y，称为科里奥利加速度（Coriolis acceleration）。在空间递推中，这个因子二来自两处：速度乘积项贡献一份，恢复普通点加速度时再贡献一份。

从根向叶处理时，每个节点都能取得已经算好的父体速度与加速度。加入本关节运动后，就可以用单刚体 Newton–Euler 方程求本体力。此时尚未计入后代的负担；它们将在下一遍反向累加。

本节开始使用树的编号：世界节点为 0，父节点为 p(i)，刚体按照父在前、子在后的顺序存放。每次处理节点时，父节点的结果已经存在。固定基座的速度为零。计算重力时，把负重力放在根节点加速度的线部：

采用这一初值之后，计算中 A_i 的线部统一表示 a_{O_i}-g_i-\omega_i\times u_i，其中 R_{Wi} 把当前体坐标分量转换为世界分量，g_i=R_{Wi}^{\mathsf T}g_W 是重力在体坐标中的分量。它已经包含重力修正，已知外力 E_i 随后只记录其余外力。

在每个节点先算速度、加速度，再算该体所需的空间力。记自身惯量为 I_i，外界直接施加的空间力为 E_i：

外力已经承担了部分运动需求，所以在所需关节力中扣除。E_i 沿体坐标轴表示，力矩关于体原点；重力已经由根加速度计入。此时 B_i 只包含该刚体自身的贡献。

若世界点力 \boldsymbol f_W 作用在局部位置 \boldsymbol r，并有附加世界力偶 \boldsymbol t_W，换成局部空间力为

根加速度含重力时，递推得到的 A_i 也含这一修正。要读取物理点加速度，在局部坐标中先恢复 a_{O_i}=(A_i)_{\rm lin}+\omega_i\times u_i+g_i，再按式（8.2）求质心加速度。

处理完这一遍，每个数组各有明确内容：V_i 是绝对运动，A_i 是重力修正后的空间加速度，B_i 是本体需求。下一遍只需要将这些本体需求沿树累加。

一根连杆的关节既要推动本体，也要承担与它相连的后代。叶节点的全部需求已经算出，便可先投影出本关节力，再把完整的空间受力信息送给父节点。父节点等待所有子节点完成后相加，就能处理自己的关节。

先处理一条父子边。以下两个向量记录子树已经累加的受力。子体的力为 f_i、关于子原点的力矩为 t_i，两者沿子坐标轴表示。先乘 R_i 换成父坐标分量，再把计矩点移到父原点：

这正是式（8.4）的换点操作，再加上一项坐标旋转。将两行合起来，就是运动变换的转置：

叶节点只承担自身受力。父节点还要承担各个子树传回的力。子空间力变到父表示时使用 X_i^{\mathsf T}，所以子树力与关节力依次为

\mathcal C(i) 是直接连接到节点 i 的子节点集合。叶节点的集合为空，故 F_i=B_i；两连杆则是 F_2=B_2、F_1=B_1+X_2^{\mathsf T}F_2。

将六维力投影到 S_i，得到关节允许方向上的驱动力。铰链取 \tau_i=n_i^{\mathsf T}t_i，滑动关节取 \tau_i=n_i^{\mathsf T}f_i。理想关节对其他方向提供约束反力。投影得到 \tau_i 后，完整的 F_i 仍继续回传，这与第一节的作用和反作用分析一致。

遍历先处理全部子节点，再处理父节点。固定连接没有独立广义力，仍把整棵子树的空间力传给父体。

下面改用两个关节都有转角的构型，以便观察父子坐标之间的旋转。实验采用 q=(30^\circ,60^\circ)^{\mathsf T}、v=(1,-0.5)^{\mathsf T}\,\mathrm{rad/s}、a=(0.4,-0.2)^{\mathsf T}\,\mathrm{rad/s^2} 和 g=9.81\,\mathrm{m/s^2}。

先保留默认输入，从根初值开始逐步推进。到节点二时，检查 V_2=X_2V_1+S_2v_2；进入回传阶段后，检查 F_2=B_2、F_1=B_1+X_2^{\mathsf T}F_2。每一步把输入贡献与相加结果并排显示；先逐分量核对，再观察该量在机构上的物理方向。

默认输入的计算结果为 \tau\approx(13.337760,5.504846)^{\mathsf T}\,\mathrm{N\,m}。前向阶段分别建立本体运动，反向阶段才把各本体的需求汇到关节；后面的完整两连杆算例将逐项计算这些数值。把重力设为零，可以单独比较惯性和速度项的作用。切换为静止支撑后，质心加速度为零，关节仍要承担重力产生的力矩；改用正交关节轴后，力矩投影改沿各自关节轴计算。完整六维公式保持同样的结构。

先用零重力，只保留末端点力。静止条件下，维持构型的关节力应为 -J^{\mathsf T}f；逐级回传外力也应得到同一个结果。改变外力方向，观察力臂与关节轴的投影怎样改变两个关节力矩。

再开启重力，支承力矩成为 \tau=c-J^{\mathsf T}f。这里 J^{\mathsf T}f 是外力已经提供的广义力，驱动补足剩余部分。图中两种计算的差值检查了空间力换点、坐标旋转和关节投影的符号。

两遍遍历各处理每个刚体一次。对自由度数固定的关节，RNEA 的运算量与刚体数量成正比。树可以有多个分支，回传时将各分支贡献相加即可。

下面使用 RNEA 实验中的同一组输入，把两个刚体的计算完整展开。与开头的水平构型相比，这组姿态需要实际进行父子坐标旋转。先求速度，再求加速度和本体力，最后回传；每一步继续使用前一步已经求得的量。

取 q=(\pi/6,\pi/3)^{\mathsf T}、v=(1,-0.5)^{\mathsf T}\,\mathrm{rad/s}、a=(0.4,-0.2)^{\mathsf T}\,\mathrm{rad/s^2}。体坐标原点在各杆上端，关节轴为 \boldsymbol n=(0,-1,0)^{\mathsf T}，质心局部位置为 (0,0,-0.5)^{\mathsf T}\,\mathrm m。

速度递推给出下列随体分量。六维数组的前三项为角速度，后三项为原点线速度：

包含重力修正的加速度和本体力为

力数组的前三项单位为 \mathrm{N\,m}，后三项为 \mathrm N；加速度数组分别使用 \mathrm{rad/s^2} 和 \mathrm{m/s^2}。回传并投影后

RNEA 已经把一组指定加速度转换为关节力。现在固定姿态和速度，连续做两次计算：第一次令所有关节加速度为零，第二次只给某个关节增加单位加速度。两次的重力、当前速度与外力相同，所需关节力的差就来自这次加速度变化。

先只保留重力作为已知外力。零加速度结果称为偏置力，记为 c。保持当前关节速度而令关节加速度为零，刚体仍可能有向心加速度；因此偏置力既包含重力，也包含由速度引起的受力。

固定配置和速度后，关节加速度的增量会线性传递到空间加速度；惯量乘法、回传和投影也保持线性。因此，所需关节力由零加速度结果与加速度贡献相加得到：

重力、速度效应和给定外力进入零加速度结果。本文用 c 表示无额外点力的偏置；有点力时，记零加速度结果为 c_E=c-J^{\mathsf T}f。

令 e_j 为单位加速度数组，两次逆动力学调用之差给出质量矩阵的一列：

例如某个两自由度机构中，两个关节分别增加单位加速度时，额外力矩为 (4,1)^{\mathsf T} 和 (1,2)^{\mathsf T}，就把这两列排成 M=\begin{bmatrix}4&1\\1&2\end{bmatrix}。第一列中第二行的一表示：仅增加第一关节加速度，也会改变第二关节所需力矩。这就是质量矩阵非对角项的惯性耦合含义。

逐列调用是直观的质量矩阵构造方法。它可以核对 CRBA 的每一列；对长链，重复回传的运算量约为自由度数的平方。

逐列调用 RNEA 可以得到质量矩阵，但每一列都会重复计算许多相同的后代贡献。复合刚体算法（Composite Rigid Body Algorithm，CRBA）先把这些贡献合并，再沿祖先链装配矩阵。先在两连杆上看清合并的是哪一部分。

从两根杆的动能读出三个系数

固定构型后，运动变换 X_2 是已知矩阵。两个刚体的速度为

把它们代入两杆动能之和，分别收集 v_1^2、v_1v_2、v_2^2 的系数：

第二关节只移动第二杆，所以第二个对角项只含第二杆惯量。第一关节移动两根杆，所以第一个对角项包含两者之和。中间的混合项描述两种关节运动在第二杆上共同产生的动能。

将同样的合并操作用于整棵子树

两连杆的 I_1+X_2^{\mathsf T}I_2X_2 就是关于第一连接点的复合惯量。它表示：在当前姿态将第二关节暂时锁住后，整组刚体随第一杆共同运动时具有的惯量。实际动力学仍允许第二关节运动；锁定用于提取这一个系数。

逐列调用逆动力学会反复经过同一子树。质量矩阵只关心给定构型下的惯性响应，因此可以先把子树的惯量合并，再把各个单位关节方向送入这个合并结果。复合惯量保存了所有后代对共同运动的总贡献。

设刚体局部空间速度为 V_i=J_iv。从每个刚体的动能相加，第七章的质量矩阵写成

为了合并重复的后代贡献，在当前配置下暂时锁住子树内的关节。整棵子树相对根节点作同一个刚体运动，得到复合惯量 I_i^C：

用复合惯量装配一列

取某关节的单位加速度、其余关节加速度为零，并将速度和重力清零。整棵后代子树的惯性需求可以一次由复合惯量求出。沿祖先链回传这股需求，就得到这一列对各祖先关节的影响。

装配第 j 列时，先取该节点的惯性力方向 H=I_j^CS_j。本关节和祖先关节依次对它投影：

这里 k 随父链向上移动，i 为遇到的可运动祖先。对称位置同时写入 M_{ji}=M_{ij}。遇到固定连接时继续运输，越过它后再投影。

若关节 i 是关节 j 的祖先，记沿这条路径的运动变换为 Y。只保留这两个关节速度时，子树根的速度为 YS_iv_i+S_jv_j。动能中 v_iv_j 的系数就是 M_{ij}=S_i^{\mathsf T}Y^{\mathsf T}I_j^CS_j；逐条父边回传惯性力方向，计算的正是这个系数。

数值装配：同一构型的三个矩阵元素

仍取逐项算例的 q=(\pi/6,\pi/3)^{\mathsf T}、v=(1,-0.5)^{\mathsf T} 和 g=9.81\,\mathrm{m/s^2}。CRBA 的质量矩阵与第七章解析式相同：

先看叶到根的惯量汇总：每次把子树的 I_j^C 变到父表示后相加。再看逐列装配：选定一列，先形成 H=I_j^CS_j，沿祖先链回传，并将 S_i^{\mathsf T}H 写入对应矩阵元素。切换分支树后，比较跨分支元素为何保持为零。

锁定子树的辅助运动用于计算当前构型下的惯性系数；实际运动仍可包含全部关节速度。每列所需的后代贡献已经保存在复合惯量中，所以装配时只需沿祖先链回传。图中的质量矩阵随后可用同一配置的单位加速度 RNEA 独立核对。

树结构决定可能出现的耦合项；某些构型还会让这些项恰好为零。多自由度关节使用矩阵子空间，相应对角项和耦合项成为小矩阵块，仍按同样的动能与投影关系计算。

给定关节驱动力后，前向动力学解 Ma=\tau-c_E。质量矩阵正定时，可先作平方根分解（Cholesky decomposition），再解两个三角方程：

例如取 M=\begin{bmatrix}4&1\\1&2\end{bmatrix}、右端 b=\tau-c_E=(1,3)^{\mathsf T}。令 L=\begin{bmatrix}\ell_{11}&0\\\ell_{21}&\ell_{22}\end{bmatrix}，直接乘开 LL^{\mathsf T}=M：

先解下三角方程，其第一行只含一个未知量；再将结果代入第二行：

转置三角方程从最后一行开始：

每一步都通过一个已经求出的量消去下一行中的已知部分。推广到任意大小的矩阵，就得到下面的求和形式。

先将重力和两个驱动力矩都设为零，检查加速度是否为零；再只给第一关节施力。由于 M_{12} 一般非零，第二关节也会响应。改变相对角，比较耦合曲线与求解后的两个加速度，同时展开 L 和中间解 y 检查两次三角求解。

求解残差 Ma+c-\tau 应接近零。两连杆共线时，末端位置雅可比可能退化，质量矩阵仍可保持正定，因为关节运动仍会改变有质量杆件的运动。这样便能把运动学奇异性与惯性方程的可解性分别判断。

物理惯量正定、广义速度相互独立且每个自由度影响有质量的刚体时，质量矩阵保持正定。末端位置雅可比的奇异性只涉及某个点的运动映射；两连杆共线时，总动能仍包含独立的刚体运动。

自由度按父节点在前编号时，一个关节只可能与自己及祖先、后代直接耦合。链上所有节点互为祖先或后代，质量矩阵通常稠密；不同分支之间的元素结构性为零。

线性消元会修改尚未处理的矩阵。若当前消元变量为 k，剩余子块按舒尔补（Schur complement）更新：

先消元叶节点时，它的剩余邻居都在同一祖先链上，原先已有可能耦合的位置；先消元共同祖先则会连接不同分支。MuJoCo 从后向前分解，利用这种树结构保留稀疏性。附加 tendon armature 等项可带来额外耦合，其存储模式也随模型扩展。

在星形树中先选择根节点先消元，观察原本不同分支间的零元素。当前示例的根对角为 4，两条连接系数均为 0.4，因此第一次更新会产生 0-0.4\times0.4/4=-0.04 的填充。再改成叶节点先消元，比较剩余邻居集合。

从叶节点开始时，剩余邻居位于同一祖先链，新的数值落在已有的结构位置。先消去共同祖先则会让多个分支互相连接，后续分解需要保存更多非零数。这说明树上的遍历顺序不仅决定计算先后，也决定矩阵因子的存储规模。

前面的每个刚体使用自己的坐标轴和连接点，因此每条边都需要 X_i 或 X_i^{\mathsf T}。MuJoCo 在计算动力学前，将同一运动学树的相关数据预先换到世界方向和共同参考点。接下来的加法发生在这一共同表示中。

共同参考点与六维字段

以下对照本地 MuJoCo 3.10.0，函数位于 \texttt{src/engine/engine\_core\_smooth.c}。\texttt{mj\_comPos} 先计算子树质心，再把各体惯量与运动轴写到世界方向，并把参考点选为

同一运动学树内的节点由根编号关联到同一个参考点。不同树各自使用对应的根子树质心。\texttt{cinert} 用十个数存放对称空间惯量，\texttt{cdof} 存放六维运动轴。铰链轴在共同点的线部由轴到参考点的位移产生。

统一表示需要同时保持功率和动能。令 Z_i 把本体的局部速度变到共同点、世界轴表示，则力用逆转置变换，惯量满足 \bar I_i=Z_i^{-\mathsf T}I_iZ_i^{-1}。因此局部递推中的每一次坐标运输，可以预先吸收到本体数据中；下式分别列出速度、力和惯量的对应。

速度、加速度与本体受力

\texttt{mj\_comVel} 复制父速度，再逐自由度加入 \texttt{cdof}\,v_i。普通单自由度分支用运动叉乘生成 \texttt{cdof\_dot}，为加速度递推提供速度乘积项。共同参考点随配置变化，读取这个字段时，应结合运输约定和实际叉乘构造理解。

mji_crossMotion(cdofdot + 6*j, cvel, d->cdof + 6*(bda+j));
mju_mulDofVec(tmp, d->cdof + 6*(bda+j), d->qvel + bda+j, 1);
mju_addTo(cvel, tmp, 6);

这是 \texttt{mj\_comVel} 中普通单自由度分支的连续语句。球关节的三个轴使用加入该关节速度前的同一父速度计算叉乘；自由关节的平移三项另有专门分支。

字段 cdof_dot 参与的是空间加速度递推中的速度项。共同参考点随机构运动时，数组普通时间导数还包含取样位置移动产生的变化。单个铰链就能检验两者的关系：父体固定时源码叉乘为零，而共同点上的轴线分量仍可能随构型改变。

\texttt{mj\_rne} 用 -g_W 初始化根加速度，累加轴加速度和速度乘积，计算单体力；随后回传并投影。在共同表示中，反向累加使用

if (j) {
  mju_addTo(loc_cfrc_body + 6*j, loc_cfrc_body + 6*i, 6);
}
// 以上摘自反向遍历；j 为父刚体编号。

该函数的 \texttt{flg\_acc}=0 用于刚体偏置计算。点外力由前向动力学流程另行投影加入平滑广义力；\texttt{mj\_rne} 的这段循环计算惯性、重力与速度贡献。

复合惯量与质量矩阵

\texttt{mj\_crb} 先复制 \texttt{cinert} 到 \texttt{crb}，直接累加子树惯量；然后按自由度祖先链装配质量矩阵。当前实现的 \texttt{d->M} 使用 \texttt{M\_rownnz/M\_rowadr/M\_colind} 描述稀疏下三角，每行对角位于行末。\texttt{mj\_makeM} 随后加入 tendon armature，并把数值散射到兼容布局 \texttt{qM}。

在共同表示中，同一列的惯性力方向可以直接与各祖先轴作六维内积。源码还会向质量矩阵加入关节、执行器和肌腱附加惯量；将程序结果与手算刚体矩阵比较时，需要把这些额外系数设置成相同的值。

M[Madr_ij] = dof_armature[i]
             + mj_actuatorArmature(m, mjOBJ_JOINT, m->dof_jntid[i]);
mju_mulInertVec(buf, crb + 10*dof_bodyid[i], cdof + 6*i);

for (int j=i; j >= 0; j=dof_parentid[j]) {
  M[Madr_ij--] += mji_dot6(cdof + 6*j, buf);
}

分解形式与求解次序

当前 \texttt{mj\_factorM} 将 \texttt{d->M} 复制到 \texttt{qLD}，调用 \texttt{mj\_factorI} 从后向前计算 M=L^{\mathsf T}DL。\texttt{mj\_solveM} 包装 \texttt{mj\_solveLD}，按转置三角、对角、三角的次序求解。

这里 L 是单位下三角，D 是正对角。前一节教学代码采用 LL^{\mathsf T} 的 Cholesky 形式，两者都通过分解和三角求解获得加速度。

算法原始资料可参阅 Featherstone 的 Spatial_v2 文档，其中 ID、HandC 与 FDcrb 对应逆动力学、动力学系数和复合刚体前向求解。引擎计算流程见 MuJoCo 官方计算文档；上述字段和调用以本地 3.10.0 源码为准。

配套模型由固定基座、固定轴铰链、滑动关节和固定连接组成。每个刚体给出父编号、零位姿、关节轴、质量、质心和质心惯量。自由度数组只为可运动的关节分配位置；固定连接仍参与运动运输、惯量合并与受力回传。

配套 tree_dynamics.py 复用前几章的位姿、空间代数和分解函数。输入、输出均为普通 Python 数组；矩阵按行展开。外力字典以刚体编号为键，值为局部原点处的六维空间力。

from math import pi
from kinematics import two_link_model
from tree_dynamics import rnea, crba, forward_dynamics

model = two_link_model()
q, v, a = [pi/6, pi/3], [1, -0.5], [0.4, -0.2]
tau = rnea(model, q, v, a)["tau"]
M = crba(model, q)["mass"]
recovered = forward_dynamics(model, q, v, tau)
print(tau, M, recovered)

默认重力为世界 (0,0,-9.81)^{\mathsf T}\,\mathrm{m/s^2}。这个例子重建 a=(0.4,-0.2)^{\mathsf T}\,\mathrm{rad/s^2}；将速度、加速度和外力设为零，可以读出静止姿态所需的重力力矩。

核心程序的输入是模型、配置、速度、指定加速度、重力与直接空间外力，输出为关节力数组。前向循环按父在前的顺序建立运动和本体力，反向循环把后代力合并并投影。保留中间数组，可把每行程序与 RNEA 实验中的同一步运算对应。

for i in range(1, len(model)):
    p, j = model[i].parent, dof[i]
    vj = scale(S[i], 0 if j is None else v[j])
    V[i] = add(matvec(X[i], V[p]), vj)
    A[i] = add(matvec(X[i], A[p]),
               add(scale(S[i], 0 if j is None else a[j]),
                   cross_motion(V[i], vj)))
    F[i] = sub(add(matvec(I[i], A[i]),
                   cross_force(V[i], matvec(I[i], V[i]))), E[i])

for i in range(len(model)-1, 0, -1):
    j, p = dof[i], model[i].parent
    if j is not None:
        tau[j] = dot(S[i], F[i])
    F[p] = add(F[p], matvec(transposed(X[i]), F[i]))

python3 notes/series_html/examples/tree_dynamics.py
python3 notes/series_html/examples/check_tree_dynamics.py

from tree_dynamics import mass_by_rnea
from analytic_dynamics import two_link_mass

reference = two_link_mass(q)
columns = mass_by_rnea(model, q)
composite = crba(model, q)["mass"]
assert max(abs(x-y) for x, y in zip(reference, columns)) < 1e-12
assert max(abs(x-y) for x, y in zip(reference, composite)) < 1e-12
tau = rnea(model, q, v, a)["tau"]
recovered = forward_dynamics(model, q, v, tau)
assert max(abs(x-y) for x, y in zip(a, recovered)) < 1e-12