约束雅可比与 EFC 行

设球心高度为 z。球面与平面的有符号距离为 s=z-\rho：分离时为正，相切时为零，重叠时为负。理想的几何限制写成 s\geq0。为提前产生接触响应，MuJoCo 用进入约束的边界 m_c 定义恢复误差 r=s-m_c。因此，接触行开始起作用时，两个表面仍可能保有正间隙。

本地 MuJoCo 3.10.0 中，窄相位按 s\leq m_c+g 搜集候选接触，其中 g 是 gap；随后用 s<m_c 决定该接触是否进入约束。边界 s=m_c 被排除。处于两条边界之间的接触保留几何信息，尚未分配有效 EFC 行。具体几何算法对候选外边界的浮点判定也会影响恰好位于边界的接触。

球心下降时，会先经过候选边界，再经过激活边界，最后才到达真实表面。下面比较三种时刻，特别注意 s>0 与 r<0 可以同时成立：这对应尚有物理间隙、已经需要恢复的接触。

图中的两条阈值只改变接触何时进入数据和方程。进入约束之后，法向运动的符号仍由同一个距离行确定：球向上运动使 s 与 r 同时增大。

关节限位也可以取“允许区域为正”的残差。下限用 r=q-q_{\min}，上限用 r=q_{\max}-q，对应的雅可比符号分别为 +1 与 -1。正的限位力经雅可比转置返回后，分别增大和减小关节坐标。双边位置约束则要求 r=0，恢复力可取两个符号。

给定可微的几何残差 r(q)，它沿系统运动的变化率由链式法则得到。配置含四元数时，速度与配置导数通过第六章的切空间映射 \dot q=N(q)v 联系；由此得到相对于广义速度的雅可比。

式（6.6）把存储配置的导数与广义速度联系起来。这里在这个映射之后，再求几何残差的变化，因此接触雅可比的列数与广义速度维数相同。

两个滑块沿同一直线运动，位置分别为 x_1,x_2。若要求距离为 L，可取 r=x_2-x_1-L，于是 J=(-1,1)。第一个滑块向正方向运动使残差减小；第二个滑块同向运动使残差增大。

沿同一条运动轨迹再求一次导数，会同时得到速度变化和雅可比变化两部分。记 a=\dot v、c=\dot Jv，则 \ddot r=Ja+c；给定几何加速度目标 \alpha 后，投影加速度应取 a_{\rm ref}=\alpha-c。

曲率项可由圆周运动直接看出。平面质点位置为 p，受长度为 L 的杆约束，取 r=(p^{\mathsf T}p-L^2)/2。此残差的单位是面积，雅可比为 p^{\mathsf T}。杆的几何形状保持不变时，径向加速度与切向速度仍同时存在。

对长度固定的杆，平方距离残差始终为零，所以 \ddot r=0。在右端点沿竖直切向运动时，这个条件给出向心加速度 a_x=-u_t^2/L。它来自雅可比随质点位置转动产生的曲率项。

质点在圆周上始终满足位置残差为零，但径向加速度仍非零。图中把加速度投影项 Ja 与速度产生的曲率项 c 分开显示；匀速播放时两项应大小相等、符号相反。

圆周例把二阶约束的两部分分清了：速度越大，几何曲率要求的向心加速度越大。后续构造参考伪加速度时，要把已知曲率从几何加速度目标中移到右侧。

MuJoCo 的 EFC 统一保存伪加速度目标。当前源码先按位置、速度建立恢复项，再由 mj_Jdotv 为 connect 和 weld 等式行减去相应曲率项。接触行、关节限位行以及其他等式类型遵循各自的装配规则；解释一个目标时，应同时查看该行的残差定义与源码中的修正。

一个球体可以保持质心不动而原地转动，此时接触点仍会沿表面运动。摩擦响应取决于这个局部速度，因此需要先从质心运动恢复接触点运动，再把两侧速度相减。

一般刚体的接触点会同时受到质心平移和刚体转动的影响。在当前接触点 p_c 处，令 c_i 为第 i 侧质心，\ell_i=p_c-c_i。所有量都在世界轴下表示。两侧点速度及其差为

在质心之外测量点速度时，角速度对点速度的贡献由力臂决定。将质心线速度和角速度各自写成广义速度的线性映射，就得到 J_p=J_C-[\ell]_\times J_\omega。

把相对线速度投影到接触坐标轴，便得到前三条物理方向行。相对角速度为 \omega_2-\omega_1，其投影形成扭转和滚动方向。接触坐标系满足 t_2=n\times t_1。

先记两侧雅可比之差为 J_\Delta=J_{p_2}-J_{p_1}、J_{\omega\Delta}=J_{\omega_2}-J_{\omega_1}。前三行对相对线速度取分量，后三行对相对角速度取分量：

把这六行乘以同一个广义速度，所得各项的物理意义如下。每行的力变量都与该行速度相乘得到功率：

模型中的平面静止，球体只有三个平移自由度，故前三行为世界分量的重排。角速度行在这个模型中为零。

法向速度为负，表示两侧正在接近；第一切向速度为正，表示球体相对平面沿世界横轴滑动。滑动方向力的单位为牛顿，扭转与滚动方向的力矩单位为牛顿米。因此滑动摩擦系数无量纲，而把法向力转换为转动力矩界限的摩擦系数具有长度单位。

先在一个刚体上观察这项分解。图中的材料点可以代表当前接触位置；参考点取质心时，它与接触点之间的向量就是本节的力臂。

对接触的另一侧重复同样的计算，再用第二侧减第一侧，便得到相对速度。投影到法线决定接近或离开，投影到两个切向则给出滑动方向。

condim 决定一个接触保留哪些物理方向。椭圆锥直接使用这些方向的力分量；棱锥采用成对的非负棱系数。两种表示由此具有不同的行数。

设物理维数为 d_c>1，第 j 个摩擦方向的系数为 \mu_j。该方向的一对棱行是 J_{j+}=J_n+\mu_jJ_j 与 J_{j-}=J_n-\mu_jJ_j。各棱系数 \beta_{j+},\beta_{j-}\geq0 共同合成物理法向力和摩擦分量。

非负棱系数先分别沿棱行产生广义力。合并它们的法向部分得到法向合力，合并正负切向部分得到摩擦力：每一对系数的和增加法向力，二者的差决定该方向的摩擦分量。

同一接触的两种锥表示使用不同的行。固定球体速度后，先看椭圆表示中的纯法向、纯切向速度，再看棱锥行如何把二者相加或相减。图中行数、雅可比和速度由同一次投影得到。

每条棱行中的法向贡献都保留下来，因此多个非负棱系数可以共同形成一个物理法向力。角速度行在固定姿态的球体中为零，这也说明接触维数规定了可表示的方向，而实际作用还取决于系统自由度。

EFC 中的位置字段按行的用途存放。椭圆接触仅在第一行存放物理距离与 margin，切向行的两个位置字段均为零；所有棱行都复制法向距离和 margin。这使各棱行共享法向恢复误差，同时对不同方向的相对运动做出响应。

因此，\dot r=Jv 的链式法则应施加在已定义的几何残差上。存储字段 efc_pos 兼有提供恢复量的用途；椭圆摩擦行虽然存零，其行速度仍能描述滑动，棱行的速度也包含法向位置字段以外的摩擦方向运动。

约束速度 Jv 与约束力分量 f 成为功率共轭的一对量。它们的内积等于各个广义自由度获得的功率，因此广义约束力为 J^{\mathsf T}f。

式（6.5）的雅可比转置曾把一个末端点力传回关节。现在把各个接触方向逐行排列，同一功率关系会把整组约束力一起传回。

固定当前配置与速度，把重力、驱动力和速度项先合并成自由加速度 a_u。加入约束力后，动力学方程及其约束空间投影为

A 描述单位约束力引起的行加速度变化，称为约束空间逆惯量。非对角项反映两条约束通过共同刚体或关节发生的耦合。计算时可以解 Mx=J_i^{\mathsf T} 再投影各行，从而复用质量矩阵分解。

对一条独立的距离行，令约束力引起的相对加速度变化为 \Delta a_r。关系 \Delta a_r=Af 可以改写成 f=m_{\rm eff}\Delta a_r，其中 m_{\rm eff}=1/A。这个有效质量描述沿所选相对方向推动系统的难易；两滑块共同运动时，两侧的逆质量相加。

质量矩阵正定时，A 对称半正定。若某组行力经 J^{\mathsf T} 恰好相互抵消，这组力处于 A 的零方向；独立的行才能使 A 在整个行空间正定。

给距离行施加一个单位力，会同时加速两个滑块；第二滑块的位置行也因此感到加速度变化。下面令第二行的力为零，再改变第一行的力和滑块质量，可以直接读出 A 的第一列。交换激励行后，就读出第二列。

两条行通过第二个滑块共享惯性，所以非对角项为 1/m_2。当两条行同时受力，各列的响应相加得到 Af；逐行独立猜测力值会忽略这一联系，后面的多接触求解必须同时满足所有行。

叠放方块的两行接触

再把单条距离约束扩展成上下叠放的两个方块：下方块与地面接触，上方块与下方块接触。两方块只沿竖直方向平移，质量各为一千克，高度各为一米，当前两处间隙都为零。地面接触与层间接触共享下方块，因而会互相影响。

以 z_1,z_2 表示下、上两方块的质心高度。两行间隙及其雅可比为

地面支撑力为 f_1，层间支撑力为 f_2，两者正向都规定为向上作用在接触的上侧物体。层间力在下方块上的反作用向下，于是

第一列表示只增加一牛顿地面力：下方块加速度增加一，地面间隙加速度增加一，层间间隙加速度减少一。第二列表示只增加一牛顿层间力：下方块向下、上方块向上，各改变一，层间分离加速度合计增加二。矩阵的每个数都有对应的物体响应。

这个例子在第十三章用于确定哪几处接触承载，在第十五章用于逐行计算接触力。物体模型与力的正方向保持一致。

多条行同时求解时，1/A_{ii} 表示其他行力固定时第 i 行的局部有效质量。其他行若也随之调整，最终响应还含 A 的非对角耦合。几何残差乘以尺度后，这个数值也会改变，因此使用有效质量时必须同时说明残差与力的单位。

为保持同一个物理运动，残差缩放应同时作用于共轭力与行空间惯性。设常数 \gamma>0，取 \widetilde r=\gamma r，则 \widetilde J=\gamma J，共轭力应取 \widetilde f=f/\gamma。

源码还使用 diagApprox 为软约束确定惯性尺度。常规接触分支把两侧刚体的参考平均平移或转动逆惯量相加；棱行再按摩擦系数的平方组合它们。这个近似只提供对角尺度，完整 A 保留当前配置、点偏置及跨行耦合。启用 diagexact 时，源码先由质量分解求出精确对角，再重建阻抗参数；摩擦方向随后仍按锥的规则调整。

对自由刚体，单位法向力既推动质心，也通过力矩转动刚体。沿同一法向测量接触点加速度，平移与转动的贡献相加；因此精确对角会随力臂和接触方向改变。

efc_diagA 在 mj_makeImpedance 末尾还会被重写，使它与最终的 R 和阻抗保持约定关系。读取最终数组时，应结合调用阶段理解该值。

EFC 将等式约束、干摩擦、限位与接触排列在同一行空间中。mj_makeConstraint 的装配顺序依次为等式、自由度与肌腱干摩擦、关节与肌腱限位、接触。ne 和 nf 记录前两组的行数，求解器据此选择双边、饱和摩擦或单边等处理规则。

稠密模式按行连续存放 J；稀疏模式用 efc_J_rownnz、efc_J_rowadr、efc_J_colind 描述每行长度、起始地址与广义自由度列号。contact.efc_address 指向该接触的第一条行；被排除或没有可作用自由度的接触保留负地址。

同一个行数组中既有单边接触，也有双边等式。行类型决定允许力的范围，行雅可比决定力作用到哪里；两者要共同解释，才能把数组中的一行还原成具体的力学关系。

connect 等式比较两侧锚点世界位置，生成三个残差。weld 再加入三个姿态残差，形成六行。源码的姿态部分使用相对误差四元数的向量部，并乘以具有长度尺度作用的 torquescale。

weld 将目标相对姿态与当前相对姿态合成一个误差四元数。其向量部等于误差轴乘以半角正弦；乘以长度尺度 \ell 后，与平移残差一起构成六行。姿态雅可比由这个四元数乘积求导，小角附近的比例为 \ell/2。

求解误差从自由加速度开始。记目标为 a_{\rm ref}，则加入约束力后的目标偏差满足

对于双边软约束，力与目标偏差满足 Rf=-(Ja-a_{\rm ref})。把动力学投影代入后，得到 (A+R)f=-b。因此 A 来自动力学，R 来自选定的软约束关系，b 来自零约束力时的目标偏差。

取球心高度 0.19\,\mathrm m、速度 (0.8,0,-0.1)^{\mathsf T}\,\mathrm{m/s}，质量矩阵为 M=2I。此处给定行空间候选力 f=(20,-4,1)^{\mathsf T}\,\mathrm N，用它核对从约束空间到广义加速度的完整关系。

该状态下，候选力投影后的加速度为 a=(-2,0.5,0.19)^{\mathsf T}\,\mathrm{m/s^2}。将同一结果再投影到接触轴，应与先计算 Af 的结果相同；下面把两条计算路径与功率一起核对。

配套程序 efc.py 使用普通 Python 列表实现上述计算，check_efc.py 分别用 Newton 方程、功率和有限差分核对投影。运行目录为 notes/series_html。

python3 examples/efc.py
python3 examples/check_efc.py

model = model_data()
rows = contact_rows(condim=3)
velocity = matvec(rows, model["velocity"])
A = inverse_inertia(rows, [1/model["mass"]]*3)
force = [20, -4, 1]
tau = transpose_product(rows, force)
acceleration = [g + f/model["mass"]
                for g, f in zip(model["gravity"], tau)]

以下链接指向当前仓库源码。阅读顺序沿几何接触、约束行、参考量和动力学求解展开。

官方 Computation — Constraint model 给出了约束空间形式；本章的字段、函数与边界约定以所链接的本地 3.10.0 源码为准。