牛顿-欧拉递推Recursive Newton–Euler

8.1 节用拉格朗日方程得到了动力学方程 \(M(\mathbf q)\ddot{\mathbf q}+C(\mathbf q,\dot{\mathbf q})\dot{\mathbf q}+\mathbf g(\mathbf q)=\boldsymbol\tau\)（第 8 章始终用 \(\mathbf q=(\theta_1,\dots,\theta_n)\) 表示关节坐标，偏置项记作 \(\mathbf b=C\dot{\mathbf q}+\mathbf g\)）。真实的机器人软件需要的是它的两种"算法形式"：

• 逆动力学 \(\boldsymbol\tau=\mathrm{ID}(\mathbf q,\dot{\mathbf q},\ddot{\mathbf q})\)：给定想要的运动，算出需要的力矩。力矩前馈、计算力矩控制（Part IV）、重力补偿、从关节电流估计接触力，用的都是它。
• 正动力学 \(\ddot{\mathbf q}=\mathrm{FD}(\mathbf q,\dot{\mathbf q},\boldsymbol\tau)\)：给定力矩，算出机器人会怎么动。物理仿真器每个时间步都在积分它。

7 自由度的机械臂要在 1 kHz 下算，30 多个自由度的人形机器人也一样。对一个 6 轴机械臂手推拉格朗日方程，\(M\)、\(C\) 的符号展开已经要写满好几页；自由度再多，项数增长得更快，没有人会把它们手抄进控制器。递推牛顿-欧拉算法（Luh–Walker–Paul，1980）的特点是：没有任何符号展开，每根连杆只做固定次数的向量运算，所以总计算量是 \(O(n)\)。

它的思想朴素到几乎是常识：每根连杆都是一个刚体，刚体满足 \(\mathbf F=m\mathbf a\) 和欧拉方程；连杆之间的相互作用力只通过关节传递。于是先向外传运动，再向内传力，一趟就算完。

Lesson 8.1 derived the equations of motion \(M(\mathbf q)\ddot{\mathbf q}+C(\mathbf q,\dot{\mathbf q})\dot{\mathbf q}+\mathbf g(\mathbf q)=\boldsymbol\tau\) from the Lagrange equations (throughout Chapter 8, \(\mathbf q=(\theta_1,\dots,\theta_n)\) collects the joint coordinates and \(\mathbf b=C\dot{\mathbf q}+\mathbf g\) is the bias term). Real robot software needs two algorithmic forms of them:

• Inverse dynamics \(\boldsymbol\tau=\mathrm{ID}(\mathbf q,\dot{\mathbf q},\ddot{\mathbf q})\): given the motion you want, compute the torques it takes. Torque feed-forward, computed-torque control (Part IV), gravity compensation and estimating contact forces from motor currents all use it.
• Forward dynamics \(\ddot{\mathbf q}=\mathrm{FD}(\mathbf q,\dot{\mathbf q},\boldsymbol\tau)\): given the torques, compute how the robot will move. A physics simulator integrates it at every time step.

A 7-joint arm must evaluate these at 1 kHz, and so must a humanoid with 30-odd joints. Expanding \(M\) and \(C\) symbolically for a 6-axis arm already fills pages, and the number of terms grows faster still with more joints; nobody copies that into a controller. The recursive Newton–Euler algorithm (Luh–Walker–Paul, 1980) has no symbolic expansion at all: every link costs a fixed number of vector operations, so the total cost is \(O(n)\).

The idea is almost common sense. Every link is a rigid body obeying \(\mathbf F=m\mathbf a\) and Euler's equation, and neighbouring links interact only through the joint between them. So pass the motion outward, then pass the forces inward, and you are done in one sweep each way.

先想一条鞭子。你抖动手腕（关节 1），整根鞭子的运动是一级一级传下去的：第 \(i\) 段的运动，等于第 \(i-1\) 段已经有的运动，再加上关节 \(i\) 自己多转的那一部分。这就是向前递推 (forward pass)，只涉及运动学：角速度 \(\omega_i\)、角加速度 \(\alpha_i\)、质心加速度 \(\mathbf a_{c_i}\)。

质心加速度怎么来的？7.4 节和 1.8 节的刚体速度公式再求一次导：刚体上离参考点 \(\mathbf r\) 处的一点，加速度是

Think of a whip. You flick your wrist (joint 1), and the motion travels down the whip one segment at a time: segment \(i\) does whatever segment \(i-1\) is already doing, plus whatever joint \(i\) adds by turning. This is the forward pass, and it is pure kinematics: angular velocity \(\omega_i\), angular acceleration \(\alpha_i\), and the acceleration \(\mathbf a_{c_i}\) of the link's centre of mass.

Where does the centre-of-mass acceleration come from? Differentiate the rigid-body velocity formula of Lessons 7.4 and 1.8 once more: for a point at offset \(\mathbf r\) from a reference point of a rigid body, the acceleration is

三项各有各的含义：继承项（参考点自己的加速度）、欧拉项（角加速度造成的切向加速度，垂直于 \(\mathbf r\)）、向心项（匀速转动也有，指向转轴）。下面的 FIG 1 用三根箭头把这三项接在一起，合成的琥珀色箭头就是质心加速度。点"下一步"，让递推从基座走到末端。

The three terms mean three different things: the inherited term (the reference point's own acceleration), the Euler term (tangential, caused by angular acceleration, perpendicular to \(\mathbf r\)) and the centripetal term (present even at constant spin, pointing at the axis). FIG 1 chains the three arrows head to tail, and the amber arrow they add up to is the centre-of-mass acceleration. Press "Next" to let the recursion walk from the base to the tip.

运动都知道了，质心加速度 \(\mathbf a_{c_i}\) 和角加速度 \(\alpha_i\) 已经在手里。现在反过来，从末端往基座走。取出第 \(i\) 根连杆，只看它受到的力：

• 关节 \(i\) 处，前一根连杆通过关节推它：力 \(\mathbf f_i\)，力矩 \(\boldsymbol\mu_i\)；
• 关节 \(i+1\) 处，后一根连杆反推它：由牛顿第三定律，是 \(-\mathbf f_{i+1}\) 和 \(-\boldsymbol\mu_{i+1}\)；
• 重力 \(m_i\mathbf g\) 作用在质心。

这条连杆做 \(\mathbf a_{c_i}\) 的运动，所以三个力之和必须等于 \(m_i\mathbf a_{c_i}\)：只有 \(\mathbf f_i\) 是未知的，立刻解出。力矩平衡同理解出 \(\boldsymbol\mu_i\)。而关节 \(i\) 的电机在转轴方向上出的力矩，正好就是 \(\tau_i=\mu_i\)。算完 \(\mathbf f_i,\boldsymbol\mu_i\)，它们就是下一根（更靠近基座的）连杆要用的 \(\mathbf f_{i+1},\boldsymbol\mu_{i+1}\)。末端是递推的起点：手爪对环境施加的力 \(\mathbf F_{\text{tip}}\) 就是 \(\mathbf f_{n+1}\)（空载时为 0）。

The motion is known: we hold every \(\mathbf a_{c_i}\) and \(\alpha_i\). Now walk the other way, from the tip toward the base. Cut out link \(i\) and look only at the forces on it:

• at joint \(i\), the previous link pushes on it: force \(\mathbf f_i\), moment \(\boldsymbol\mu_i\);
• at joint \(i+1\), the next link pushes back: by Newton's third law, \(-\mathbf f_{i+1}\) and \(-\boldsymbol\mu_{i+1}\);
• gravity \(m_i\mathbf g\) acts at the centre of mass.

The link moves with \(\mathbf a_{c_i}\), so the three forces must add up to \(m_i\mathbf a_{c_i}\): only \(\mathbf f_i\) is unknown, so solve for it at once. Moment balance gives \(\boldsymbol\mu_i\) the same way. The motor at joint \(i\) supplies, about the joint axis, exactly \(\tau_i=\mu_i\). Once \(\mathbf f_i,\boldsymbol\mu_i\) are known they play the role of \(\mathbf f_{i+1},\boldsymbol\mu_{i+1}\) for the next link inward. The tip starts the recursion: the force \(\mathbf F_{\text{tip}}\) that the gripper exerts on the environment is \(\mathbf f_{n+1}\) (zero when the arm carries nothing).

牛顿-欧拉从"力平衡"出发，拉格朗日从"能量"出发，两条路没有共同的中间步骤，却应当给出同一个 \(\boldsymbol\tau\)。FIG 3 把它们放在一起比：一边是 RNEA 的递推结果，另一边是完全独立写成的 \(M(\mathbf q)\ddot{\mathbf q}+C(\mathbf q,\dot{\mathbf q})\dot{\mathbf q}+\mathbf g(\mathbf q)\)，其中 \(M=\sum_i\big(m_iJ_{c_i}^\top J_{c_i}+I_iJ_{\omega_i}^\top J_{\omega_i}\big)\)（质心雅可比，8.3 节详细讲）、\(C\) 由 \(M\) 的偏导数（Christoffel 符号）得到、\(\mathbf g=\sum_i m_iJ_{c_i}^\top(0,g)^\top\)。右图把力矩拆成三块：惯性 \(M\ddot{\mathbf q}\)、速度项 \(C\dot{\mathbf q}\)、重力 \(\mathbf g\)，三块首尾相接，应当恰好落在 RNEA 的琥珀色菱形上。两连杆时还会和 8.1 节的闭式解比较。

Newton–Euler starts from force balance and Lagrange from energy; the two routes share no intermediate step, yet they must give the same \(\boldsymbol\tau\). FIG 3 sets them side by side: on one side the RNEA result; on the other a fully independent evaluation of \(M(\mathbf q)\ddot{\mathbf q}+C(\mathbf q,\dot{\mathbf q})\dot{\mathbf q}+\mathbf g(\mathbf q)\), with \(M=\sum_i\big(m_iJ_{c_i}^\top J_{c_i}+I_iJ_{\omega_i}^\top J_{\omega_i}\big)\) (centre-of-mass Jacobians, detailed in Lesson 8.3), \(C\) from the partial derivatives of \(M\) (Christoffel symbols) and \(\mathbf g=\sum_i m_iJ_{c_i}^\top(0,g)^\top\). The right panel splits the torque into three pieces: inertia \(M\ddot{\mathbf q}\), velocity terms \(C\dot{\mathbf q}\) and gravity \(\mathbf g\); chained head to tail they must land exactly on RNEA's amber diamond. For two links the closed form from Lesson 8.1 is checked as well.

RNEA 是 \(\boldsymbol\tau\) 的黑盒，而 \(\boldsymbol\tau=M\ddot{\mathbf q}+\mathbf b\)，其中 \(\mathbf b=C\dot{\mathbf q}+\mathbf g\) 与 \(\ddot{\mathbf q}\) 无关。令 \(\dot{\mathbf q}=0\)，再令 \(\ddot{\mathbf q}=\mathbf e_j\)（只有关节 \(j\) 单位加速），有 \(\mathrm{RNEA}(\mathbf q,0,\mathbf e_j)=M\mathbf e_j+\mathbf g\)，也就是 \(M\) 的第 \(j\) 列加上重力项；再算一次 \(\mathrm{RNEA}(\mathbf q,0,0)=\mathbf g\)，相减：

RNEA is a black box for \(\boldsymbol\tau\), and \(\boldsymbol\tau=M\ddot{\mathbf q}+\mathbf b\) with \(\mathbf b=C\dot{\mathbf q}+\mathbf g\) independent of \(\ddot{\mathbf q}\). Set \(\dot{\mathbf q}=0\) and \(\ddot{\mathbf q}=\mathbf e_j\) (unit acceleration of joint \(j\) only): \(\mathrm{RNEA}(\mathbf q,0,\mathbf e_j)=M\mathbf e_j+\mathbf g\), the \(j\)-th column of \(M\) plus the gravity term. One more call, \(\mathrm{RNEA}(\mathbf q,0,0)=\mathbf g\), and subtract:

这就是 Walker–Orin（1982）的想法：用 \(n\) 次（关掉重力就只要 \(n\) 次，开着重力是 \(n+1\) 次）\(O(n)\) 的 RNEA 调用拼出 \(M\)，再解线性方程 \(M\ddot{\mathbf q}=\boldsymbol\tau-\mathbf b\)，就得到正动力学，其中 \(\mathbf b=\mathrm{RNEA}(\mathbf q,\dot{\mathbf q},0)\)。FIG 4 一列一列地做这件事。

This is the Walker–Orin (1982) idea: assemble \(M\) from \(n\) calls of the \(O(n)\) RNEA (\(n\) if gravity is off, \(n+1\) with gravity on), then solve the linear system \(M\ddot{\mathbf q}=\boldsymbol\tau-\mathbf b\) with \(\mathbf b=\mathrm{RNEA}(\mathbf q,\dot{\mathbf q},0)\) and you have forward dynamics. FIG 4 does it column by column.

平面 \(n\) 连杆臂，所有转轴垂直于纸面，\(\theta_i\) 是关节角。第 \(i\) 根连杆：质量 \(m_i\)，质心处的转动惯量 \(I_i\)（均匀细杆是 \(m_il_i^2/12\)），\(\mathbf r_i\) 为关节 \(i\) 指向关节 \(i+1\) 的向量，\(\mathbf r_{c_i}\) 为关节 \(i\) 指向质心的向量。所有向量都写在世界系里。角量是标量，叉积 \(\mathbf a\times\mathbf b=a_xb_y-a_yb_x\) 也是标量。记 \(\omega_0=\alpha_0=0\)，\(\mathbf a_1\) 是关节 1（基座）的加速度，先取 \(\mathbf a_1=(0,\,g)\)（原因见下面的"重力技巧"）。

向前（\(i=1\to n\)）：

A planar \(n\)-link arm with every axis perpendicular to the page and joint angles \(\theta_i\). Link \(i\) has mass \(m_i\) and moment of inertia \(I_i\) about its centre of mass (\(m_il_i^2/12\) for a uniform rod); \(\mathbf r_i\) points from joint \(i\) to joint \(i+1\) and \(\mathbf r_{c_i}\) from joint \(i\) to the centre of mass. All vectors are written in the world frame. Angular quantities are scalars, and so is the cross product \(\mathbf a\times\mathbf b=a_xb_y-a_yb_x\). Take \(\omega_0=\alpha_0=0\), let \(\mathbf a_1\) be the acceleration of joint 1 (the base), and for now set \(\mathbf a_1=(0,\,g)\) (the reason is in "the gravity trick" below).

Forward (\(i=1\to n\)):

向后（\(i=n\to1\)）：起点 \(\mathbf f_{n+1}=\mathbf F_{\text{tip}}\)（手爪施加给环境的力），\(\mu_{n+1}=0\)。

Backward (\(i=n\to1\)): start from \(\mathbf f_{n+1}=\mathbf F_{\text{tip}}\) (the force the tip exerts on the environment) and \(\mu_{n+1}=0\).

来源。第 \(i\) 根连杆受到 \(\mathbf f_i\)（作用在关节 \(i\)）、\(-\mathbf f_{i+1}\)（作用在关节 \(i+1\)）和重力 \(m_i\mathbf g\)。牛顿第二定律：\(\mathbf f_i-\mathbf f_{i+1}+m_i\mathbf g=m_i\mathbf a^{\text{真实}}_{c_i}\)。把重力移到右边，\(\mathbf f_i-\mathbf f_{i+1}=m_i(\mathbf a^{\text{真实}}_{c_i}-\mathbf g)\)，括号里就是上面递推里的 \(\mathbf a_{c_i}\)（见下）。对质心取矩（欧拉方程）：\(I_i\alpha_i=\mu_i-\mu_{i+1}+(-\mathbf r_{c_i})\times\mathbf f_i+(\mathbf r_i-\mathbf r_{c_i})\times(-\mathbf f_{i+1})\)，移项就是上面的 \(\mu_i\)；重力过质心，不产生力矩。

重力技巧。令 \(\mathbf g=(0,-g)\)，把基座当作以 \(\mathbf a_1=-\mathbf g=(0,+g)\) 向上加速。这样每根连杆的质心加速度都多出一个 \(-\mathbf g\)，而 \(m_i(\mathbf a^{\text{真实}}_{c_i}-\mathbf g)\) 正好就是 \(m_i\mathbf a_{c_i}\)：重力不再需要单独处理。好像你在一部以 \(g\) 向上加速的电梯里，重力和"电梯加速"无法区分。

平行轴定理。递推用的是质心惯量 \(I_i\)。一根均匀杆绕自己端点的惯量是 \(ml^2/3=ml^2/12+m(l/2)^2\)，多出来的 \(m(l/2)^2\) 在递推里已经由叉积项 \(\mathbf r_{c_i}\times m_i\mathbf a_{c_i}\) 提供，不能再算一次。

检验。静止时（\(\omega=\alpha=0\)，\(\mathbf F_{\text{tip}}=0\)）\(\mathbf f_i=\sum_{k\ge i}m_k(0,g)\)，关节 \(i\) 要托住它外面所有连杆的重量；一根水平的均匀杆 \(\tau=\mathbf r_c\times\mathbf f=(l/2)\,mg\)。

Where it comes from. Link \(i\) feels \(\mathbf f_i\) (at joint \(i\)), \(-\mathbf f_{i+1}\) (at joint \(i+1\)) and gravity \(m_i\mathbf g\). Newton's second law: \(\mathbf f_i-\mathbf f_{i+1}+m_i\mathbf g=m_i\mathbf a^{\text{true}}_{c_i}\). Moving gravity to the right, \(\mathbf f_i-\mathbf f_{i+1}=m_i(\mathbf a^{\text{true}}_{c_i}-\mathbf g)\), and the bracket is the \(\mathbf a_{c_i}\) of the recursion above (see below). Euler's equation about the centre of mass: \(I_i\alpha_i=\mu_i-\mu_{i+1}+(-\mathbf r_{c_i})\times\mathbf f_i+(\mathbf r_i-\mathbf r_{c_i})\times(-\mathbf f_{i+1})\), which rearranges to the \(\mu_i\) above; gravity acts through the centre of mass and makes no moment.

The gravity trick. With \(\mathbf g=(0,-g)\), let the base accelerate upward at \(\mathbf a_1=-\mathbf g=(0,+g)\). Every link's centre of mass then picks up an extra \(-\mathbf g\), and \(m_i(\mathbf a^{\text{true}}_{c_i}-\mathbf g)\) is exactly \(m_i\mathbf a_{c_i}\): gravity no longer needs separate handling. It is the elevator argument: in a lift accelerating upward at \(g\), you cannot tell gravity from the acceleration.

Parallel-axis theorem. The recursion uses the inertia about the centre of mass, \(I_i\). A uniform rod about its end has \(ml^2/3=ml^2/12+m(l/2)^2\); the extra \(m(l/2)^2\) is already supplied by the cross-product term \(\mathbf r_{c_i}\times m_i\mathbf a_{c_i}\) and must not be counted a second time.

Sanity check. At rest (\(\omega=\alpha=0\), \(\mathbf F_{\text{tip}}=0\)), \(\mathbf f_i=\sum_{k\ge i}m_k(0,g)\): joint \(i\) holds up the weight of everything beyond it; for a horizontal uniform rod, \(\tau=\mathbf r_c\times\mathbf f=(l/2)\,mg\).

三维里角速度、速度、力、力矩都是三维向量，惯量是 \(3\times3\) 矩阵。把它们成对地装进 6 维向量，递推就和平面版长得一样了。约定（与全课一致）：twist \(\mathcal V=(\boldsymbol\omega,\mathbf v)\) 角部分在前，wrench \(\mathcal F=(\boldsymbol\mu,\mathbf f)\) 力矩在前。坐标系 \(\{0\}\) 固定在基座；\(\{i\}\) 固定在连杆 \(i\) 上，原点在它的质心；\(M_{i-1,i}\) 是零位 (\(\mathbf q=0\)) 时 \(\{i\}\) 相对 \(\{i-1\}\) 的位姿；\(\mathcal A_i\) 是关节 \(i\) 的螺旋轴在 \(\{i\}\) 系中的表示（5.10 节的 \(S_i\) 经 \(\mathrm{Ad}_{M_{0,i}^{-1}}\) 变到 \(\{i\}\) 中）。则

In 3D, angular velocity, velocity, force and moment are 3-vectors and the inertia is a \(3\times3\) matrix. Stack them in pairs into 6-vectors and the recursion looks just like the planar one. Conventions (as everywhere in this course): a twist \(\mathcal V=(\boldsymbol\omega,\mathbf v)\) has the angular part first, a wrench \(\mathcal F=(\boldsymbol\mu,\mathbf f)\) the moment first. Frame \(\{0\}\) is fixed to the base; \(\{i\}\) is fixed to link \(i\) with its origin at the centre of mass; \(M_{i-1,i}\) is the pose of \(\{i\}\) in \(\{i-1\}\) at \(\mathbf q=0\); \(\mathcal A_i\) is the screw axis of joint \(i\) expressed in \(\{i\}\) (Lesson 5.10's \(S_i\) carried into \(\{i\}\) by \(\mathrm{Ad}_{M_{0,i}^{-1}}\)). Then

其中 \(\mathcal F_{n+1}\) 是末端坐标系 \(\{n+1\}\) 中手爪对环境的 wrench，\(T_{n+1,n}\) 是常量。各项来源：

• \(\mathrm{Ad}_{T_{i,i-1}}\mathcal V_{i-1}\)：把前一根连杆的 twist 从 \(\{i-1\}\) 搬到 \(\{i\}\)（5.8 节的伴随）；\(\mathcal A_i\dot\theta_i\)：本关节自己多转出的 twist。这是平面里 \(\omega_i=\omega_{i-1}+\dot\theta_i\) 的 6 维版本。
• \(\mathrm{ad}_{\mathcal V}\)是 twist 的李括号矩阵：对 \(\mathcal V=(\boldsymbol\omega,\mathbf v)\) 和任意 twist \(\mathcal W\)，\([\mathcal V,\mathcal W]=\mathrm{ad}_{\mathcal V}\mathcal W\)， \(\mathrm{ad}_{\mathcal V}=\begin{bmatrix}[\boldsymbol\omega]_\times&0\\ [\mathbf v]_\times&[\boldsymbol\omega]_\times\end{bmatrix}\)。它也是伴随的导数：若 \(T(t)=e^{-[\mathcal A]\theta(t)}M\)，则 \(\tfrac{d}{dt}\mathrm{Ad}_T=-\dot\theta\,\mathrm{ad}_{\mathcal A}\mathrm{Ad}_T\)。
• \(\dot{\mathcal V}_i\) 的来源：把 \(\mathcal V_i\) 的式子对时间求导，\(\dot{\mathcal V}_i=\mathrm{Ad}_T\dot{\mathcal V}_{i-1}-\dot\theta_i\,\mathrm{ad}_{\mathcal A_i}\mathrm{Ad}_T\mathcal V_{i-1}+\mathcal A_i\ddot\theta_i\)。用 \(\mathrm{Ad}_T\mathcal V_{i-1}=\mathcal V_i-\mathcal A_i\dot\theta_i\)、\(\mathrm{ad}_{\mathcal A}\mathcal A=0\) 和 \(\mathrm{ad}_{\mathcal X}\mathcal Y=-\mathrm{ad}_{\mathcal Y}\mathcal X\)，得到 \(-\dot\theta_i\,\mathrm{ad}_{\mathcal A_i}\mathcal V_i=\mathrm{ad}_{\mathcal V_i}\mathcal A_i\dot\theta_i\)，这一项在平面里就是向心项（\(\hat z\times\) 的二次叠加）。
• \(\mathcal G_i\dot{\mathcal V}_i-\mathrm{ad}^\top_{\mathcal V_i}\mathcal G_i\mathcal V_i\)：单个刚体在自身质心系里的牛顿-欧拉方程。把 \(\mathcal V=(\boldsymbol\omega,\mathbf v)\) 代入，它等于 \((\mathcal I\dot{\boldsymbol\omega}+\boldsymbol\omega\times\mathcal I\boldsymbol\omega,\; m(\dot{\mathbf v}+\boldsymbol\omega\times\mathbf v))\)：第一项是欧拉方程（含陀螺项 \(\boldsymbol\omega\times\mathcal I\boldsymbol\omega\)），第二项是 \(m\mathbf a\)（体坐标里的真实加速度是 \(\dot{\mathbf v}+\boldsymbol\omega\times\mathbf v\)）。平面里陀螺项消失，只剩 \(I\alpha\) 与 \(m\mathbf a\)。
• \(\mathrm{Ad}^\top_{T_{i+1,i}}\mathcal F_{i+1}\)：把后一根连杆通过关节 \(i+1\) 传来的 wrench 换算到 \(\{i\}\) 中。为什么是伴随的转置？功率不变：\(\mathcal F_{i+1}^\top\mathcal V_{i+1}=\mathcal F_{i+1}^\top\mathrm{Ad}_{T_{i+1,i}}\mathcal V_i=(\mathrm{Ad}^\top\mathcal F_{i+1})^\top\mathcal V_i\)（7.6 节）。
• \(\tau_i=\mathcal F_i^\top\mathcal A_i\)：电机沿螺旋轴做功，\(\tau_i\dot\theta_i=\mathcal F_i^\top(\mathcal A_i\dot\theta_i)\)。

数值核对。我们把平面三连杆臂嵌入三维（转轴沿 \(z\)，连杆沿 \(x\) 放置，\(\mathcal A_i=(0,0,1,\,0,\,l_i/2,\,0)\)，\(\mathcal I_i=\mathrm{diag}(0,0,m_il_i^2/12)\)，平面运动只用到 \(zz\) 分量），随机抽取 300 个状态和手爪力，空间递推与平面递推的 \(\boldsymbol\tau\) 在 \(3\times10^{-14}\) 内一致；平面递推又与独立的拉格朗日公式（FIG 3）在同样的量级内一致。

树与浮动基座。树结构只要把"上一根"换成"父连杆"，向后一步对所有子连杆的 \(\mathrm{Ad}^\top\mathcal F\) 求和。8.3 节会看到 \(M(\mathbf q)=\sum_iJ_i^\top\mathcal G_iJ_i\)（\(J_i\) 是连杆 \(i\) 质心系的体雅可比）。

Here \(\mathcal F_{n+1}\) is the wrench the tip exerts on the environment, written in the tip frame \(\{n+1\}\), and \(T_{n+1,n}\) is constant. Where each term comes from:

• \(\mathrm{Ad}_{T_{i,i-1}}\mathcal V_{i-1}\) carries the previous link's twist from \(\{i-1\}\) into \(\{i\}\) (the adjoint of Lesson 5.8); \(\mathcal A_i\dot\theta_i\) is the extra twist from this joint's own motion. Together they are the 6-D version of \(\omega_i=\omega_{i-1}+\dot\theta_i\).
• \(\mathrm{ad}_{\mathcal V}\) is the Lie-bracket matrix of twists: for \(\mathcal V=(\boldsymbol\omega,\mathbf v)\) and any twist \(\mathcal W\), \([\mathcal V,\mathcal W]=\mathrm{ad}_{\mathcal V}\mathcal W\), with \(\mathrm{ad}_{\mathcal V}=\begin{bmatrix}[\boldsymbol\omega]_\times&0\\ [\mathbf v]_\times&[\boldsymbol\omega]_\times\end{bmatrix}\). It is also the derivative of the adjoint: if \(T(t)=e^{-[\mathcal A]\theta(t)}M\), then \(\tfrac{d}{dt}\mathrm{Ad}_T=-\dot\theta\,\mathrm{ad}_{\mathcal A}\mathrm{Ad}_T\).
• Origin of \(\dot{\mathcal V}_i\): differentiate the \(\mathcal V_i\) equation in time, \(\dot{\mathcal V}_i=\mathrm{Ad}_T\dot{\mathcal V}_{i-1}-\dot\theta_i\,\mathrm{ad}_{\mathcal A_i}\mathrm{Ad}_T\mathcal V_{i-1}+\mathcal A_i\ddot\theta_i\). Using \(\mathrm{Ad}_T\mathcal V_{i-1}=\mathcal V_i-\mathcal A_i\dot\theta_i\), \(\mathrm{ad}_{\mathcal A}\mathcal A=0\) and \(\mathrm{ad}_{\mathcal X}\mathcal Y=-\mathrm{ad}_{\mathcal Y}\mathcal X\) gives \(-\dot\theta_i\,\mathrm{ad}_{\mathcal A_i}\mathcal V_i=\mathrm{ad}_{\mathcal V_i}\mathcal A_i\dot\theta_i\). In the plane this is the centripetal term (\(\hat z\times\) applied twice).
• \(\mathcal G_i\dot{\mathcal V}_i-\mathrm{ad}^\top_{\mathcal V_i}\mathcal G_i\mathcal V_i\) is the Newton–Euler equation of a single rigid body in its own centre-of-mass frame. Substituting \(\mathcal V=(\boldsymbol\omega,\mathbf v)\) it equals \((\mathcal I\dot{\boldsymbol\omega}+\boldsymbol\omega\times\mathcal I\boldsymbol\omega,\; m(\dot{\mathbf v}+\boldsymbol\omega\times\mathbf v))\): the first part is Euler's equation (with the gyroscopic term \(\boldsymbol\omega\times\mathcal I\boldsymbol\omega\)), the second is \(m\mathbf a\) (the true acceleration in body coordinates is \(\dot{\mathbf v}+\boldsymbol\omega\times\mathbf v\)). In the plane the gyroscopic term vanishes, leaving \(I\alpha\) and \(m\mathbf a\).
• \(\mathrm{Ad}^\top_{T_{i+1,i}}\mathcal F_{i+1}\) converts the wrench arriving through joint \(i+1\) into frame \(\{i\}\). Why the transpose of the adjoint? Power is invariant: \(\mathcal F_{i+1}^\top\mathcal V_{i+1}=\mathcal F_{i+1}^\top\mathrm{Ad}_{T_{i+1,i}}\mathcal V_i=(\mathrm{Ad}^\top\mathcal F_{i+1})^\top\mathcal V_i\) (Lesson 7.6).
• \(\tau_i=\mathcal F_i^\top\mathcal A_i\): the motor does work along the screw axis, \(\tau_i\dot\theta_i=\mathcal F_i^\top(\mathcal A_i\dot\theta_i)\).

Numerical check. We embedded the planar three-link arm in 3D (axes along \(z\), links laid along \(x\), \(\mathcal A_i=(0,0,1,\,0,\,l_i/2,\,0)\); only the \(zz\) entry of \(\mathcal I_i=m_il_i^2/12\) matters for planar motion) and drew 300 random states and tip forces: the spatial and planar recursions give the same \(\boldsymbol\tau\) to \(3\times10^{-14}\), and the planar one agrees with the independent Lagrangian formulas (FIG 3) to the same order.

Trees and floating bases. For a tree, replace "previous link" by "parent" and let the backward step sum \(\mathrm{Ad}^\top\mathcal F\) over all children. Lesson 8.3 shows that \(M(\mathbf q)=\sum_iJ_i^\top\mathcal G_iJ_i\), with \(J_i\) the body Jacobian of link \(i\)'s centre-of-mass frame.

仿真需要的是正动力学。把 FIG 4 的办法接上 2.5 节的 ODE 积分：每个时间步里，先用 RNEA 算出偏置项 \(\mathbf b=\mathrm{RNEA}(\mathbf q,\dot{\mathbf q},0)=C\dot{\mathbf q}+\mathbf g\)，再逐列拼出 \(M\)，解 \(M\ddot{\mathbf q}=\boldsymbol\tau-\mathbf b\)，然后用四阶 Runge–Kutta 积分。电机不出力（\(\boldsymbol\tau=0\)，可加粘性摩擦 \(-b\dot{\mathbf q}\)），手臂在重力下自由摆动。无摩擦的保守系统总能量 \(E=T+V\) 应当不变，漂移的大小反映积分器的误差，是对整个动力学模型和积分器的一次体检。

A simulator needs forward dynamics. Combine FIG 4's trick with the ODE integration of Lesson 2.5: at every time step compute the bias \(\mathbf b=\mathrm{RNEA}(\mathbf q,\dot{\mathbf q},0)=C\dot{\mathbf q}+\mathbf g\) with RNEA, assemble \(M\) column by column, solve \(M\ddot{\mathbf q}=\boldsymbol\tau-\mathbf b\), and integrate with fourth-order Runge–Kutta. The motors apply no torque (\(\boldsymbol\tau=0\), optionally with viscous friction \(-b\dot{\mathbf q}\)) and the arm swings freely under gravity. Without friction the total energy \(E=T+V\) of this conservative system must stay constant, so its drift measures the integrator error: a health check of the dynamics model and the integrator together.

FIG 6 数出了这一页上几种实现的乘加次数（把每个 rnea、lagr 内部的标量运算累加起来，这是一个计数模型，不是计时 benchmark；真实库的常数项完全不同，但增长的阶不变）。四条线：① RNEA 求逆动力学；② 调用 \(n\) 次 RNEA 拼出 \(M\)；③ 在 ② 基础上再解线性方程，得到正动力学；④ 先显式算 \(M\)、\(\partial M/\partial\mathbf q\)、\(C\)、\(\mathbf g\)，再相乘求逆动力学。④ 的阶是 \(n^4\)（单独算 \(M\) 是 \(n^3\)），①是 \(n\)。链越长，差距越大。

FIG 6 counts the multiply-adds of several implementations on this page (the scalar operations inside each rnea and lagr call are tallied up). It is a count model, not a timing benchmark: real libraries have entirely different constants, but the growth rates are the same. Four curves: ① RNEA for inverse dynamics; ② \(n\) RNEA calls to assemble \(M\); ③ ② plus a linear solve, i.e. forward dynamics; ④ explicit \(M\), \(\partial M/\partial\mathbf q\), \(C\), \(\mathbf g\) and then the products, for inverse dynamics. ④ grows like \(n^4\) (building \(M\) alone is \(n^3\)), ① like \(n\). The longer the chain, the wider the gap.