🔗 刚体碰撞解析#

Genesis 遵循与 MuJoCo 非常相似的二次惩罚公式来强制执行刚体约束。本文档总结了该模型的数学原理和物理解释。

1. 一般公式#

设

我们寻求最小化二次成本的加速度

\[ \frac12 (M a \,{-}\, \tau_{ext})^{\!T} (a \,{-}\, a^{\text{prev}}) \;{+}\; \frac12 (J a \,{-}\, a_{ref})^{\!T} D (J a \,{-}\, a_{ref}) \]

对于每个接触对，Genesis 创建四个约束，它们是摩擦金字塔的基础。数学上每个方向 tᵢ 是

\[ \mathbf t\_i = \pm d_1\,\mu - \mathbf n \quad\text{或}\quad \pm d_2\,\mu - \mathbf n , \]

因此 tᵢ 上的正乘数产生位于锥体 \(|\mathbf f_t| \le \mu f_n\) 内部的力。与组合逆质量成比例的对角项 Dᵢ 给出了熟悉的软约束行为，其中较大的 imp（隐式性）值导致更硬的接触。

旋转和平移关节可选地带有下限和上限位置限制。每当到限制的带符号距离变为负数时

\[ \phi = q - q_{min} < 0 \quad\text{或}\quad \phi = q_{max} - q < 0 \]

生成一个单一的 1-DOF 约束，其雅可比为

\[ J = \pm 1 \]

和参考加速度

\[ a_{ref} = k\,\phi + c \,\dot q, \]

从标量 sol_params（弹簧-阻尼器式柔软度）获得。D 的对角项再次随逆关节惯性缩放。

Genesis 支持三种完整等式：

类型	移除的自由度	描述
Connect	3	强制不同体上的两个点共享相同的世界位置。适合球窝关节。
Weld	6	保持两个坐标系在平移和旋转上都重合（可选缩放扭矩）。
Polynomial joint	1	将一个关节约束为另一个关节的多项式函数（对复杂机构有用）。

每个等式将行写入 J，使得它们的相对平移/旋转速度消失。柔软度和 Baumgarte 稳定再次来自每个约束的 sol_params。

求解器类实现两种可互换算法：

两个变体共享在 _func_linesearch 中实现的相同的线搜索逻辑，选择步长 (\alpha) 最小化二次模型同时尊重不等式激活/去激活。算法停止当

通过从上一子步的平滑加速度 acc_smooth 初始化来支持热启动。