约束求解器与时间积分

两个方块上下叠放在水平地面上。地面要托住两个方块的总重量，两方块之间的接触只需托住上方块。静止时，这两股支撑力很容易由受力平衡求出；将它们作为已知的检验结果，便能逐步观察求解器怎样从零力走到正确的接触力。

设下方块为物体 1，上方块为物体 2。两者质量都是 1\,\mathrm{kg}、高度都是 1\,\mathrm m，只允许竖直平移。竖直向上为正，重力加速度大小取 10\,\mathrm{m/s^2}。当前两个接触都刚好闭合，速度为零。用 f_1 表示地面对下方块的向上支撑力，用 f_2 表示下方块对上方块的向上支撑力。由作用与反作用，下方块还受到大小为 f_2 的向下作用力。

对每个物体单独列牛顿第二定律，得到下面两行。质量都是一千克，数值上合力与加速度相等，但两者的单位仍分别为牛顿和米每二次方秒。

先看静止支撑：上方块的 a_2=0 要求 f_2=10\,\mathrm N；代入下方块的 a_1=0，便得 f_1=20\,\mathrm N。地面的反力比层间反力大，是因为它还承担了下方块自身的重量。下面所有迭代都应趋近这组结果。

从物体加速度到接触加速度

求解器处理的是接触处能否分离或继续挤入。记两物体的质心高度为 z_1,z_2。地面间隙为 e_1=z_1-1/2，层间间隙为 e_2=z_2-z_1-1。它们在当前时刻都为零。对时间求两次导数，接触处的相对加速度便是

第一行只看下方块相对地面的运动；第二行比较上、下两物体的运动。若二者一起自由下落，a_1=a_2=-10，层间间隙的二阶导数仍为零，地面间隙却有继续减小的趋势。因此，两行的物理意义各不相同。

把式（15.1）代入式（15.2），接触加速度成为接触力的线性函数：

允许的接触状态与优化目标

对于当前闭合、零相对速度的硬接触，支撑力只能推开物体，因此 f_i\geq0；间隙加速度应满足 \ddot e_i\geq0。若接触开始分离，支撑力应为零；若支撑力为正，接触保持闭合。把这两种情况写在一起，得到第十三章互补条件在本例中的形式：

这里用 w 简记两行接触残差。硬接触且参考加速度为零时，它正好等于 \ddot e。式（13.3）的最优性条件与式（13.6）的接触二次目标给出同一关系。本例具体为

软接触与时间推进

加入第十二章的软约束时，以 a_{\rm ref} 表示接触参考加速度，以正对角矩阵 R 表示柔顺程度。式（13.6）的对偶目标仍是二次式，只需把矩阵和一次项改成

a_u 是尚未施加约束力时的物体加速度；本例为 (-10,-10)^{\mathsf T}。软模型的残差变为 w=Ja-a_{\rm ref}+Rf。正力行要求这一残差为零，因而允许实际接触加速度偏离参考值。下一节先计算 R=0 的静止支撑，再计算 R=0.1I 的软响应，区分模型差异与迭代误差。

求解期间固定当前位置和速度，反复修改候选力，直到满足最优条件。此后才用 a=a_u+M^{-1}J^{\mathsf T}f 得到加速度并积分。迭代轮数控制当前受力问题的求解精度，时间步长控制状态推进的离散误差。

投影 Gauss–Seidel 方法（Projected Gauss–Seidel，PGS）每次只改变一个接触力，暂时固定其余力。对叠放方块来说，第一行调整地面支撑力，第二行调整层间支撑力；后一行立即使用前一行刚算出的结果。

第一轮：两处接触的相互影响

从 f=(0,0)^{\mathsf T} 开始。两个物体都自由下落。先固定 f_2=0，调整地面反力，使下方块的 a_1=-10+f_1 为零，得到 f_1=10\,\mathrm N。此时下方块停止加速下落，上方块仍以 -10\,\mathrm{m/s^2} 加速，层间接触便出现挤入趋势。

接着固定新的 f_1=10\,\mathrm N，调整层间力。由式（15.3），层间加速度为 -10+2f_2，令它为零得到 f_2=5\,\mathrm N。这股力向上推上方块，同时向下推下方块，因此两物体的加速度都变成 -5\,\mathrm{m/s^2}。层间条件暂时满足，地面条件又出现了误差。

第二轮地面反力补到 15\,\mathrm N，层间反力随后补到 7.5\,\mathrm N。两物体向下的加速度减半。接触之间的耦合使一次逐行扫描只能消除一部分误差，后续扫描继续传递其余影响。表内各行都属于同一时刻的候选解，物体尚未推进到下一帧。

力平面、坐标更新与一维截面

把地面力作为横坐标、层间力作为纵坐标，每一组候选力就在力平面上对应一个点。两种接触都只能推开物体，因此允许区域是包含坐标轴的第一象限。默认模型的目标由式（15.5）展开为

它的等值线是一组以 (20,10) 为中心的椭圆。固定层间力时，候选点只能水平移动；固定地面力时，只能竖直移动。把同一个目标沿这条水平线或竖直线截开，就得到当前行的一元二次函数。

第一步固定 f_2=0，截面为 s^2/2-10s，极小点在 s=10。第二步固定新值 f_1=10，去掉与本行变量无关的常数后，截面为 s^2-10s，极小点在 s=5。于是前两段路径依次水平到达 (10,0)、竖直到达 (10,5)。

在默认参数下，第二次行更新沿竖直方向到达 (10,5)。这一点已使竖直截面达到极小值，仍未到达完整二维目标的中心。下一次水平更新继续降低目标，于是路径逐段接近 (20,10)。切换到受力图，同一步对应两物体均向下的 5\,\mathrm{m/s^2} 加速度，恰好与前表一致。

一般更新式与非负投影

上述两次计算来自同一条规则。把当前要更新的第 i 个分量暂记为 s，其余分量视为已知，目标函数就只剩一个一元二次式。先求其极小点，再把负的试算力截到零：

代入本例的 H=A 和 b，更新式成为 f_1\leftarrow10+f_2、f_2\leftarrow f_1/2。从零开始，两式的试算结果一直非负，所以暂时看不到截断的作用。完成第 k 轮后，结果可以写成

分离接触与软约束

现在给上方块施加向上的外力 P。它的自由加速度变为 -10+P，所以 b=(-10,P)^{\mathsf T}。取 P=15\,\mathrm N，第一行仍算出 f_1=10\,\mathrm N；第二行的试算值变成

负试算值要求两物体互相吸引。普通接触只能推，投影便把层间力置零。此时下方块保持 a_1=0，上方块有 a_2=5\,\mathrm{m/s^2}，层间开始分离。残差为 w=(0,5)^{\mathsf T}，仍完全满足式（15.4）；用“所有残差分量都必须为零”作为停止条件会误判这一正确解。

再回到 P=0，取软正则化 R=0.1I\,\mathrm{kg^{-1}}、参考加速度为零。此时 H_{11}=1.1、H_{22}=2.1，两股正力满足 Hf+b=0。解得

软解中的向下加速度来自所选参考值与正则化。继续增加迭代轮数，只会更准确地趋近这组软解。要描述软接触的静止支撑，还需像第十二章那样由位置误差产生非零恢复参考值；静止时应满足 a_{\rm ref}=Rf。

停止量、初值与其他约束行

检查一行是否还需要修正，可以用同一条 PGS 更新式计算“当前值减去重新更新后的值”。将各行绝对值取最大，得到具有力单位的停止量：

热启动使用前一时间步的信息构造本步初值。图中可以把初值改成接近支撑解的 (18,9)^{\mathsf T}\,\mathrm N，对比达到同一误差所需的行更新次数。接触状态突变时，旧近似也可能离当前解较远，仍须按本步的约束重新检查。MuJoCo 如何从保存的加速度建立初值，将在源码一节具体对应。

式（15.7）的截断范围由约束类型决定。等式约束的力可取任意实数，直接接受试算值；有限干摩擦损失使用给定上下限；无摩擦法向接触使用 [0,\infty)。椭圆摩擦锥把同一接触的多个分量联系起来，需要作为一个块处理。

记当前接触块的变量为 u，对应主对角块为 H_{BB}；其他块的固定值并入一次项系数 c_B。无约束块解为 u_*=-H_{BB}^{-1}c_B。配方后，允许锥内的最优点就是离 u_* 最近的点，距离由 H_{BB} 加权。这个式子给出理想的精确块更新目标。

本地 MuJoCo 的椭圆锥 PGS 先沿当前射线调整，再固定法向处理切向二次约束子问题。上面的矩阵度量说明完整局部问题的目标，具体实现的两个子步骤在第十四章已有展开。

可运行的逐行计算

下面的教学实现与受力图采用同一模型。输入是向上外力、正则化和迭代轮数，输出保留每次行更新后的力、加速度及停止量。P=0,R=0 时趋向静止支撑；P=15,R=0 时得到层间分离。

from stack_contact import pgs_stack

for row in pgs_stack(pull=0, regularization=0, sweeps=2):
    print(row["row"], row["force"], row["acceleration"], row["error"])

# 第二次完整扫描：力为 (15, 7.5)，加速度为 (-2.5, -2.5)
separating = list(pgs_stack(pull=15, sweeps=1))[-1]
assert separating["force"] == (10, 0)
assert separating["acceleration"] == (0, 5)

在 examples 目录运行；完整代码逐行保留了试算值和非负投影。

逐变量更新一次只处理一个坐标方向。另一种选择是同时改变全部变量：先用梯度找到当前下降方向，再利用二次曲面的形状避免反复修正已经处理过的方向。下面从一条搜索直线上的精确最小值建立这两种方法。

式（13.2）给出了二次目标的梯度与曲率。PGS 沿坐标方向逐项极小化，本节从一般搜索方向出发，计算沿一条直线应该前进多远。

仍取叠放方块的硬接触模型，从 f=0 出发。其内部极小点恰好满足非负性，可以先沿整个力平面研究搜索方向。下面用 x 代表一般优化变量；在这组具体计算中，x=f。到 Newton 一节，变量将改成物体加速度，受力结果仍与前面的软模型核对。

以下优化运算采用物理量在国际单位制下的数值坐标，求得的力按牛顿解释。梯度决定数值坐标中的搜索方向，步长决定沿该方向的更新幅度。

本例梯度为 g=Af+b。起点的负梯度方向是 (10,0)^{\mathsf T}，先增加地面力；到达 (10,0)^{\mathsf T} 后，下一共轭方向为 (10,10)^{\mathsf T}，把两股力同时增加到静止支撑解。

先考虑没有不等式边界的固定二次目标。梯度为 g=Hx+b，沿方向 p 前进时，可以精确计算使目标最小的步长：

最速下降每次使用 p=-g。狭长椭圆的两侧梯度方向变化较大，连续步长往往形成折返路径。共轭梯度法（Conjugate Gradient，CG）用 Hessian 定义方向间的正交性，让新的搜索保留已有方向上的极小化结果。

对于固定正定 Hessian 和精确线搜索，方向系数可取 \beta_k=\|g_{k+1}\|^2/\|g_k\|^2。它使新方向与已有方向关于 H 共轭，从而保留此前完成的极小化。

再用二维对角例核对两个方向。取 H=\operatorname{diag}(1,4)、b=0、x_0=(2,1)^{\mathsf T}，第一步的精确线搜索后，梯度转为与旧梯度正交。第二个共轭方向再消除剩余误差，恰好在两步到达原点。

对同一个固定二次目标，可以直接比较不同搜索方向的作用。图中最速下降沿当前梯度法线，共轭梯度保留此前已消除的方向误差，Newton 则利用完整曲率求解；增大条件数会拉长等值椭圆。

图的初始条件数为 8，初值为 (1,1)^{\mathsf T}。最速下降与 CG 的第一方向同为 (-1,-8)^{\mathsf T}，精确步长为 65/513，所以两条路径的第一个点重合在 (448/513,-7/513)^{\mathsf T}。下一步 CG 改用共轭方向，最速下降继续取新的负梯度，两条路径由此分开。Newton 则解 Hp=-g，在这个固定二次例中一步到达原点。

路径差异来自方向选择与曲率处理。图中的固定二次模型使这些关系清楚可见；实际接触改变活动集合时，目标的局部曲率也随之变化，方向与步长需要重新计算。

图中的条件数等于最大曲率与最小曲率之比。某一方向的曲率远大于另一方向时，同一个长度单位会让梯度在各轴上尺度失衡。预条件通过一组较便宜的线性求解重新缩放变量，使后续方向在更均衡的坐标中形成。

预条件用一个容易求解的正定矩阵 P 改善方向尺度。首先解 Pz_k=g_k，得到预条件梯度 z_k；初始方向取 p_0=-z_0。每一轮先沿当前方向做线搜索，再更新位置与梯度：

随后再次求解预条件方程，比较新、旧梯度在这一尺度下的长度，并组合下一方向：

位置更新负责降低当前目标；预条件求解负责调整尺度；方向组合负责保留共轭关系。这些公式采用固定的正定 H,P 与精确线搜索。梯度已经为零时即可终止。

PGS 直接修改接触力。原始方法则先提出一组候选物体加速度，由软接触规律计算对应的力，再检查这组力能否产生所提出的加速度。两种变量组织方式在最优点满足同一条动力学关系。继续采用式（15.9）的模型：两物体质量为一千克，R=0.1I\,\mathrm{kg^{-1}}，自由加速度为 (-10,-10)^{\mathsf T}，参考加速度为零。

式（13.7）给出了独立法向接触的加速度目标及其与力空间问题的对应。这里把它逐项代入方块模型，再对同一目标求导、解方向和选步长。

地面行的接触加速度是 a_1，层间行是 a_2-a_1。各行若低于参考值，按软关系产生正力；高于参考值时，接触力为零：

第十三章的原始目标在这个模型中完整展开为

前两项衡量物体加速度离自由下落有多远；后两项分别衡量地面和层间的压入趋势。图中两条分界线因此为 a_1=0 与 a_2=a_1。在每个分区中，参与求导的接触项固定，目标就是一个普通二次函数。

在加速度变量中，独立法向软约束对应分段二次目标。记 z_i=J_i a-a_{{\rm ref},i}，并用 R_i>0 表示各行正则化：

第一项衡量加速度偏离自由值的惯性代价；每个接触项只在伪加速度低于参考值时产生代价。一次求解中 M,J,R 都固定，改变候选加速度只会改变各行的残差 z_i 及其所在分段。

每一行在 z_i<0 时产生力 f_i=-z_i/R_i，在 z_i\geq0 时力为零。在活动集合保持不变的区域内，梯度与 Hessian 为

Newton 方向求解 H_a p=-g。若整条步长都留在同一个二次区域，取单位步长便能到达该区域的极小点；跨越活动边界时，重新评价目标，并缩短步长保证下降。

当梯度非零、当前 Hessian 正定时，Newton 方向一定是下降方向。即使完整一步跨越了接触分区，足够短的正步长仍能满足回溯下降条件。

一次完整的方向计算与回溯

从候选加速度 a=(2,2)^{\mathsf T}\,\mathrm{m/s^2} 开始。它使地面分离、层间相对加速度为零，所以接触力暂为零。式（15.11）给出 F=144，梯度为 (12,12)^{\mathsf T}。边界行取非活动侧曲率，当前 Hessian 是单位阵，因此 Newton 方向为 p=(-12,-12)^{\mathsf T}。

若直接走完整一步，候选点变成 (-10,-10)^{\mathsf T}。此时地面项变为 500，总目标从 144 升到 500。缩短为半步后，到达 (-4,-4)^{\mathsf T}：惯性项为 36，地面项为 80，层间项为零，总目标降为 116，满足回溯判据。

最终接触力由软关系恢复为 f=(2100/131,1000/131)^{\mathsf T}\,\mathrm N，与式（15.9）的 PGS 极限一致。途中某个候选加速度和它产生的接触力可能尚不满足物体动力学，梯度 g=M(a-a_u)-J^{\mathsf T}f 正是两边的差；到达零梯度后，两种计算相互吻合。

MuJoCo 的 CG 与 Newton 在原始加速度目标上共用目标更新和线搜索框架。摩擦会使约束代价具有更丰富的分段结构。接触状态切换时，梯度和曲率都需要用当前状态重新计算。

用质量为 1\,\mathrm{kg} 的弹簧阻尼振子考察时间推进。自然角频率为 \omega，阻尼系数除以质量后记作 \gamma：

相平面以位置为横轴、速度为纵轴。无阻尼时，轨迹沿能量椭圆运动；有阻尼时，机械能逐渐减少。

显式 Euler 用步起点的导数更新全部状态。半隐式 Euler 先更新速度，再用新速度更新位置：

对无阻尼振子，显式 Euler 在任意正步长下都会放大振幅；半隐式 Euler 在 0<h\omega<2 时保持有界。这个差别由更新矩阵的特征值决定，端点的重根还需单独检查。

显式 Euler 每一步把真实能量乘以 1+h^2\omega^2。半隐式方法在稳定区间内则保持一个含交叉项的修正能量，真实能量围绕它振荡；因此有界轨迹仍会带有相位与能量误差。

同一连续振子可以在位置、相平面和能量三个视图中检查。位移曲线显示周期与相位，相平面显示轨迹是否收缩或发散，能量曲线则量化每一步带来的增益或耗散。下面所有曲线使用同一初值和时间步。

有界性只说明离散轨迹没有无限增大，准确性还体现在相位和能量偏差随步长减小而收敛。比较积分器时，应同时看这三个视图，而不能仅由短时位移接近判断长期行为。

全隐式 Euler 使用步终点的导数。对线性振子，可以直接解出新速度与新位置：

将新位置代入新速度方程，可直接解出两者。无阻尼时，全隐式 Euler 每步把能量除以 1+h^2\omega^2，因而即使连续运动守恒，离散轨迹也会收缩。

标量测试方程 \dot y=\lambda y 把稳定性归结成复平面上的放大因子。设 z=h\lambda，一步更新写成 y_{k+1}=R(z)y_k。稳定域由 |R(z)|\leq1 给出。

显式 Euler 的稳定域是以 (-1,0) 为圆心的单位圆盘，全隐式 Euler 的稳定域包含整个左半平面。RK4 与虚轴相交于 |h\lambda|\leq\sqrt8 的区间，给出了无阻尼振子的线性稳定步长界。

放大因子把一个时间步对线性模式的作用浓缩成复数。下面把 z=h\lambda 作为平面上的点：落在某方法的稳定域内，表示该模式的模长每步都不会被放大。

无阻尼振子的特征值在虚轴上，减小步长会把测试点沿虚轴推向原点；阻尼使特征值移入左半平面。步长与连续系统的频率、衰减率因此共同决定它在稳定图中的位置。

一个时间步内，物体位置和受力都会变化。只在步起点求一次导数，难以反映整段运动的变化。RK4 在步内构造几个试探状态，每到一个状态就重新求导，再把这些斜率加权成一次更新。

一般一阶系统写作 \dot y=F(t,y)。四阶 Runge–Kutta 方法（RK4）在一步内使用四个试探状态，每次都重新求导：

第二、第三次求值的时间相同，试探状态各自来自不同的斜率，因此它们一般产生不同结果。在仿真中，每次求值都可能改变几何、质量矩阵、驱动力和接触状态。

线性系统的四次试探求值可依次展开成矩阵幂。加权结果与精确矩阵指数的前五项一致，所以一步误差从 h^5 开始，光滑固定时段内的累积误差为四阶。

对光滑非线性系统，函数在各试探点的多元展开还会出现二阶、三阶导数之间的组合。逐项匹配连续解的 Taylor 展开，可得到八个四阶条件；经典 RK4 的系数同时满足这些条件。

RK4 的四个斜率分别来自四个试探状态。下面逐级画出试探点及其斜率，并与真实轨迹比较；两个中点求值发生在同一时间，却使用由不同斜率构造的状态。

最后一步使用四个斜率的加权和。试探点本身可以偏离真实轨迹，权重通过抵消低阶误差形成更准确的步终点；接触切换降低光滑性时，这种高阶误差抵消的条件也会改变。

高阶精度要求一步内的导数足够光滑。碰撞激活、摩擦状态切换和不连续控制都会削弱这一条件。因此选择积分器还要考虑模型的接触频率、阻尼和每步计算成本。

速度隐式方法固定当前配置，在速度方向近似步终点的力。把当前净广义力记为 \tau(q,v)，并定义 D=\partial\tau/\partial v。质量矩阵在这一步线性化中固定为当前值。

前面的积分公式已经决定怎样使用新速度。这里进一步处理速度变化引起的力变化，使阻尼等速度相关作用进入步终点的近似方程。

令 \Delta v=v_{\rm new}-v。把步终点的力展开到一阶，并收集所有含未知增量的项，可将这一步写成一个线性求解：

对于振子，保持弹簧位置项为 q_k、对速度阻尼项取新值，得到

在零阻尼情况下，上式回到半隐式 Euler；全隐式 Euler 的分母还含位置弹簧引入的 h^2\omega^2。振子图中的两条曲线可用来比较这两种取值方式。

只保留线性阻尼时，速度隐式方法对任意正步长都保持速度符号并衰减；显式更新要求 0<h\gamma<2 才有幅值衰减。两者的衰减倍率可以直接由一步方程求出。

以下对照仓库内 MuJoCo 3.10.0。约束求解的主要实现位于 engine_solver.c；状态推进位于 engine_forward.c。局部模型和启用的功能会影响实际分支。

本地 PGS 在约束力变量上逐块更新，每轮用固定种子的伪随机序列打乱块的访问次序。椭圆摩擦块先沿法向或当前力的射线更新，再固定新的法向力，求解切向 QCQP。这两个子步骤降低块目标；第二节的精确锥块极小化给出了该块问题的整体目标。本地 PGS 每轮累加目标改进，按惯性与模型规模归一化后与 tolerance 比较，也受最大迭代次数限制。第十四章给出了固定法向 QCQP 的推导。

CG 与 Newton 在加速度变量上调用共同的原始求解框架：线搜索、更新约束状态、更新梯度，再产生下一方向。Newton 还更新约束 Hessian 及其分解。

本地 CG 默认采用 Hager–Zhang 类型的非线性共轭方向，并带重启与截断。编译分支 mjCG_PRP 选用另一种方向系数。第三节的固定二次 CG 为理解共轭性提供了可手算模型；阅读引擎时还要跟踪变化的活动状态和预条件梯度。

Euler 路径在 mj_EulerSkip 中处理关节阻尼。启用相应阻尼项时，它使用修改质量矩阵求加速度；随后 mj_advance 先更新速度，再更新配置。RK4 对一步内的试探状态重复调用动力学计算。

本地积分路径的右端包含已经求得的约束力。修改矩阵中的 D 由选定的平滑力项构造：驱动力、被动力，以及相应分支中的速度偏置力。约束力在这次速度线性化中按当前值保留，因此 qDeriv 中保存的是这些平滑力项的速度导数。

implicit 路径调用 mjd_smooth_vel 并包含速度偏置力的导数，将 M-hD 装配到一般稀疏矩阵后进行 LU 分解。implicitfast 路径跳过该偏置导数，使用下三角存储与对称分解。柔性插值刚度还有额外的校正分支。

本地 implicitfast 对符合条件的自由刚体还采用中点速度推进部分自由度。该分支要求自由体构成独立六自由度树、保持唤醒且未参与约束，并在外层检查流体参数和离散逆动力学选项。质心是否与关节原点重合，决定中点更新涉及三个还是六个速度分量；相关判定位于 midpoint_eligible 与 midpoint_aligned。普通关节的速度隐式公式与这个自由体专用分支分别对应不同代码路径。

转动配置按流形更新。自由关节位置有平移和单位四元数，速度中则含线速度和三维角速度，因此两者数组长度可能不同。局部角速度的四元数更新沿用第三章：

小角分支用对应的平滑极限。实现可从 mj_integratePos 跟踪到四元数积分函数，核对角速度坐标系及乘法次序。RK4 的配置组合同样要通过这种配置更新映射。

# 教学内核：在当前状态求加速度，再作半隐式一步。
def advance(q, v, h, acceleration):
    a = acceleration(q, v)
    v_next = [vi + h*ai for vi, ai in zip(v, a)]
    q_next = [qi + h*vi for qi, vi in zip(q, v_next)]
    return q_next, v_next

这个函数的配置采用普通标量分量。加入球关节或自由关节时，将最后一行配置更新替换为相应的流形映射。

本地求解器的初值与停止条件

在 engine_forward.c 的 warmstart 中，MuJoCo 将保存的 qacc_warmstart 代入当前雅可比与参考量，再由约束代价计算候选力。PGS 比较这组力的对偶目标与零力目标；原始方法比较热启动加速度与自由加速度的原始目标。接触集合已变化时，当前几何和约束参数仍参与这次重算。

本地 solPGS 每轮按块遍历约束，并对块序作伪随机置换；本节教学例固定先地面、后层间，以便逐行手算。源码用按问题惯性与规模归一化的目标改进量及最大迭代次数终止，式（15.10）则提供一个可以直接解释为力修正量的独立检验。

源码：约束求解器 · 前向动力学与积分 · 配置更新 · MuJoCo 计算文档

沿用第十一至十四章的平面与球体模型，球的姿态固定，只允许质心平移。质量为 2\,\mathrm{kg}，半径为 \rho=0.2\,\mathrm m，法线向上，切向沿世界横轴。下面的轨迹让球从指定高度下落，同时带有水平初速度。

质心高度为 z，几何间距为 \delta=z-\rho。本地碰撞候选距离采用 margin 与 gap 的和，力约束在 \delta<\texttt{margin} 时激活。恢复误差为 e=\delta-\texttt{margin}。

第十二章的函数用误差和速度生成参考加速度、阻抗与正则化。取相同的法向、切向正则化，组装两个接触变量的目标：

本例的二维允许力域由内部、顶点和两条边组成。先检查无约束解，再沿每条边求一元极小点，最后比较可行候选的目标值，就能得到法向与切向的联合最优解。

恢复世界方向的加速度后，使用半隐式 Euler 进行时间推进：

现在把几何距离、软参考值、摩擦锥极小化和半隐式积分接成一条时间过程。下图分别显示球心位置与同一状态下重算的接触力；改变步长可比较接触瞬间的峰值与随后支撑状态。

接触力峰值受进入接触时的速度和位置误差影响，静止段则应满足法向力等于重力。摩擦使水平速度逐渐衰减；零摩擦时水平速度保持不变。这几条关系可以逐项检验整条仿真流程。

软接触允许一定的位置偏差，margin 也会使力在几何表面相交前出现。正间隙表示两表面尚未接触，负间隙的绝对值则是穿透深度；恢复误差还需减去激活距离。静止时支撑力趋近重力，水平力在零滑速附近衰减。

静止支撑时速度和加速度同时为零，所以法向力必须等于 mg。软约束方程随后把支撑力转成平衡位置误差：-Kd(e)e=R(e)mg。将该误差加回接触边界即可得到球心的支撑高度。

from solvers_integrators import contact_trace

trajectory = contact_trace(height=0.6, speed=0.8, step=0.005,
                           duration=2.0, friction=0.6)
last = trajectory[-1]
print("state:", last["state"])        # x, z, vx, vz
print("force:", last["force"])        # normal, tangent
print("surface gap:", last["distance"])

在 examples 目录运行这段代码即可得到与图示相同的数据。几何间距、参数映射、锥求解和积分分别保存在独立函数中；每个时刻的力读数都由同一时刻的状态重新计算。

可运行代码：求解器与积分器 · 软接触参数