多体动力学方程与 Gauss 原理

若直接对多体系统中的每个刚体列出三维 Newton–Euler 方程，方程中会同时出现大量未知的关节内力（如铰链轴承反力），需要联立消元才能求得关节运动。反之，若只考察符合关节连接关系的微小允许位置变化，由于理想无摩擦关节的约束反力垂直于允许运动方向，它们在这些方向上所做的功会自动抵消为零。单摆提供了最直观的原型：刚性轻杆锁死了径向距离，摆角 q 则完整刻画了剩下的切向允许运动。

第六章式（6.5）曾利用瞬时功率相等导出了雅可比转置力映射；若将瞬时速度替换为同一时刻无穷小的允许虚位移，即可从虚功角度导出相同的广义力表达式，并自然推广到包含惯性力的完整动力学方程。

考察一个质量为 m 的质点摆球，通过长度为 L 的刚性无质量轻杆连接在世界原点处的固定铰链上。设单摆在世界 xz 平面内运动，零角时竖直朝下，正摆角 q 向正横轴方向抬起，则摆球的世界坐标为：

在保持时间冻结的同一瞬间，设想摆角发生一个任意无穷小增量 \delta q。由此引起的一阶允许位置变化称为虚位移（virtual displacement）：

可以看到，虚位移 \delta\boldsymbol x 严格沿着圆周切线方向，一阶保持杆长不变。当摆球受到外力 \boldsymbol f 作用时，该力在虚位移 \delta\boldsymbol x 上所做的虚功（virtual work）为：

其中比例系数 \tau=J_p^{\mathsf T}\boldsymbol f 即为外力 \boldsymbol f 在摆角自由度上产生的广义力（此处为关于铰链的广义力矩）。由于轻杆施加的径向张力始终沿着杆身方向，与圆周切向虚位移 \delta\boldsymbol x 严格正交，因此径向张力对摆角 q 的广义力矩恒为零——张力的物理作用是提供径向向心加速度以维持圆周轨迹，而不会直接推动摆角加速或减速。

在下方实验前，可先在固定正摆角 q>0 下对比径向拉力与竖直重力：重力具有指向最低点的切向分量，因此给出负的恢复广义力矩；而无论把径向拉力调得多大，只要它严格沿着杆身方向，其与切线的内积就始终为零。

从图中可以清楚看到，广义力 \tau=J_p^{\mathsf T}\boldsymbol f 只提取外力沿允许切线方向的有效分量。利用这一正交分解，我们得以先在切向子空间中独立解出摆角的角加速度，随后再代入完整二维牛顿方程反算出维持圆周所需的径向杆张力。一般多刚体系统的运动学树动力学正是建立在同一投影思想之上。

当一般多刚体机构上同时作用有多个外力与力偶时，只需将各受力点的贡献沿同一组独立切空间虚位移 \delta\xi\in\mathbb R^{n_v} 累加。利用第六章式（6.4）的线速度雅可比 J_{p,k} 与角速度雅可比 J_{r,k}，所有外力系在虚位移 \delta\xi 下所做的总虚功及合成广义力 \tau\in\mathbb R^{n_v} 为：

由于树形关节的几何连接条件已经内嵌在允许虚位移 \delta\xi 的生成过程之中，各关节轴承处的成对理想内力在总虚功中自动正负抵消，只有重力、主动力矩、弹簧阻尼及外部接触力会保留在广义力之中。

第 1 节将静外力沿允许虚位移投影得到了广义力。在动力学过程中，每个质量微元不仅受外力作用，还具有加速度 \boldsymbol a_k；若将达朗贝尔惯性力 -m_k\boldsymbol a_k 与外力一并向每个独立关节坐标的允许方向投影，所有理想连接内力同样会自动消去。进一步利用系统总动能 T 整理其中反复出现的速度与加速度系数，即可由牛顿定律直接导出多体系统的 Lagrange 方程。

我们先考察采用独立标量关节坐标（v_i=\dot q_i）的系统。将各刚体看作由质量微元 m_k 组成的集合，每个微元的世界位置 \boldsymbol x_k(q) 由广义坐标 q 唯一确定。根据牛顿第二定律，微元所受的主动外力 \boldsymbol f_k、理想约束内力与惯性力 -m_k\boldsymbol a_k 之和为零；将其与满足几何连接条件的任意虚位移 \delta\boldsymbol x_k 作内积并对全部微元求和，理想内力的总虚功抵消为零，从而得到达朗贝尔原理（d’Alembert’s principle）：

上式中 \boldsymbol f_k 仅包含重力、电机驱动力与外部环境力等主动力；对于连续刚体，只需将离散求和替换为对质量微元 \mathrm dm 的体积分，全部推导过程完全相同。

记系统的总动能为 T(q,v)，重力等保守有势力的势能为 U(q)，其余非保守主动外力投影所得的广义力为 \tau。通过将惯性力投影项 \sum_k m_k\boldsymbol a_k^{\mathsf T}(\partial\boldsymbol x_k/\partial q_i) 改写为系统总动能偏导数的组合，达朗贝尔原理即化为第二类 Lagrange 方程：

下方推导在位置映射 \boldsymbol x_k(q) 二阶光滑且不显含时间的定常完整约束假设下，完整展示了如何利用微分换序恒等式把加速度内积转化为动能对速度与位置的偏导数。

在物理引擎的数值仿真中，每个时间步已知当前时刻的构型 q 与广义速度 v，需要求解广义加速度 a=\dot v 以推进状态。为此，我们将 Lagrange 方程（7.1）中所有线性依赖于广义加速度 a 的项提取合并为一个矩阵乘积 M(q)a，而将仅由当前状态 (q,v) 决定的速度二次项（科氏力、离心力）与重力项归并为一个偏置向量 c(q,v)。这种标准矩阵形式不仅结构清晰，而且同一质量矩阵分解还能直接复用于后续的约束力求解。

在给定构型 q 下，由于各刚体的质心线速度与角速度均为广义速度 v 的齐次线性函数，系统的总动能 T(q,v) 恒为关于广义速度 v 的齐次二次型：

其中 n_v\times n_v 对称矩阵 M(q) 称为系统的广义质量矩阵（generalized mass matrix，或关节空间惯量矩阵），它将广义加速度 a 映射为惯性广义力。将式（7.1）展开并按加速度线性项与状态偏置项整理，即得到多体系统的标准二阶动力学方程：

按照本系列与 MuJoCo 的约定，重力项与科氏/离心速度二次项统一包含在偏置力（bias force）c(q,v) 之中，而关节弹簧、阻尼、执行器驱动力以及外部施加力则汇总在右端的广义力 \tau 之中。

利用第六章建立的各刚体质心线速度雅可比 J_{p,i}（满足 \boldsymbol v_{C_i}=J_{p,i}v）与角速度雅可比 J_{r,i}（满足 \boldsymbol\omega_i=J_{r,i}v），我们无需写出动能的标量展开式，就能直接由各刚体的质量 m_i 与世界系质心转动惯量 I_{C_i,W}=R_iI_{C_i}R_i^{\mathsf T} 组装出整个系统的广义质量矩阵：

上式中每一项都直接源自第四章式（4.2）的刚体平动动能与转动动能二次型。只要每个活动刚体具有正质量与正定转动惯量，且不存在驱动零质量虚构节点的退化自由度，则任意非零广义速度 v\ne\boldsymbol0 必使至少一个刚体产生严格为正的动能 T=\tfrac12v^{\mathsf T}Mv>0。因此，广义质量矩阵 M(q) 恒为对称正定矩阵，在任意构型下均可逆，从而保证无约束正动力学方程 Ma=\tau-c 存在唯一解。

在物理意义上，质量矩阵的对角元 M_{ii} 反映了第 i 个关节单独加速时所感受到的等效惯量，而非对角元 M_{ij}\ (i\ne j) 则刻画了第 j 个关节加速时通过共同推动下游连杆质量而在第 i 个关节上引起的惯性耦合（inertial coupling）。当机构同时包含转动关节与移动关节时，M_{ij} 的物理量纲也随行、列对应的自由度类型（\mathrm{kg\,m^2}、\mathrm{kg\,m} 或 \mathrm{kg}）相应改变。

现在将 Lagrange 方程与广义质量矩阵具体应用到第六章的平面双连杆机构上。两根均匀长方体连杆的长度、质量、截面与重力参数均沿用同一份共享模型数据：第一关节角 q_1 为相对于竖直下方的绝对转角，第二关节角 q_2 为相对于第一杆的相对转角。记各杆质心到自身上游铰链的距离为 r_i=L_i/2，绕各自质心负纵轴的主转动惯量为 I_i；同时在 xz 运动平面内定义径向单位向量 \boldsymbol e(\varphi)、切向单位向量 \boldsymbol t(\varphi)=\boldsymbol e'(\varphi) 以及第二杆的绝对转角 s=q_1+q_2：

由此可简洁写出两杆质心的平面坐标及其对时间求导所得的质心线速度：

由于两根连杆在平面内的绝对角速率分别为 v_1 与 v_1+v_2，系统的总动能 T 为两杆质心平动动能与绕质心转动动能之和，而总重力势能 U 则由两杆质心的竖直高度决定：

为了将质量矩阵与构型的依赖关系清晰分离，定义如下三个仅由连杆几何与质量分布决定的常系数（量纲均为 \mathrm{kg\,m^2}）：

整理动能二次型即可发现，平面双连杆的 2\times2 广义质量矩阵 M(q) 仅依赖于第二关节的相对夹角 q_2，而与第一关节角 q_1 无关：

接着计算式（7.2）中的偏置力 c(q,v)。记耦合系数 h=B\sin q_2，重力广义力项记为 G(q)=\partial U/\partial q，则双连杆的科氏/离心速度二次项与重力项之和为：

由于质量矩阵 M(q) 仅通过 \cos q_2 依赖于构型，其对位置与时间的导数全部归结为同一个因子 h=B\sin q_2。下方推导逐行展开了 Lagrange 方程中的动量时间导数与动能位置偏导数。

在下方实验中，默认初值即为上述例题状态，可先核对面板显示的初始质量矩阵、偏置力与角加速度读数；随后点击播放观察无驱动自由摆动过程：随着相对转角 q_2 不断变化，质量矩阵的耦合元 M_{12} 与对角元 M_{11} 实时改变，动能 T 与重力势能 U 发生此消彼长的周期性转换。

可以看到，由于非对角耦合项 M_{12} 与科氏/离心项的存在，两根连杆在摆动中持续交换动量与能量。在无外力矩与阻尼的保守条件下，连续动力学方程严格保持总机械能 E=T+U 守恒；因此除了逐帧检查瞬时方程 Ma+c=\boldsymbol0 外，监测整条轨迹上的总机械能漂移也是检验动力学实现正确性的重要手段（图中动画采用时间步长为 0.005\,\mathrm s 的四阶 Runge–Kutta 积分，读数中极微小的尾数波动源于有限步长离散截断误差）。

在前面的平面双连杆中，两个广义速度恰好是两个标量关节角的时间导数（v=\dot q）。而当机构中包含第六章第 8 节介绍的球关节或自由关节时，姿态由 4 维单位四元数表示，广义速度 v 则取为 3 维局部体坐标系角速度 \boldsymbol\omega_b，并通过 \dot q=N(q)v 驱动四元数变化。由于表达角速度的体坐标基向量本身随刚体一起旋转，这种“移动坐标基”效应同样会进入动力学偏置力 c(q,v)——第四章 Euler 转动方程中的陀螺力矩项 \boldsymbol\omega_b\times(I_C\boldsymbol\omega_b) 正是由此产生。

因此，对于包含球关节或自由关节的一般多体系统，尽管位置配置满足 \dot q=N(q)v，切空间上的二阶动力学方程依然保持标准形式 M(q)a+c(q,v)=\tau，其中 n_v\times n_v 质量矩阵仍由总动能二次型 T=\tfrac12v^{\mathsf T}M(q)v 唯一确定。

为了从变分与虚功角度看清移动坐标基如何生成偏置力，不妨考察绕固定质心转动的单个刚体：在随体坐标系下，刚体的转动惯量矩阵 I_C 为常矩阵，转动动能与 Euler 方程分别为：

虽然此处 I_C 不随姿态改变，但因为角速度 \boldsymbol\omega_b 是在随体旋转的非坐标基下度量，当对姿态路径施加无穷小局部转角变分 \boldsymbol\eta(t)（满足两端点时刻为零）时，体角速度的诱导变分并非简单的 \dot{\boldsymbol\eta}，而是多出一项由体坐标轴旋转引起的叉乘修正 \delta\boldsymbol\omega_b=\dot{\boldsymbol\eta}+\boldsymbol\omega_b\times\boldsymbol\eta。下方推导展示了将该变分关系代入达朗贝尔时间积分后如何直接生出陀螺力矩项 \boldsymbol\omega_b\times(I_C\boldsymbol\omega_b)。

运动学树中的关节连接已经通过广义坐标 q 自动满足。然而在实际场景中，机构还可能存在闭环连杆（如并联机构或双手协同抓取同一物体）、齿轮耦合或末端沿指定轨道运动等树外附加几何约束。为了在二阶动力学方程 Ma+c=\tau 中求解维持这些约束所需的约束力，必须将位置层面的几何约束关于时间连续求导两次，转化为对广义加速度 a 的线性等式约束。

设树外闭环、点重合或距离约束由一组光滑位置残差方程 r(q)=\boldsymbol0\in\mathbb R^m 描述。按第六章第 8 节的链式法则定义 m\times n_v 的约束雅可比矩阵 J(q)=(\partial r/\partial q)N(q)，沿系统运动轨迹对 r(q(t)) 连续求一阶与二阶时间导数：

因此，一条严格保持定常位置约束 r(q)\equiv\boldsymbol0 的光滑运动轨迹，必须在位置、速度与加速度三个层次上同时满足相容条件：

特别需要注意加速度层面的右端项 b=-\dot Jv：即使当前时刻的位置 r=\boldsymbol0 与速度 Jv=\boldsymbol0 已经完全符合约束流形，只要约束曲面是弯曲的（\dot J\ne0）且系统处于运动中（v\ne\boldsymbol0），广义加速度 a 就必须提供非零的法向曲率分量 -\dot Jv 以防止轨迹沿切线飞离约束曲面。平面圆周运动为这一几何曲率项提供了最清晰的直观解释。

考察平面内受定长约束 \|\boldsymbol x\|=L 限制在圆周上运动的质点：在任意位置 \boldsymbol x 处，允许速度 \boldsymbol u 沿着圆周切线方向（\boldsymbol x^{\mathsf T}\boldsymbol u=0）；但若质点的切向速率为 u=\|\boldsymbol u\|>0，要使轨迹继续弯曲留在圆周上，其加速度 \boldsymbol a_P 就必须包含指向圆心的向心加速度 -u^2\boldsymbol x/L^2，对应于线性约束条件 \boldsymbol x^{\mathsf T}\boldsymbol a_P=-u^2。下方推导通过对半平方距离残差连续求导两次，直接导出了这一右端项。

更一般地，若约束支承点本身按已知时间规律运动，位置残差显含时间 t，写作 r(q,t)=\boldsymbol0。此时固定时刻的允许虚位移依然满足齐次条件 J\delta\xi=\boldsymbol0，而真实速度与加速度的约束条件则需计入支承自身的时间偏导项 r_t=\partial r/\partial t：

例如，当圆周约束的圆心按给定轨迹 \boldsymbol c(t) 移动时，记质点相对于圆心的径向向量为 \boldsymbol\rho=\boldsymbol x-\boldsymbol c(t)，相对速度为 \boldsymbol u-\dot{\boldsymbol c}，则约束雅可比为 J=\boldsymbol\rho^{\mathsf T}，而加速度约束 J\boldsymbol a_P=b 的右端项变为：

式中第一项 \boldsymbol\rho^{\mathsf T}\ddot{\boldsymbol c} 负责跟随移动圆心的牵连加速度，第二项 -\|\boldsymbol u-\dot{\boldsymbol c}\|^2 则负责提供相对于移动圆心做圆周运动的向心曲率；当圆心静止（\dot{\boldsymbol c}=\ddot{\boldsymbol c}=\boldsymbol0）时自然退化回固定圆心公式。

在下方实验中，可依次切换位置、速度与加速度三个视图，并在“固定圆心”与“移动圆心”之间对比：在速度视图中，必须先从质点绝对速度 \boldsymbol u 中减去圆心牵连速度 \dot{\boldsymbol c}，所得相对速度才与半径 \boldsymbol\rho 正交；而在加速度视图中，质点加速度沿半径的投影严格等于右端项 b/\|\boldsymbol\rho\|。

图中圆周半径取为 L=1\,\mathrm m，相对转角按匀角加速规律 \theta(t)=\theta_0+\omega_0t+\tfrac12\alpha t^2 演化，移动圆心轨迹则取为 \boldsymbol c(t)=(0.3t^2,0)^{\mathsf T}\,\mathrm m（时间单位为 \mathrm s）。由于每一帧的位置、速度与加速度均由同一解析轨迹逐阶求导生成，三层约束方程在任意时刻均精确闭合。

第 6 节导出的线性约束方程 Ja=b 通常只限制了广义加速度的某些组合方向：例如用轻杆连接两个同向运动的滑块以保持固定间距时，约束只要求两滑块加速度相等（a_1-a_2=0），而并未单独规定共同加速度的具体数值。要从无穷多组满足几何约束的可行加速度中唯一确定真实的物理响应，动力学必须引入一个与各刚体惯性相容的选择准则——质量矩阵度量下的最小修正原理。

具体而言，式（7.2）首先给出了无附加约束时的自由（平滑）广义加速度 a_u=M^{-1}(\tau-c)，而式（7.3）的 Ja=b 则在加速度空间中划出了一个可行仿射子空间。高斯最小拘束原理（Gauss’s principle of least constraint，以下简记为 Gauss 原理）指出：受理想约束作用的真实广义加速度 a，是可行集合 \{a\mid Ja=b\} 中到自由加速度 a_u 的质量矩阵加权二次距离最小的那一点：

从物理上看，令惯量越大的刚体发生相同的加速度偏离需要施加越大的约束力；因此 Gauss 代价函数 \Phi(a) 用广义质量矩阵 M(q) 对加速度偏差 a-a_u 加权，并完整保留了各自由度之间的惯性耦合（\Phi 的物理量纲为 \mathrm{kg\,m^2/s^4}=\mathrm{N\,m/s^2}）。

引入拉格朗日乘子 \lambda（代表约束力系数，对应的广义约束力为 \tau_c=J^{\mathsf T}\lambda），写出等式约束二次规划（7.4）的一阶最优性条件与消元后的乘子方程：

其中对称矩阵 A=JM^{-1}J^{\mathsf T} 将约束力冲量/力系数 \lambda 直接映射为约束空间中的加速度响应，称为约束空间逆惯量矩阵或 Delassus 算子（Delassus operator）。先由 A\lambda=b-Ja_u 解出维持约束所需的乘子 \lambda，再将其引起的加速度修正叠加回自由加速度 a_u，即得到最终的受约束广义加速度：

由于质量矩阵 M 对称正定，只要线性约束 Ja=b 相容可行，严格凸二次函数 \Phi(a) 在可行仿射子空间上就一定存在唯一极小点 a；若进一步有约束雅可比 J 行满秩（各约束行线性无关），则 Delassus 矩阵 A=JM^{-1}J^{\mathsf T} 亦严格对称正定，约束力乘子 \lambda 也可唯一求解。

在下方实验中，默认参数即为 m_1=2\,\mathrm{kg},\ m_2=1\,\mathrm{kg}，可行集为对角直线 a_1=a_2。可在左侧加速度平面上观察以 a_u=(2,-1)^{\mathsf T}\,\mathrm{m/s^2} 为中心的椭圆等值线簇如何与可行直线相切于 (1,1)^{\mathsf T}\,\mathrm{m/s^2}；再拖动可行直线上的候选点，在右侧一维代价曲线上确认该相切点正是 Gauss 代价 \Phi(a) 的唯一最低点。

若在图中将第一滑块质量调为 m_1=1\,\mathrm{kg}（与第二滑块等质量），此时质量矩阵变为单位矩阵的倍数，椭圆等值线退化为同心圆，质量度量投影点便与普通欧氏正交投影点完全重合。为了便于纯粹比较不同质量比对投影方向的影响，图中在调节 m_1 时同步缩放了第一滑块的驱动力以固定自由加速度中心 a_u=(2,-1)^{\mathsf T}\,\mathrm{m/s^2}。

第 7 节的理想硬约束要求加速度严格满足 Ja=b；而在处理带有柔顺性的连接或存在冗余冲突的约束时，MuJoCo 采用软约束（soft constraint）模型，改用有限的二次惩罚项来度量实际约束加速度 Ja 偏离参考目标加速度 a_{\mathrm{ref}} 的代价。设对角正定矩阵 R 为约束正则化矩阵（regularization matrix），软双边等式约束的增广 Gauss 目标函数定义为：

当正则矩阵 R 正定时，目标偏差 a_{\mathrm{ref}}-Ja 通过柔度逆矩阵 R^{-1} 直接转化为有限的恢复约束力系数 \lambda=R^{-1}(a_{\mathrm{ref}}-Ja)。将其与一阶最优性条件 M(a-a_u)=J^{\mathsf T}\lambda 联立，即可消去 a 得到正则化对偶力方程：

对于双边等式约束，乘子 \lambda 的正负号不受限制；正则量 R 越大，约束表现得越“软”，相同加速度偏差所激发的约束恢复力越小。当 R\to0^+ 时，式（7.5）连续退化回第 7 节的理想硬约束模型。

在下方实验中，保持第 7 节两个滑块的质量与驱动力不变，将正则系数 R 从零逐步调大：可以观察到两滑块的相对加速度 Ja=a_1-a_2 从硬约束目标 a_{\mathrm{ref}}=0 平滑过渡向无约束自由响应 Ja_u=3\,\mathrm{m/s^2}，且在每一个参数点处都严格满足 M(a-a_u)=J^{\mathsf T}\lambda 与 a_{\mathrm{ref}}-Ja=R\lambda。

例如当取 a_{\mathrm{ref}}=0,\ R=0.5\,\mathrm{kg^{-1}} 时，总逆惯量为 A+R=1.5+0.5=2\,\mathrm{kg^{-1}}，由此算出连接力乘子为 \lambda=-3/2=-1.5\,\mathrm N，两滑块加速度变为 a=(1.25,0.5)^{\mathsf T}\,\mathrm{m/s^2}，残余相对加速度 Ja=0.75\,\mathrm{m/s^2} 恰好等于 -R\lambda。

当约束来自物体表面之间的单边接触与干摩擦时，约束力乘子 \lambda 还必须额外满足物理可行域限制。对于无黏附的单边接触，记表面法向间隙为 \phi（分离时 \phi>0），法向排斥接触力为 \lambda_n（推开两物体时 \lambda_n>0），则理想硬接触在位置层满足互补条件（complementarity condition）：

该式表明：当两物体分离（\phi>0）时法向接触力必为零；而当法向力非零承力（\lambda_n>0）时两表面必须闭合接触（\phi=0）。在已经闭合且法向相对速度为零（\phi=0,\ \dot\phi=J_nv=0）的持续接触阶段，这一互补关系进一步体现在法向相对加速度 \gamma_n=\ddot\phi 与法向力之间：

即：若物体即将加速脱离接触（\gamma_n>0），法向接触力立即降为零；若接触面持续承力（\lambda_n>0），则法向加速度维持为零（碰撞瞬间的有限速度跳变则通过速度冲量层面的互补条件处理）。与此同时，在三维各向同性 Coulomb 干摩擦模型下，切向摩擦力向量 \lambda_t\in\mathbb R^2 的模长受法向正压力与摩擦系数 \mu 的限制，构成摩擦锥（friction cone）可行域：

当切向相对滑动速度为零（静摩擦黏着）时，切向力 \lambda_t 可取摩擦锥内部或边界上的任意平衡值；而当存在非零切向滑动速度 u_t\ne\boldsymbol0（动摩擦滑动）时，理想 Coulomb 摩擦遵循最大耗散原理（使摩擦功率 \lambda_t^{\mathsf T}u_t 最小），切向摩擦力必落在摩擦锥边界上且严格与滑动速度 u_t 反向，即 \lambda_t=-\mu\lambda_nu_t/\|u_t\|，瞬时耗散功率为 -\mu\lambda_n\|u_t\|。

在下方实验中，可在“候选力检验”模式下调节法向力 \lambda_n 与摩擦系数 \mu，观察一维切向摩擦区间 [-\mu\lambda_n,\mu\lambda_n] 的伸缩；切换至“滑动耗散”模式后，改变切向滑动速度 u_t 的方向与大小，观察滑动摩擦力如何在区间两个端点之间切换以始终保持负功。

可以看到，法向正压力 \lambda_n\ge0 的大小直接控制了切向摩擦力的上下限；当法向力或摩擦系数减为零时，整个摩擦锥收缩为原点。在后续第十一至十五章中，我们将看到 MuJoCo 如何将法向与切向接触统一放入带正则项 R 与摩擦锥约束的凸优化问题中求解。

以下源码分析对应本地 MuJoCo 3.10.0。在正动力学流水线 mj_fwdVelocity 中，引擎首先调用零广义加速度下的递归逆动力学计算出偏置力 qfrc_bias（包含科氏/离心速度二次项与重力项）。随后在 mj_fwdAcceleration 中，程序汇总关节被动弹簧阻尼力 qfrc_passive、用户外加广义力 qfrc_applied 与执行器驱动力 qfrc_actuator，减去偏置力 qfrc_bias，并通过 mj_xfrcAccumulate 将作用于刚体上的笛卡尔外力旋量经雅可比转置累加进平滑净广义力 qfrc_smooth：

mju_sub(d->qfrc_smooth, d->qfrc_passive, d->qfrc_bias, nv);
mju_addTo(d->qfrc_smooth, d->qfrc_applied, nv);
mju_addTo(d->qfrc_smooth, d->qfrc_actuator, nv);
mj_xfrcAccumulate(m, d, d->qfrc_smooth);

接着，引擎调用 mj_solveLD 利用预先分解好的广义质量矩阵 M(q) 求解线性系统 Ma_u=\tau-c，所得的无约束自由加速度即存入 qacc_smooth（即本章的 a_u；质量矩阵组装与稀疏分解将在第八章详细剖析）。

在约束建构阶段，mj_referenceConstraint 根据当前位置违约量 efc_pos - efc_margin 与约束空间速度 efc_vel 组装弹簧阻尼恢复目标加速度 efc_aref，并对球铰连接（connect）、刚性固接（weld）等双边等式约束调用 mj_Jdotv 减去速度二次曲率项 \dot Jv：

d->efc_aref[i] = -KBIP[4*i+1]*d->efc_vel[i]
                 -KBIP[4*i]*KBIP[4*i+2]*(d->efc_pos[i]-d->efc_margin[i]);

if (d->ne > 0) {
  mj_Jdotv(m, d, d->efc_aref);
}

此处的 efc_aref 即对应第 8 节对约束加速度 Ja 设定的参考目标 a_{\mathrm{ref}}（包含维持曲率所需的 -\dot Jv 以及消除位置/速度漂移的反馈稳定项）。MuJoCo 的官方软约束计算文档正是将 Gauss 动能度量代价与约束偏差惩罚结合为统一的凸优化问题；其中双边等式行直接化为式（7.5）的无约束二次型，而单边接触与摩擦行则在此基础上附加摩擦锥可行域约束。

进入约束求解器后，源码严格按照“先求自由加速度偏差 Ja_u-a_{\mathrm{ref}}，再在 A+R 与可行域下求解约束力 \lambda，最后将 J^{\mathsf T}\lambda 送回广义空间修正加速度”的三步流程运作。下方说明逐段对照了这三步在源码中的具体数组操作。

mj_mulJacVec(m, d, d->efc_b, d->qacc_smooth);
mju_subFrom(d->efc_b, d->efc_aref, nefc);

d->efc_AR[r*(nefc+1)] += d->efc_R[r];

mj_mulJacTVec(m, d, d->qfrc_constraint, d->efc_force);
mj_solveM(m, d, d->qacc, d->qfrc_constraint, 1);
mju_addTo(d->qacc, d->qacc_smooth, m->nv);

而在原始空间（primal）求解器 PrimalUpdateConstraint 中，Gauss 拘束代价项直接按式（7.4）的内积形式逐分量累加：

Gauss += 0.5 * (ctx->Ma[i] - ctx->qfrc_smooth[i])
             * (ctx->qacc[i] - ctx->qacc_smooth[i]);

其中第一个因子 Ma[i] - qfrc_smooth[i] 是 M(a-a_u) 的第 i 个分量，第二个因子 qacc[i] - qacc_smooth[i] 是 a-a_u 的第 i 个分量，求和后恰为 \Phi(a)=\tfrac12(a-a_u)^{\mathsf T}M(a-a_u)。

配套教学脚本 analytic_dynamics.py 实现了本章平面双连杆的解析质量矩阵、科氏/重力偏置力、机械能计算以及带正则化的 Gauss 约束加速度投影。在线性方程组求解上，程序采用对称正定矩阵的 Cholesky 分解与前向/后向三角回代，与数学公式中的矩阵求逆运算严格对应：

from analytic_dynamics import *

q = model_data()["default_angles"]
v = [1.0, -0.5]
mass = two_link_mass(q)
bias = two_link_bias(q, v)
acceleration = two_link_acceleration(q, v)

projection = project_acceleration(
    [2.0, 0.0, 0.0, 1.0],
    [2.0, -1.0],
    [1.0, -1.0],
    [0.0],
)
print(acceleration)
print(projection["acceleration"])

运行上述代码将依次输出第 4 节例题的双连杆瞬时角加速度与第 7 节例题的双滑块受约束共同加速度。可在仓库根目录按相对路径运行脚本与自动化测试：

python3 notes/series_html/examples/analytic_dynamics.py
python3 notes/series_html/examples/check_analytic_dynamics.py