计算

简介

本章介绍了 MuJoCo 的数学和算法基础。对于熟悉广义坐标或关节坐标建模与仿真的读者来说，整体框架是相当标准的，因此我们在此仅作简要总结。本章大部分篇幅致力于说明我们如何处理接触及其他约束。这种方法基于我们最近的研究，是 MuJoCo 所独有的，因此我们花时间对其动机进行了说明并作了详细解释。更多信息可在下方的论文中找到，尽管本章中的一些技术思想是新的，且尚未在其他地方描述过。

Analytically-invertible dynamics with contacts and constraints: Theory and implementation in MuJoCo E. Todorov (2014)（具有接触和约束的解析可逆动力学：MuJoCo 中的理论与实现）。

软接触模型

机器人和人类主要通过物理接触与环境进行交互。鉴于物理建模在机器人技术、机器学习、动画、虚拟现实、生物力学等领域的重要性日益增加，迫切需要既物理准确又计算高效的接触动力学仿真模型。仿真模型的一种应用是在物理系统上部署之前，评估候选的估计和控制策略。另一种应用是使这些策略的设计自动化——通常通过在内循环中使用仿真的数值优化来实现。后一种应用提出了额外的约束：相对于接触动力学定义的客观函数应易于进行数值优化。MuJoCo 底层的接触模型在这些及其他相关维度上具有优势。在接下来的章节中，我们将讨论其优势，同时阐明其与作为事实标准的线性互补 (LCP) 接触模型家族的区别。

物理真实感与软接触

我们接触模型的许多优势可以归结为我们摒弃了 LCP 公式核心的严格互补约束。我们将此类模型称为凸模型；相关工作见参考文献。对于无摩擦接触，放弃显式的互补约束并无影响，因为由此产生的凸二次规划的 Karush-Kuhn-Tucker (KKT) 最优性条件等同于 LCP。但对于摩擦接触，则存在差异。

如果将凸模型视为 LCP 的近似，一个合乎逻辑的问题是：这种近似有多好？然而，我们并不这样看。相反，我们将 LCP 模型和凸模型视为对物理现实的不同近似，各有优缺点。放弃严格互补并以成本（代价函数）替代它的直接后果是，互补性可能会被破坏——这意味着接触法线方向的力和速度可能同时为正，且摩擦力可能不会达到最大耗散。一个相关的现象是，启动滑动的唯一方法是在法线方向产生一定的运动。这些影响在数值上很小，但并不理想。然而，这一缺陷在实际应用中几乎没有意义，因为它基于硬接触的假设。然而，所有物理材料都允许一定的形变。这在机器人技术中尤为重要，因为机器人与环境接触的部件通常设计为软性的。对于软接触，必须破坏互补性：当存在渗透且材料将接触体推开时，法向力和速度均为正。此外，如果一个物体放在软表面上并产生一定程度的渗透，当我们横向推动它时，我们期望它在开始滑动时稍微向上移动一点。因此，在存在软接触的情况下，与 LCP 的偏差实际上增加了物理真实感。

当然，并非每个软模型都是可取的；例如，弹簧-阻尼器模型是软性的，但深受不稳定性困扰。同时，不同的材料具有不同的特性，因此与硬接触模型不同，如果软模型要适应多个感兴趣的系统，则必须具有足够丰富的参数化。这反过来促进了接触模型参数的系统辨识。

计算效率

带有摩擦接触的 LCP 模型对应于 NP-hard 优化问题。这催生了一个近似求解器产业，产生的一个不幸副作用是，许多流行的物理引擎使用了文档记录较差的捷径，导致产生的运动方程难以表征。公平地说，NP-hard 是对最坏情况性能的陈述，并不意味着在实践中快速求解 LCP 是不可能的。尽管如此，凸优化具有公认的优势。在 MuJoCo 中，我们观察到对于典型的机器人模型，投影 Gauss-Seidel 方法 (PGS) 的 10 次扫描就能产生在实际应用中与全局最小值难以区分的解。当然，有些问题即使是凸的，在数值上也更难求解，对于此类问题，我们提供了具有更高阶收敛性的共轭梯度求解器。

对计算效率的要求因用例而异。如果仅仅需要实时仿真，现代计算机即使使用低效的求解器，也足够处理大多数感兴趣的机器人系统。然而，在优化背景下，没有所谓的“足够快”的仿真。如果客观函数及其导数可以计算得更快，这意味着更大的搜索范围、训练集或样本量，进而带来性能的提升。这就是为什么我们在开发高效求解器上投入了大量精力。

连续时间

人们可能认为任何物理系统的运动方程在连续时间内都是唯一定义的。然而，摩擦接触是有问题的，因为库仑摩擦模型在连续时间内定义不明确（Painleve 悖论）。这使得离散时间近似和相关的速度步进方案非常流行。这些模型的连续时间极限很少被研究。对于单个接触，且在施加力的一些不一定现实的假设下，该极限满足库仑摩擦模型的微分包含形式；而对于多个同时接触，根据连续时间极限的具体取法，可能会有多个解。这些困难可以追溯到硬接触的假设。

摩擦接触的凸模型在过去也依赖于离散时间近似，但这并非必要。目前的模型定义在连续时间内，以力和加速度的形式表示。考虑到现实世界中的时间是连续的，这更自然。这也是控制文献中首选的公式化方法，实际上我们也希望 MuJoCo 能吸引该领域的开发者。连续时间公式的另一个优点是它们适用于复杂的数值积分，而无需支付离散时间变分积分器的计算开销（当惯性取决于配置时，这些积分器必然是隐式的）。连续时间动力学在时间上也是向后定义的，这在某些优化算法中是必需的。

逆动力学与优化

逆动力学的目标是在给定多关节系统的位置、速度和加速度的情况下，恢复所施加的力和接触力。对于硬接触，这种计算是不可能的。考虑在不移动的情况下推墙。除非我们考虑材料形变，否则无法从运动学中恢复接触力——在这种情况下，我们需要一个软接触模型。使用接触的弹簧-阻尼器模型，逆动力学很容易计算，因为在这种情况下，接触力仅是位置和速度的函数，而不依赖于所施加的力。但这也是弹簧-阻尼器模型不理想的原因：忽略施加的力意味着在每个时间步都会引入误差，从而使模拟器处于持续的误差修正模式，进而导致不稳定。相比之下，现代接触求解器在计算接触力/冲量时会考虑所施加的力（以及所有内力）。但这使求逆变得复杂。目前的接触模型具有唯一定义的逆。事实上，逆动力学的计算比正向动力学更容易，因为优化问题变成了对角线形式，并分解为单个接触上的独立优化问题——这些问题可以解析求解。

逆动力学在系统辨识、估计和控制的优化算法中起着关键作用。它们使得将位置序列（或其参数化表示）作为被优化对象成为可能。然后通过对位置求导来计算速度和加速度；使用逆动力学来计算施加力和接触力；最后构建一个可以依赖于上述所有内容的客观函数。这被交替称为时空优化、谱方法、直接配置。MuJoCo 具有独特的优势，可以在存在接触和其他约束的情况下促进此类计算。

通用框架

我们的符号总结在下表中。特定于约束的额外符号将在后面介绍。在可用时，我们还会显示对应于数学符号的主要数据结构 mjModel 和 mjData 的字段。

符号	大小	描述	MuJoCo 字段
\(\nq\)		位置坐标数量	mjModel.nq
\(\nv\)		自由度数量	mjModel.nv
\(\nc\)		活动约束数量	mjData.nefc
\(q\)	\(\nq\)	关节位置	mjData.qpos
\(v\)	\(\nv\)	关节速度	mjData.qvel
\(\tau\)	\(\nv\)	施加力：被动、驱动、外部	mjData.qfrc_passive + mjData.qfrc_actuator + mjData.qfrc_applied
\(c(q, v)\)	\(\nv\)	偏置力：科里奥利力、离心力、重力	mjData.qfrc_bias
\(M(q)\)	\(\nv \times \nv\)	关节空间惯性	mjData.qM
\(J(q)\)	\(\nc \times \nv\)	约束雅可比矩阵	mjData.efc_J
\(r(q)\)	\(\nc\)	约束残差	mjData.efc_pos
\(f(q, v,\tau)\)	\(\nc\)	约束力	mjData.efc_force

所有模型元素在编译时被枚举并组装成上述系统级向量和矩阵。在我们早期的手臂模型示例中，该模型具有 \(\nv = 13\) 个自由度：球关节 3 个，4 个铰链关节各 1 个，自由浮动对象 6 个。它们在所有维度为 \(\nv\) 的系统级向量和矩阵中以相同的顺序出现。对应于给定模型元素的数据可以通过索引操作恢复，如概览章节的澄清部分所示。维度为 \(\nc\) 的向量和矩阵则有所不同，因为活动约束会在运行时发生变化。在这种情况下，仍然存在固定的枚举顺序（对应于模型元素在 mjModel 中出现的顺序），但任何非活动约束都会被省略。

每当使用四元数表示 3D 方向时，位置坐标的数量 \(\nq\) 就会大于自由度的数量 \(\nv\)。当模型包含球关节或自由关节时（即在大多数模型中），就会出现这种情况。在这种情况下，\(\dot{q}\) 不等于 \(v\)，至少在通常意义上不是这样。相反，必须考虑刚体方向的群 \(SO(3)\)——它在 4D 空间中具有单位球面的几何形状。速度存在于该球面的 3D 切空间中。所有内部计算都考虑到了这一点。对于自定义计算，MuJoCo 提供了函数 mj_differentiatePos，它“减去”两个维度为 \(\nq\) 的位置向量并返回一个维度为 \(\nv\) 的速度向量。还提供了一些与四元数相关的实用函数。

MuJoCo 在连续时间内计算正向和逆向动力学。然后，正向动力学使用选定的数值积分器在指定的 mjModel.opt.timestep 上进行积分。连续时间内的通用运动方程为

(1)\[M \dot{v} + c = \tau + J^T f \]

雅可比矩阵建立了关节坐标和约束坐标中数量之间的关系。它将运动向量（速度和加速度）从关节坐标映射到约束坐标：关节速度 \(v\) 映射到约束坐标中的速度 \(J v\)。雅可比矩阵的转置将力向量从约束坐标映射到关节坐标：约束力 \(f\) 映射到关节坐标中的力 \(J^T f\)。

关节空间惯性矩阵 \(M\) 总是可逆的。因此，一旦已知约束力 \(f\)，我们就可以完成正向和逆向动力学计算，如下所示

\[\begin{aligned} \text{正向:} & & \dot{v} &= M^{-1} (\tau + J^T f - c) \\ \text{逆向:} & & \tau &= M \dot{v} + c - J^T f \\ \end{aligned} \]

约束力的计算是困难的部分，将在后面描述。但首先，我们通过总结如何计算上述达到约束雅可比矩阵的量，来完成通用框架的描述。

• 施加的力 \(\tau\) 包括来自弹簧-阻尼器和流体动力学的被动力、驱动力以及用户指定的额外力。

• 偏置力 \(c\) 包括科里奥利力、离心力和重力。它们的总和是使用加速度设置为 0 的递归牛顿-欧拉 (RNE) 算法计算得出的。

• 关节空间惯性矩阵 \(M\) 是使用复合刚体 (CRB) 算法计算的。该矩阵通常非常稀疏，我们以针对运动学树定制的自定义格式来表示它。

• 由于我们经常需要将向量乘以 \(M\) 的逆，我们以保留稀疏性的方式计算其 \(L^T D L\) 分解。当稍后需要形式为 \(M^{-1} x\) 的量时，通过稀疏回代计算。

在进行任何这些计算之前，我们应用正向运动学，它计算所有空间对象的全局位置和方向以及关节轴。虽然通常建议在局部坐标中应用 RNE 和 CRB，但在这里我们为碰撞检测奠定了基础，碰撞检测是在全局坐标中完成的，因此 RNE 和 CRB 也在全局坐标中实现。尽管如此，为了提高浮点精度，我们将每个运动学子树的数据表示在以子树质心为中心的全局框架中（mjData 中以 c 开头的字段）。仿真流水线的详细总结在本章末尾给出。

驱动模型

MuJoCo 提供了一个灵活的执行器模型。所有执行器均为单输入单输出 (SISO)。执行器 \(i\) 的输入是用户指定的标量控制 \(u_i\)。输出是一个标量力 \(p_i\)，它通过由传动机构确定的力臂向量映射到关节坐标。执行器还可以具有其自身动力学的激活状态 \(w_i\)。所有执行器的控制输入存储在 mjData.ctrl 中，力输出存储在 mjData.actuator_force 中，激活状态（如果有）存储在 mjData.act 中。

执行器的这三个组件——传动、激活动力学和力生成——决定了执行器的工作方式。用户可以独立设置它们以获得最大的灵活性，或者使用执行器快捷方式，它们实例化常见的执行器类型。

传动

每个执行器都有一个由传动类型及其参数定义的标量长度 \(l_i(q)\)。梯度 \(\nabla l_i\) 是一个 \(\nv\) 维的力臂向量。它决定了从标量执行器力到关节力的映射。传动属性由执行器所附着的 MuJoCo 对象决定；可能的附着对象类型为 joint（关节）、tendon（肌腱）、jointinparent（父级中的关节）、slider-crank（滑块曲柄）、site（位点）和 body（主体）。

joint 和 tendon

joint 和 tendon 传动类型按预期工作，对应于执行器对目标对象施加力和扭矩。球关节比较特殊，有关更多详细信息，请参阅 actuator/general/joint 文档。

jointinparent

jointinparent 传动是球关节和自由关节所独有的，它断言旋转应在父级而非子级框架中测量。

slider-crank

slider-crank 传动装置将线性力转换为扭矩，就像在活塞驱动的内燃机中一样。此模型包含教学示例。滑块曲柄也可以通过创建 MuJoCo 主体并将它们与等式约束耦合来显式建模，但这既效率较低也不太稳定。

body

body 传动对应于在属于主体的接触点施加力，以模拟真空吸盘和生物力学粘附附属物。有关粘附的更多信息，请参阅 adhesion 执行器文档。这些传动目标具有固定的零长度 \(l_i(q) = 0\)。

site

Site 传动对应于在 site 的框架中施加笛卡尔力和扭矩。当未定义 refsite（参考点，见下文）时，这些目标具有固定的零长度 \(l_i(q) = 0\)，对于模拟喷气发动机和螺旋桨很有用：即固定在 site 框架上的力和扭矩。

如果定义了带有可选 refsite 属性的 site 传动，则力和扭矩将在参考点（reference site）的框架中施加，而不是在 site 自身的框架中施加。如果定义了参考点，执行器的长度将非零，并对应于两个 site 的姿态差，投影到参考框架中的选定方向上。然后可以使用 position 执行器控制此长度，从而实现笛卡尔末端执行器控制。有关更多详细信息，请参阅 refsite 文档。

状态执行器

一些执行器（例如气动和液压缸以及生物肌肉）具有称为“激活（activation）”的内部状态。这是一个真正的动态状态，超出了关节位置 \(q\) 和速度 \(v\)。在模型中包含此类执行器会导致三阶动力学。我们用 \(w\) 表示执行器激活向量。它们具有一些一阶动力学

\[\dot{w}_i \left( u_i, w_i, l_i, \dot{l}_i \right) \]

由激活类型和相应的模型参数决定。请注意，每个执行器都有独立于其他执行器的标量动力学。目前实现的激活类型有

\[\begin{aligned} \text{integrator (积分器)}: & & \dot{w}_i &= u_i \\ \text{filter (滤波器)}: & & \dot{w}_i &= (u_i - w_i) / \texttt{t} \\ \text{filterexact (精确滤波器)}: & & \dot{w}_i &= (u_i - w_i) / \texttt{t} \\ \text{muscle (肌肉)}: & & \dot{w}_i &= \textrm{muscle}(u_i, w_i, l_i, \dot{l}_i) \end{aligned} \]

其中 \(\texttt{t}\) 是存储在 mjModel.actuator_dynprm 中的执行器特定时间常数。此外，类型可以是“user”，在这种情况下，\(w_i\) 由用户定义的回调函数 mjcb_act_dyn 计算。类型也可以是“none”，这对应于没有激活状态的常规执行器。 \(w\) 的维度等于激活类型不同于“none”的执行器数量。

有关肌肉激活动力学的更多信息，请参阅 肌肉。

对于 filterexact 激活动力学，\(\dot{w}\) 的欧拉积分被解析积分所取代

\[\begin{aligned} \text{filter}: & & w_{i+1} &= w_i + h (u_i - w_i) / \texttt{t} \\ \text{filterexact}: & & w_{i+1} &= w_i + (u_i - w_i) (1 - e^{-h / \texttt{t}}) \\ \end{aligned} \]

这两个表达式在 \(h \rightarrow 0\) 极限下收敛到相同的值。请注意，欧拉积分滤波器在 \(\texttt{t} < h\) 时发散，而精确积分滤波器在任何正数 \(\texttt{t}\) 下都是稳定的。

actearly:

如果 actearly 属性设置为“true”，则 mjData.actuator_force 基于 \(w_{i+1}\)（下一次激活）计算，将 \(u\) 的变化与其对加速度影响之间的延迟减少了一个时间步（因此总动力学是二阶而非三阶）。

力生成

每个执行器都会产生一个标量力 \(p_i\)，它是某种函数

\[p_i \left( u_i, w_i, l_i, \dot{l}_i \right) \]

与激活动力学类似，力生成机制是执行器特定的，不能与模型中的其他执行器交互。目前，力在存在激活状态时是激活状态的仿射函数，否则是控制的仿射函数

\[p_i = (a w_i \; \text{或} \; a u_i) + b_0 + b_1 l_i + b_2 \dot{l}_i \]

此处 \(a\) 是执行器特定的增益参数，\(b_0, b_1, b_2\) 是执行器特定的偏置参数，分别存储在 mjModel.actuator_gainprm 和 mjModel.actuator_biasprm 中。增益和偏置参数的不同设置可用于模拟直接力控制以及位置和速度伺服——在这种情况下，控制/激活具有参考位置或速度的含义。还可以通过安装回调函数 mjcb_act_gain 和 mjcb_act_bias 并将增益和偏置类型设置为“user”来计算自定义增益和偏置项。请注意，仿射力生成使得可以使用力臂矩阵的伪逆，从逆动力学中计算出的施加力中推断出控制/激活。然而，现实世界中使用的一些执行器并非仿射的（特别是那些具有嵌入式低级控制器的执行器），因此我们正在考虑对上述模型进行扩展。

综合所有这些，由所有执行器在广义坐标中贡献的净力为

\[\sum_i \nabla l_i(q) \; p_i \left(u_i, w_i, l_i(q), \dot{l}_i(q, v) \right) \]

此量存储在 mjData.qfrc_actuator 中。它与任何用户定义的关节或笛卡尔坐标中的力（分别存储在 mjData.qfrc_applied 和 mjData.xfrc_applied 中）一起被添加到施加的力向量 \(\tau\) 中。

被动力

被动力定义为仅依赖于位置和速度，而不依赖于正向动力学中的控制或逆向动力学中的加速度的力。因此，此类力是正向和逆向动力学计算的输入，在两种情况下都是相同的。它们存储在 mjData.qfrc_passive 中。MuJoCo 计算的被动力在物理学意义上也是被动的，即它们不会增加能量，但用户可以安装回调函数 mjcb_passive，并将可能增加能量的力添加到 mjData.qfrc_passive 中。只要此类用户力仅依赖于位置和速度，这就不会干扰 MuJoCo 的运行。

MuJoCo 可以计算三种类型的被动力

• 关节和肌腱中的弹簧-阻尼器。有关详细信息，请参阅以下属性。
  关节: stiffness（刚度）, springref（弹簧参考）, damping（阻尼）, springdamper（弹簧阻尼）。
  肌腱: stiffness, springlength, damping。

• 重力补偿力。有关详细信息，请参阅主体 gravcomp 属性。

• 周围介质施加的流体力。有关详细信息，请参阅 流体力 章节。

多项式力

非线性刚度（关节，肌腱）和阻尼（关节，肌腱）由 mjNPOLY + 1 阶的多项式 \(f\) 定义。施加到系统的实际力是 \(-f\)，这意味着保持符号的函数产生恢复力（刚度）或耗散力（阻尼）。

刚度多项式采用标准形式（其中 \(x\) 是位移）

\[f(x) = a x + b x^2 + c x^3 + \dots \]

阻尼多项式采用反对称形式（其中 \(v\) 是速度）

\[f(v) = a v + b v |v| + c v^3 + \dots \]

反对称化

阻尼多项式使用反对称的偶次幂单项式（例如 \(v^2 \rightarrow v|v|\)），从而使函数为奇函数：\(f(-v) = -f(v)\)。这保证了力会随着速度反转方向。这种公式化在物理上也是有依据的，因为某些自然形式的阻尼（如流体阻力）表现出反对称的二次剖面。

相比之下，非对称（或更确切地说是非反对称）刚度剖面在物理上很常见（例如生物筋膜），这使得标准多项式形式及其泰勒级数便利性更为适用。

符号保持

在这两种情况下，合理的系数选择通常满足符号保持条件 \(z \cdot f(z) \geq 0\)。该条件等同于要求 \(f\) 的积分（刚度的势能和阻尼的耗散）是全局凸的，且在原点处有最小值。

• 对于刚度，违反该条件会产生排斥力和/或产生多个平衡点。

• 对于阻尼，违反该条件会产生非耗散力，将机械能注入系统。

符号保持条件不由编译器强制执行；用户有责任确保满足该条件。最高 3 阶的系数的解析条件为

\[\begin{aligned} \textrm{标准:} \qquad & a \geq 0, \qquad c \geq 0, \qquad b^2 \leq 4 a c \\ \textrm{反对称:} \qquad & a \geq 0, \qquad c \geq 0, \qquad b < 0 \implies b^2 \leq 4 a c \end{aligned} \]

mjModel 字段

尽管 MJCF 接受将系数作为单个数组（正如 mjs 层 所做的那样），但 mjModel 中的线性系数与高阶系数是分开存储的。例如，如果 joint/stiffness = “a b c”，则 jnt_stiffness[i] = a，jnt_stiffnesspoly[i*mjNPOLY] = b，并且 jnt_stiffnesspoly[i*mjNPOLY + 1] = c。C-API 未来的重大更改可能会将线性系数和高阶系数统一到一个数组中。

数值积分

MuJoCo 在连续时间内计算正向和逆向动力学。正向动力学的最终结果是关节加速度 \(a=\dot{v}\) 以及模型中存在时的执行器激活 \(\dot{w}\)。这些用于将仿真时间从 \(t\) 前进到 \(t+h\)，并更新状态变量 \(q, v, w\)。

提供了四种数值积分器，三种单步积分器和多步四阶龙格-库塔 (Runge-Kutta) 积分器。在描述积分器之前，我们首先概述单步欧拉积分器：显式、半隐式和速度隐式。显式欧拉方法不被 MuJoCo 支持，但具有教学价值。它可以写为

(2)\[\begin{aligned} \textrm{激活:}\quad w_{t+h} &= w_t + h \dot{w}_t \\ \textrm{速度:}\quad v_{t+h} &= v_t + h a_t \\ \textrm{位置:}\quad q_{t+h} &= q_t + h v_t \end{aligned}\]

请注意，在存在四元数的情况下，操作 \(q_t + h v_t\) 比简单的加法更复杂，因为 \(q\) 和 \(v\) 的维度不同。不实现显式欧拉的原因是，以下称为半隐式欧拉的公式严格更好，并且是物理仿真中的标准

(3)\[ \begin{aligned} v_{t+h} &= v_t + h a_t \\ q_{t+h} &= q_t + h v_{\color{red}t+h} \end{aligned}\]

比较 (2) 和 (3)，我们可以看到在半隐式欧拉中，位置是使用新速度更新的。隐式欧拉意味着

(4)\[\begin{aligned} v_{t+h} &= v_t + h a_{\color{red}t+h} \\ q_{t+h} &= q_t + h v_{t+h} \end{aligned}\]

比较 (3) 和 (4)，我们看到速度更新右侧的加速度 \(a_{t+h}=\dot{v}_{t+h}\) 是在下一个时间步评估的。虽然在不步进的情况下无法评估下一个加速度，但我们可以使用一阶泰勒展开来近似此量，并执行牛顿法的一步迭代。当展开仅相对于速度（而非位置）时，积分器称为速度隐式欧拉。这种方法在不稳定性是由速度相关力引起的系统中特别有效：多关节摆、在空间中翻滚的物体、具有升力和阻力的系统，以及在肌腱和执行器中具有显著阻尼的系统。将加速度写为速度的函数 \(a_t = a(v_t)\)，我们旨在近似的速度更新为

\[v_{t+h} = v_t + h a(v_{t+h}) \]

这是未知向量 \(v_{t+h}\) 中的非线性方程，可以在每个时间步使用 \(a(v_{t+h})\) 在 \(v_t\) 附近的一阶展开进行数值求解。回想一下，正向动力学为

(5)\[a(v) = M^{-1} \big(\tau(v) - c(v) + J^T f(v)\big)\]

因此，我们定义导数

\[\begin{aligned} {\partial a(v) \over \partial v} &= M^{-1} D \\ D &\equiv {\partial \over \partial v} \Big(\tau(v) - c (v) + J^T f(v)\Big) \end{aligned} \]

对应于牛顿法的速度更新如下。首先，我们将右侧展开到一阶

\[\begin{aligned} v_{t+h} &= v_t + h a(v_{t+h}) \\ &\approx v_t + h \big( a(v_t) + {\partial a(v) \over \partial v} \cdot (v_{t+h}-v_t) \big) \\ &= v_t + h a(v_t) + h M^{-1} D \cdot (v_{t+h}-v_t) \end{aligned} \]

预乘以 \(M\) 并重排得到

\[(M-h D) v_{t+h} = (M-h D) v_t + h M a(v_t) \]

求解 \(v_{t+h}\)，我们得到速度隐式更新

(6)\[\begin{aligned} v_{t+h} &= v_t + h \widehat{M}^{-1} M a(v_t) \\ \widehat{M} &\equiv M-h D \end{aligned}\]

真空中自由物体的中点积分

速度隐式更新 (6) 将加速度视为速度的函数并进行线性化。虽然对阻尼类力有效，但对于旋转动力学而言，由于科里奥利力和陀螺力在角速度上是二次的，这种方法次优。对于这种情况，更好的方法是使用中点法直接离散化旋转运动方程。

考虑一个在主轴框架内旋转的刚体，其角速度为 \(\omega \in \mathbb{R}^3\)，惯性张量为对角矩阵 \(I = \text{diag}(I_1, I_2, I_3)\)。旋转动力学由欧拉旋转方程给出

\[I \dot{\omega} + \omega \times I\omega = \tau \]

其中 \(\tau\) 是主轴框架中的外部扭矩。在中点处评估速度 \(\omega_\text{mid} = (\omega_t + \omega_{t+h})/2\)，得到

\[\frac{2}{h} I (\omega_\text{mid} - \omega_t) + \omega_\text{mid} \times I \omega_\text{mid} = \tau \]

这是一个关于 3 个未知数 \(\omega_\text{mid}\) 的 3 个非线性方程组，在每个时间步使用带有回溯线搜索的牛顿法求解。求解后，新速度恢复为 \(\omega_{t+h} = 2\omega_\text{mid} - \omega_t\)。

属性。 中点法保留了 ODE 的所有二次第一积分。对于欧拉方程，这些是动能 \(H = \frac{1}{2}\omega^T I\omega\) 和角动量平方 \(C = \frac{1}{2}|I\omega|^2\)，两者在没有外部扭矩的情况下精确守恒。由于 \(C\) 是 Lie-Poisson 结构的 Casimir 函数，中点法是一种对称（时间可逆）且二阶准确的泊松积分器。

适用性。 中点积分仅在使用 implicitfast 积分器时，应用于没有子主体的自由主体，且仅当介质具有零密度和粘度时才适用。

性能。 虽然中点法带来了计算开销，但我们发现它与流水线的其余部分相比可以忽略不计，最坏情况下约为 1%。

禁用。 由于中点积分求解下一个速度的非线性方程，它破坏了离散逆动力学所假设的有限差分速度与力之间的线性关系。因此，设置 invdiscrete 标志会禁用中点积分，同时也为此积分器提供了一个通用的选择退出机制。

积分器

MuJoCo 支持四种积分器：三种单步积分器和多步四阶龙格-库塔积分器。MuJoCo 中的所有三种单步积分器都使用更新 (6)，并使用 \(D\) 矩阵的不同定义，该矩阵总是解析计算的。

具有隐式关节阻尼的半隐式积分器（Euler）

对于此方法，\(D\) 仅包括关节阻尼的导数。请注意，在这种情况下 \(D\) 是对角矩阵，\(\widehat{M}\) 是对称矩阵，因此可以使用 \(L^TL\) 分解（Cholesky 的一种变体）。此分解存储在 mjData.qH 中。如果模型没有关节阻尼或设置了 eulerdamp 禁用标志，则禁用隐式阻尼并使用半隐式更新 (3)，而不是 (6)，避免了 \(\widehat{M}\) 的额外分解（额外是因为 \(M\) 已经为加速度更新 (5) 进行了分解）。

速度隐式（implicit）

对于此方法，\(D\) 包括除约束力 \(J^T f(v)\) 之外的所有力的导数。这些目前被忽略，因为尽管计算它们是可能的，但很复杂，数值测试表明包含它们并没有带来太多好处。也就是说，计划在未来版本中实现约束力的解析导数。此外，为了计算效率，我们将 \(D\) 限制为与 \(M\) 具有相同的稀疏模式。此限制将排除连接处于运动学树不同分支上的物体的肌腱阻尼。由于 \(D\) 不是对称的，我们不能使用 Cholesky 分解，但因为 \(D\) 和 \(M\) 具有对应于运动学树拓扑的相同稀疏模式，所以 \(\widehat{M}\) 的反向顺序 \(LU\) 分解保证没有填充（no fill-in）。此分解存储在 mjData.qLU 中。

快速速度隐式（implicitfast）

对于此方法，\(D\) 包括隐式方法中使用的所有力的导数，RNE 算法计算的向心力和科里奥利力 \(c (v)\) 除外。此外，它是对称化的 \(D \leftarrow (D + D^T)/2\)。放弃 RNE 导数的原因之一是它们计算最昂贵。其次，这些力仅在复杂摆和旋转体的高旋转速度下才会发生剧烈变化，这种情况并不常见，且已被龙格-库塔积分器很好地处理（见下文）。由于 RNE 导数也是 \(D\) 不对称性的主要来源，通过放弃它们并进行对称化，我们可以使用更快的 \(L^TL\) 而不是 \(LU\) 分解。implicitfast 积分器将中点积分应用于真空中合格的自由主体，以可忽略的额外成本为旋转物体提供精确的能量守恒。

四阶龙格-库塔（RK4）

我们连续时间公式的一个优势是我们可以使用更高阶的积分器，例如龙格-库塔或多步方法。MuJoCo 实现了固定步长的四阶龙格-库塔方法，尽管用户可以通过调用 mj_forward 并自行积分加速度来轻松实现其他积分器。我们观察到，对于能量守恒系统（示例），RK4 在稳定性和准确性方面都定性地优于单步方法，即使时间步长减小了 4 倍（因此计算工作量是相同的）。在存在大速度相关力的情况下，如果选定的单步方法隐式积分这些力，单步方法可以显著比 RK4 更稳定。

选择时间步长和积分器

时间步长

所有积分器的准确性和稳定性都可以通过减小时间步长 \(h\) 来提高。当然，较小的时间步长也会减慢仿真速度。时间步长可能是用户可以调整的最重要的参数。如果太大，仿真将变得不稳定。如果太小，CPU 时间将被浪费，而准确性没有实质性的提高。总有一个时间步长“恰到好处”的舒适范围，但该范围取决于模型。

积分器

总结：推荐的积分器是 implicitfast，它通常具有稳定性与性能的最佳权衡。

Euler:

使用 Euler 以兼容旧模型。

implicitfast:

implicitfast 积分器的计算成本与 Euler 相似，但提供了更高的稳定性，因此是严格的改进。它是大多数模型的推荐积分器。

implicit:

与 implicitfast 相比的优势在于对耦合旋转系统（如多连杆摆）的科里奥利力和向心力进行隐式积分。请注意，implicit 不应用中点积分（只有 implicitfast 会），但其 RNE 导数为自由主体旋转提供了相当的稳定性。例如，gyroscopic.xml 展示了一个在斜面上滚动的椭球体；implicitfast 和 implicit 都很好地处理了这种情况，而 Euler 迅速发散。

RK4:

此积分器最适合能量守恒或几乎能量守恒的系统。pendulum.xml 展示了一个复杂的摆机构，使用 Euler 或 implicitfast 会迅速发散，但在 RK4 下能很好地守恒能量。请注意，在 implicit 下，该模型不会发散，而是会损失能量。

状态

为了完成我们的通用框架描述，我们将快速讨论状态的概念。MuJoCo 具有紧凑、定义良好的内部状态，这与确定性流水线一起，意味着（重新）设置状态和计算动力学导数等操作也得到了很好的定义。

状态完全封装在 mjData 结构中，由几个组件组成。组件在 mjtState 中作为位标志枚举。可以使用 mj_getState 和 mj_setState 方便地从 mjData 读取和写入级联状态向量。

更多详细信息可以在仿真章节的状态与控制部分找到。

约束模型

MuJoCo 具有一个非常灵活的约束模型，它通过后面描述的求解器以统一的方式进行处理。在这里，我们从概念上解释单个约束是什么，以及它们如何布置在维度为 \(\nc\) 的系统级向量和矩阵中。每个概念性约束可以向总数 \(\nc\) 贡献一个或多个标量约束，并且每个标量约束在约束雅可比矩阵 \(J\) 中都有对应的一行。活动约束按类型顺序排列，顺序为下述类型描述顺序，然后在每种类型内按模型元素排序。类型为：等式、摩擦损失、限制、接触。限制在求解器中作为无摩擦接触处理，内部不作为单独类型处理。我们在 mjData 中使用前缀 efc 来表示具有约束相关数据的系统级向量和矩阵。

等式

MuJoCo 可以以通用形式 \(r(q) = 0\) 对等式约束进行建模，其中 \(r\) 可以是位置向量 \(q\) 的任何可微标量或向量函数。它具有残差的语义。求解器实际上也可以处理非完整约束，但我们尚未定义此类约束类型。每个等式约束向总约束计数 \(\nc\) 贡献 \(\dim(r)\) 个元素。\(J\) 中的对应块仅仅是残差的雅可比矩阵，即 \(\partial r / \partial q\)。请注意，由于四元数的性质，相对于 \(q\) 的微分产生大小为 \(\nv\) 而非 \(\nq\) 的向量。

在其他应用中，等式约束可用于创建“循环关节”，即无法通过运动学树建模的关节。游戏引擎以这种方式表示所有关节。MuJoCo 中也可以这样做，但不推荐——因为它会导致仿真速度变慢且精度降低，实际上将 MuJoCo 变成了游戏引擎。使用等式约束表示关节的唯一理由是模拟软关节——这可以通过约束求解器完成，但不能通过运动学树完成。

接下来描述五种类型的等式约束。标题中的数字对应于每种情况下约束残差的维度。

connect3

此约束在一点上连接两个主体，有效地在运动学树之外创建一个球关节。模型指定要连接的两个主体，以及每个主体局部框架中的一个点（或“锚点”）。约束残差定义为这些点的全局 3D 位置之间的差值。请注意，为同一对主体指定两个连接约束可用于在运动学树之外建模一个铰链关节。指定三个或更多（其锚点不共线）的连接约束在数学上等同于焊接约束，但计算效率较低。

weld6

此约束将两个主体焊接在一起，抑制它们之间的所有相对自由度。约束求解器强制执行的相对主体位置和方向是 mjModel 中的参数。编译器根据定义模型的初始配置（即 mjModel.qpos0）计算它们，但用户稍后可以更改它们。6D 残差具有与连接约束相同的 3D 位置分量，后跟一个 3D 方向分量。后者定义为 \(\sin(\theta/2) (x, y, z)\)，其中 \(\theta\) 是弧度表示的旋转角度，\((x, y, z)\) 是对应于旋转轴的单位向量。对于小角度，这接近方向差的指数映射表示（模一个 2 的因子）。对于大角度，它避免了如果我们使用 \(\theta\) 而不是 \(\sin(\theta/2)\) 会导致的环绕不连续性。不过它确实有一个缺点：当角度接近 180 度时，约束变弱。还要注意，如果一个主体是另一个主体的子主体，实现焊接约束更快且更准确的方法是简单地删除子主体中定义的所有关节。

joint1

此约束仅适用于标量关节：铰链和滑动。它可用于将一个关节锁定在固定位置，或通过四次多项式耦合两个关节。锁定关节最好通过删除关节来实现，但在某些特殊情况下（例如通过软等式约束模拟反冲）可能会很有用。两个关节的耦合对于模拟螺旋关节或其他形式的机械耦合很有用。四次多项式模型定义如下。假设 \(y\) 是第一个关节的位置，\(x\) 是第二个关节的位置，下标 0 表示模型处于初始配置 mjModel.qpos0 时的相应关节位置。那么等式约束为

\[y-y_0 = a_0 + a_1 \left( x-x_0 \right) + a_2 \left( x-x_0 \right)^2 + a_3 \left( x-x_0 \right)^3 + a_4 \left( x-x_0 \right)^4 \]

其中 \(a_0, \ldots, a_4\) 是模型中定义的系数。如果约束仅涉及一个关节，它简化为 \(y-y_0 = a_0\)。

tendon1

此约束与上述关节约束非常相似，但适用于肌腱长度而不是关节位置。肌腱是依赖于位置向量的长度量。这种依赖性可以是标量关节位置的线性组合，或者是绕过空间障碍的最小长度字符串。与位置在模型配置 mjModel.qpos0 中可以直接从位置向量读取的关节不同，肌腱长度的计算不太简单。这就是为什么所有肌腱的“静止长度”都由编译器计算并存储在 mjModel 中的原因。通常，所有名称以 0 结尾的 mjModel 字段都是编译器在初始模型配置 mjModel.qpos0 中计算出的量。

distance1

注意

距离等式约束已在 MuJoCo 2.2.2 版本中删除。如果您使用的是更早的版本，请切换到相应版本的文档。

摩擦损失

摩擦损失也称为干摩擦、静摩擦或与载荷无关的摩擦（与随法向力缩放的库仑摩擦相对）。与阻尼或粘度类似，它具有抵抗运动的作用。然而，它在运动开始前先发制人地起作用，因此它不能建模为与速度相关的力。相反，它被建模为一种约束，即摩擦力所能产生的绝对值上限。此限制通过相应模型元素的属性 frictionloss 指定，并可应用于关节和肌腱。

摩擦损失与其他所有约束类型不同，因为它没有与之关联的位置残差；因此我们形式上将 \(r(q)\) 的对应分量设置为零。事实上，我们稍后会看到我们的约束求解器公式需要以一种不寻常的方式扩展以纳入此约束。尽管如此，受影响的关节或肌腱的速度充当了速度“残差”——因为约束的作用是降低此速度并理想地使其保持为零。因此，约束雅可比矩阵中的对应块仅仅是关节位置（或肌腱长度）相对于 \(q\) 的雅可比矩阵。对于标量关节，这是一个 0 向量，在关节地址处有一个 1。对于肌腱，这被称为力臂向量。

joint1, 3 或 6

摩擦损失不仅可以为标量关节（滑动和铰链）定义，还可以为具有 3 个自由度的球关节和具有 6 个自由度的自由关节定义。定义时，它会独立应用于受影响关节的所有自由度。frictionloss 参数具有与关节（线性或角度）兼容的隐含单位。自由关节是一个例外，因为它们既有线性分量又有角度分量，并且 MJCF 模型格式允许每个关节使用单个 frictionloss 参数。在这种情况下，相同的参数用于线性和角度分量。有人可能会争辩说，自由关节中的摩擦损失不应被允许。但是我们允许它，因为它可以模拟有用的非物理效果，例如将物体保持在原位，直到有东西用足够的力推动它。

tendon1

肌腱是标量量，因此为肌腱定义摩擦损失总是会增加一个标量约束。对于空间肌腱，这可用于模拟肌腱与它所环绕的表面之间的摩擦。不过，这种摩擦将是与载荷无关的。要构建此现象的更详细模型，请创建几个小的浮动球体，并将它们与串联的肌腱连接起来。然后球体与周围表面之间的接触将具有与载荷相关的（即库仑）摩擦，但这模拟效率较低。

限制

限制以及接触具有定义良好的空间残差，但与等式约束不同，它们是单侧的，即它们引入的是不等式而非等式约束。限制可以为关节和肌腱定义。这是通过将相应的模型元素标记为“limited”（受限）并定义其“range”（范围）参数来完成的。残差 \(r(q)\) 是当前位置/长度与 range 中指定的两个限制值中较近的一个之间的距离。该距离的符号会自动调整，以便如果尚未达到限制，它为正；在限制处为零；如果违反限制，则为负。当此距离低于“margin”（边界）参数时，约束变为活动状态。然而，这与通过 margin 偏移限制并将 margin 设置为 0 不同。相反，约束力通过后面描述的求解器参数依赖于距离。

有可能给定关节或肌腱的下限和上限都变为活动状态。在这种情况下，它们都包含在标量约束列表中，但应避免这种情况——通过增加范围或减小 margin。特别是，避免使用窄范围来近似等式约束。而是使用显式的等式约束，如果需要一些松弛，通过调整求解器参数使约束变软。这在计算上更有效，不仅因为它涉及一个而不是两个标量约束，还因为求解等式约束力通常更快。

joint1 或 2

限制可以为标量关节（铰链和滑动）以及球关节定义。标量关节按上述方式处理。球关节限制应用于关节四元数的指数映射或角度-轴表示，即向量 \((\theta x, \theta y, \theta z)\)，其中 \(\theta\) 是旋转角度，\((x, y, z)\) 是对应于旋转轴的单位向量。限制应用于旋转角度 \(\theta\) 的绝对值。在运行时，限制由两个 range 参数中的较大者决定。然而，为了语义清晰，应该使用第二个 range 参数来指定限制，并将第一个 range 参数设置为 0。此规则由编译器强制执行。

tendon1 或 2

肌腱是标量量，它们的限制按上述方式处理。请注意，固定肌腱（它们是标量关节位置的线性组合）可以具有正和负“长度”，因为关节位置是相对于关节参考定义的，并且可以是正的也可以是负的。然而，空间肌腱具有不能为负的真实长度。在为肌腱限制设置范围和 margin 时请记住这一点。

接触

接触是最复杂的约束类型，无论是在模型中指定它们方面，还是在需要执行的计算方面。这是因为接触建模本身就具有挑战性，而且我们支持通用的接触模型，允许切向、扭转和滚动摩擦，以及椭圆和金字塔摩擦锥。

MuJoCo 使用点接触，在几何上由两个几何体（geoms）之间的一个点和一个以该点为中心的、均以全局坐标表示的空间框架定义。该框架的第一个轴（\(x\)）是接触法线方向，而其余（\(y\) 和 \(z\)）轴定义切平面。人们可能期望法线对应于 \(z\) 轴，如 MuJoCo 的可视化约定，但我们支持仅使用法线轴的无摩擦接触，这就是为什么我们希望法线位于第一位置的原因。与限制类似，当两个几何体分开时，接触距离为正；当它们接触时为零；当它们渗透时为负。接触点位于法线轴上两个表面之间的中间位置（对于网格碰撞，这可能是近似的）。碰撞检测是下面详细讨论的一个单独主题。我们现在只需要碰撞检测器给出接触点、空间框架和法线距离。

除了在线计算的上述量外，每个接触都有几个从模型定义中获得的参数。

参数	描述
condim	接触框架中接触力/扭矩的维度。 它可以是 1、3、4 或 6。
friction	维度为 condim-1 的摩擦系数向量。具体系数的语义见下文。
margin	几何体表面的几何膨胀。接触在距离低于 margin + gap 时被检测到，接触力在距离低于 margin 时产生。
gap（间隙）	超出 margin 的额外检测缓冲区。距离在 margin 和 margin + gap 之间的接触包含在 mjData.contact 中作为非活动接触，但不产生接触力。这对于远程作用效果很有用，例如通过 adhesion 执行器。
solref 和 solimp	求解器参数，稍后解释。

margin 和 gap

每个几何体都有一个 margin 和一个 gap 参数，定义在上表中。考虑两个几何体之间的接触时，这两个参数的值会被求和。它们共同定义了接触检测和力生成的三个区域，如下图所示。

两个几何体表面之间的距离决定了适用哪种区域

• 无接触（距离 > margin + gap）：几何体表面，包括它们的间隙缓冲区，没有重叠。不产生接触。

• 非活动接触（margin < 距离 ≤ margin + gap）：检测到接触并包含在 mjData.contact 中，但不产生接触力（efc_address = -1）。这些接触可用于自定义计算，例如通过 adhesion 执行器。

• 活动接触（距离 ≤ margin）：接触处于活动状态并产生约束力。约束阻抗函数应用于 distance - margin，该量在此区域中是非正的。

允许负的 margin 值，对应几何形状的“收缩”。在这种情况下，必须保持 margin + gap >= 0 才能使碰撞检测正常工作。

condim

接触摩擦锥可以是椭圆形的，也可以是金字塔形的。这是一个由约束求解器的选择决定的全局设置：椭圆求解器使用椭圆锥，而金字塔求解器使用金字塔锥，如下定义。condim 参数决定了接触类型，并具有以下含义

condim = 11 表示椭圆锥，1 表示棱锥

这对应于无摩擦接触，仅增加一个标量约束。回想一下，接触坐标系的第一轴是接触法线。无摩擦接触只能沿法线产生力。这与关节或肌腱限制非常相似，但它是应用于两个几何体（geom）之间的距离。

condim = 33 表示椭圆锥，4 表示棱锥

这是一种常规的摩擦接触，既能产生法向力，也能产生抵抗滑动的切向摩擦力。对该数字的一种解释是平面的坡度，超过该坡度，扁平物体在重力作用下将开始滑动。

condim = 44 表示椭圆锥，6 表示棱锥

除了法向力和切向力外，这种接触还能产生抵抗绕接触法线旋转的扭转摩擦力矩，对应于接触面片产生的力矩。这对于模拟软手指非常有用，可以显著提高模拟抓取的稳定性。扭转摩擦系数的单位为长度，可解释为表面接触面片的直径。

condim = 66 表示椭圆锥，10 表示棱锥

这种接触可以抵抗两个几何体之间所有相对自由度的运动。特别是它增加了滚动摩擦，例如可以用来阻止球在一个平面上无限滚动。现实世界中的滚动摩擦是由接触点附近局部变形耗散的能量引起的。它可用于模拟轮胎与道路之间的滚动摩擦，并通常用于稳定接触。滚动摩擦系数的单位也是长度，可解释为能量耗散的局部变形深度。

请注意，condim 不能为 2 或 5。这是因为两个切向方向和两个滚动方向是成对处理的。不过，一对中的摩擦系数可以不同，这可以用来模拟滑冰等现象。

摩擦锥

现在我们更正式地描述摩擦锥和相应的雅可比矩阵。仅在本节中，令 \(f\) 表示单个接触的约束力向量（相对于系统级的约束力向量），\(\mu\) 表示摩擦系数向量，\(n\) 表示接触维度 condim。对于 \(n > 1\)，椭圆摩擦锥和棱锥摩擦锥定义如下：

\[\begin{aligned} \text{椭圆锥}: & & \mathcal{K} &= \left\{ f \in \mathbb{R}^n : f_1 \geq 0, f_1^2 \geq \sum_{i=2}^n {f_i^2 / \mu_{i-1}^2} \right\} \\ \text{棱锥}: & & \mathcal{K} &= \left\{ f \in \mathbb{R}^{2(n-1)} : f \geq 0 \right\} \\ \end{aligned} \]

棱锥定义中的向量不等式指逐元素比较。对于 \(n=1\)，两个锥体均定义为非负射线（这是锥体的一种特殊情况）。请注意，下文求解器章节中讨论的系统级摩擦锥也将表示为 \(\mathcal{K}\)。它是此处定义的各单个接触摩擦锥的乘积。

我们还需要指定约束力如何作用于系统。这是通过将一个 6D 基向量与 \(f\) 的每个分量关联来实现的。基向量是空间向量：3D 力后跟 3D 力矩。将基向量排列成矩阵 \(E\) 的列，则接触坐标系中由约束力产生的力/力矩为 \(E f\)。基向量矩阵构建如下。

图示展示了对应于 \(n = 6\) 情况下的完整基集。否则，根据锥体类型，我们仅使用前 \(n\) 列或 \(2(n-1)\) 列。椭圆锥更容易理解。由于矩阵 \(E\) 是单位矩阵，\(f\) 的前三个分量是沿接触坐标系轴作用的力，而后三个分量是绕坐标轴作用的力矩。对于棱锥，基向量对应于金字塔的棱边。每个向量结合了一个法向力分量和摩擦力或摩擦力矩分量。通过摩擦系数进行缩放可确保所有基向量都位于我们所近似的椭圆摩擦锥内。对于这些向量的任何凸组合也是如此。

最后，我们指定如何计算接触雅可比矩阵。首先，我们构建 \(6\) 行 \(\nv\) 列的矩阵 \(S\)，它将关节速度 \(v\) 映射为在接触坐标系中表示的空间速度 \(S v\)。这是通过将接触点视为属于其中一个几何体，计算其空间雅可比矩阵，然后相减得到 \(S\) 来完成的。我们采用的约定是接触力从第一个几何体作用向第二个几何体，因此第一个几何体的空间雅可比矩阵取负号。接触雅可比矩阵即为 \(E^T S\)。与其他所有约束一样，该矩阵被插入到系统级雅可比矩阵 \(J\) 中。

约束求解器

本节解释如何计算约束力。计算分为两个阶段。首先，约束力被定义为凸优化问题的唯一全局解。对于棱锥，这是一个二次规划问题；对于椭圆锥，这是一个锥规划问题。其次，使用下文描述的算法求解该优化问题。我们还将描述约束模型的参数及其对最终动力学的影响。

优化问题的定义本身包含两个步骤。我们从定义在加速度 \(\dot{v}\) 上的原问题开始，其中约束力是隐式的。然后，我们将加速度上的原问题转换为其拉格朗日对偶问题。对偶问题是关于约束力的凸优化问题，约束力同时也扮演了原问题拉格朗日乘子的角色。在前向动力学中，必须在数值上求解原问题或对偶问题。在逆向动力学中，问题变为对角形式，可以解析求解。

原问题的公式基于高斯最小约束原理（Gauss principle of least constraint）的推广。在高斯原理的基本形式中，如果我们有无约束动力学 \(M \dot{v} = \tau\) 并施加加速度约束 \(J \dot{v} = \ar\)，则结果加速度将为：

\[\dot{v} = \arg \min_x \left\| x-M^{-1} \tau \right\|^2_M \\ \textrm{约束条件} \; J x = \ar \]

其中加权 \(L_2\) 范数是通常的 \(\|x\|^2_M = x^T M x\)。因此，约束导致与无约束加速度 \(M^{-1}\tau\) 的偏差尽可能小，其中衡量关节坐标偏差的度量由惯性矩阵给出。已知该原理等同于受约束运动的拉格朗日-达朗贝尔原理（Lagrange-d’Alembert principle）。在此，我们将利用它来获得一个丰富且原则性的软约束模型。这将通过推广高斯原理中的代价函数和约束条件来实现。

除之前引入的符号外，我们将使用以下符号

符号	大小	描述
\(z\)	\(\nc\)	约束变形
\(\omega\)	\(\nc\)	约束变形速度
\(k\)	\(\nc\)	虚拟约束刚度
\(b\)	\(\nc\)	虚拟约束阻尼
\(d\)	\(\nc\)	约束阻抗
\(A(q)\)	\(\nc \times \nc\)	约束空间中的逆惯性
\(R(q)\)	\(\nc \times \nc\)	约束空间中的对角正则化项
\(\ar\)	\(\nc\)	约束空间中的参考加速度
\(\au(q, v, \tau)\)	\(\nc\)	约束空间中的无约束加速度
\(\ac(q, v, \dot{v})\)	\(\nc\)	约束空间中的受约束加速度
\(\mathcal{K}(q)\)		所有接触摩擦锥的乘积
\(\eta\)		摩擦损失力的上限
\(\Omega(q)\)		可行约束力的凸集
\(\mathcal{E}, \mathcal{F}, \mathcal{C}\)		等式、摩擦损失、接触约束的索引集

索引集将用于引用向量和矩阵的部分。例如，\(J_\mathcal{C}\) 是雅可比矩阵中所有对应于接触约束的行构成的子矩阵。

原问题

我们首先构建求解受约束加速度 \(\dot{v}\) 的优化问题，然后解释其含义及合理性。该问题为：

(7)\[(\dot{v}, \dot{\omega}) = \arg \min_{(x, y)} \left\|x-M^{-1}(\tau-c)\right\|^2_M + \left\|y-\ar\right\|^{\text{Huber}(\eta)}_{R^{-1}} \\ \textrm{约束条件} \; J_\mathcal{E} x_\mathcal{E} - y_\mathcal{E} = 0, \; J_\mathcal{F} x_\mathcal{F} - y_\mathcal{F} = 0, \; J_\mathcal{C} x_\mathcal{C} - y_\mathcal{C} \in \mathcal{K}^* \]

这里的新成员是使约束变软的对角正则化项 \(R > 0\)，以及稳定约束的参考加速度 \(\ar\)；后者是一个弹簧-阻尼器，定义在下文的参数部分。其思想类似于 Baumgarte 稳定化，但它不是直接添加约束力，而是修改其求解结果为约束力的优化问题。由于该问题本身受约束，\(\ar\) 和 \(f\) 之间的关系通常是非线性的。\(R\) 和 \(\ar\) 的量由求解器参数计算，详见后文。目前我们假设它们是已知的。

优化变量 \(x\) 代表高斯原理中的加速度，而 \(y\) 是约束空间中的松弛变量。这是建模软约束所必需的。如果我们强迫解达到 \(y = \ar\)（通过取极限 \(R \to 0\)），我们将得到硬约束模型。MuJoCo 不允许这种极限，但尽管如此，人们仍可以构建现象学上的硬约束模型。

符号 \(\mathcal{K}^*\) 表示摩擦锥的对偶。这源于数学上的逆向工程：我们希望在对原问题进行对偶变换后恢复约束 \(f \in \mathcal{K}\)，而锥的对偶之对偶仍是该锥本身。之前定义的棱锥摩擦锥实际上是自对偶的，但椭圆锥则不然。

Huber “范数”基于鲁棒统计学中的 Huber 函数：它在零点附近是二次的，并在参数绝对值超过阈值（由摩擦损失参数给出）时平滑过渡到线性函数。设置 \(\eta = \infty\) 可恢复二次范数；我们对所有非摩擦损失引起的约束力都使用此约定。这是逆向工程的另一个实例：我们希望获得摩擦损失力的区间约束，这并非易事，因为拉格朗日对偶通常产生非负性约束。事实证明，Huber 函数正是获得对偶区间约束所需的工具。在没有摩擦损失约束的情况下，两种范数都变为二次的。

现在我们将问题 (7) 与高斯原理更紧密地联系起来，并赋予松弛变量物理意义。考虑一个具有位置 \((q, z)\) 和速度 \((v, \omega)\) 的增强动力学系统。新的状态变量对应于变形动力学。类似于原系统中 \(v\) 与 \(\dot{q}\) 不相同的情况，此处 \(\omega\) 与 \(\dot{z}\) 也不相同，尽管原因不同。变形与非零位置残差有关。回想一下，我们有等式约束、限制、棱锥摩擦锥的所有分量以及椭圆摩擦锥法向分量的明确位置残差。对于这些变形变量，我们有 \(\dot{z} = \omega\)。然而，对于摩擦损失和椭圆锥的摩擦分量，有 \(z = 0\) 而 \(\omega \neq 0\)。这是因为即使在约束空间中可能存在运动（约束力旨在阻止这种运动），也没有位置误差。增强动力学为：

\[\begin{aligned} \tilde{q} &= {q \brack z}, & \tilde{v} &= {v \brack \omega}, & \tilde{c} &= {c \brack 0}, \\ \tilde{\tau} &= {\tau \brack {R^{-1} \ar}}, & \tilde{M} &= \left[\begin{array}{cc} M & 0 \\ 0 & R^{-1} \end{array} \right], & \tilde{J} &= \left[ \begin{array}{cc}J & -I \end{array} \right] \\ \end{aligned} \]

将高斯原理应用于该系统可得出上述原优化问题，除 Huber 范数外。通用运动方程 (1) 现在变为：

\[\tilde{M} \dot{\tilde{v}} + \tilde{c} = \tilde{\tau} + \tilde{J}^T f \]

展开所有波浪号项可得到原始动力学和变形动力学的明确形式：

\[\begin{aligned} M \dot{v} + c &= \tau +J^T f \\ \dot{\omega} &= \ar - R f \\ \end{aligned} \]

因此 \(R\) 具有逆变形惯性的含义，而 \(\ar\) 具有无力变形加速度的含义。

MuJoCo 是否将这些变形变量作为系统状态的一部分，并将其动力学与关节位置和速度一起积分？不，尽管未来可能提供这样的选项。回想一下，我们将正则化项和参考加速度的函数依赖定义为 \(R(q)\) 和 \(\ar(q, v)\)。这使得问题 (7) 仅依赖于 \((q, v, \tau)\)，因此原始动力学实际上不受变形动力学影响。由于我们迄今为止开发的通用约束模型对 \(R\) 和 \(\ar\) 的计算方式不做假设，我们的选择是一致的，并提高了模拟器效率。尽管如此，鉴于这些量最终与变形动力学有关，将其定义为 \(R(z)\) 和 \(\ar (z, \omega)\) 并模拟整个增强系统可能更自然。下面我们澄清了这种模拟的一些好处。

变形动力学何时能“精确跟踪”原始动力学？可以验证，当约束力 \(f\) 等于下文参数部分定义的量 \(f^+\) 时，就会发生这种情况。此时变形状态成为关节位置和速度的静态函数，即 \(z = r(q)\) 和 \(\omega = J(q) v\)。但通常情况并非如此。假设你将手指推入软材料中，以比材料恢复形状更快的速度拉回，然后再次推入。你在第二次推入时感受到的接触力不仅取决于手指和物体的刚体位置，还取决于第一次推入时产生的材料变形。模拟上述增强动力学将捕捉到这一现象，而 MuJoCo 实现的模型忽略了它，反而假设所有物体在下次接触前都会恢复形状。摩擦维度中的滑动也存在类似现象，也被忽略了。

降维原问题

(7) 中定义的原问题，以及稍后将得到的对偶问题，都是约束优化问题。对偶问题形式更简单，但约束优化在数值上仍不如无约束优化有效。事实证明，原问题可以简化为关于加速度的无约束优化问题。如果给定 (7) 中的 \(x\)，则对 \(y\) 的最小化可以闭式求解。这也消除了约束，即 \(y\) 的解会自动满足约束条件。剩下的是关于 \(x\) 的无约束优化问题，可以用更有效的算法求解。

这种简化基于以下事实：(7) 中对 \(y\) 的最小化归结为寻找约束集上的最近点——这要么是平面要么是锥面，且可以解析完成。代入结果，我们得到无约束问题：

(8)\[\dot{v} = \arg \min_{x} \left\|x-M^{-1}(\tau-c)\right\|^2_M + s \left( J x - \ar \right) \]

函数 \(s(\cdot)\) 起到了软约束惩罚项的作用。可以证明它是凸的且一阶连续可微的。在棱锥摩擦锥的情况下，它是一个二次样条函数。

降维公式的另一个吸引人之处是逆向动力学很容易计算。由于上述问题是无约束且凸的，唯一的全局最小值使得梯度为零。这导出了等式：

\[M \dot{v} + c = \tau - J^T \nabla s \left( J \dot{v} - \ar \right) \]

这就是存在软约束时的解析逆动力学。与运动方程 (1) 相比，我们看到约束力 \(f\) 由函数 \(s(\cdot)\) 的负梯度给出。再次对 \(\dot{v}\) 求导得到：

\[\frac{\partial \tau}{\partial \dot{v}} = M + J^T H[s] J \]

这是作用力对加速度的解析导数。因此我们看到函数 \(s(\cdot)\) 及其导数是 MuJoCo 物理模型的关键。

对偶问题

构建拉格朗日对偶的过程有些繁琐但很成熟。我们跳过中间过程直接给出结果。上述原问题的拉格朗日对偶为：

(9)\[f = \arg\min_\lambda \frac{1}{2} \lambda^{T} \left( A+R \right) \lambda + \lambda^T \left( \au - \ar \right) \\ \text{约束条件} \; \lambda \in \Omega \]

其中约束空间中的逆惯性为：

\[A = J M^{-1} J^T \]

且约束空间中的无约束加速度为：

\[\au = J M^{-1} (\tau-c) + \dot{J} v \]

约束集 \(\Omega\) 如下。\(\lambda_\mathcal{E}\) 是无约束的，因为它是原问题中等式约束的拉格朗日乘子。对于摩擦损失，我们有逐元素应用的盒约束 \(\left|\lambda_\mathcal{F}\right| \leq \eta\)。对于接触，我们有 \(\lambda_\mathcal{C} \in \mathcal{K}\)。对于棱锥，这仅是 \(\lambda_\mathcal{C} \geq 0\)，而对于椭圆锥，这是二阶锥约束。虽然 \(A\) 只是半正定的，但 \(R\) 构造上是正定的，因此上述二次代价函数是严格凸的。因此对于棱锥摩擦锥，我们有一个凸盒约束二次规划问题；对于椭圆摩擦锥，我们有一个盒约束和二阶锥约束的混合问题。求解该问题的算法将在后文描述。

如前所述，MuJoCo 的约束模型具有明确定义的逆动力学，我们已经在上述降维公式中看到了一种推导方法。这里我们从对偶公式再次推导。回想一下，在逆动力学中我们拥有 \((q, v, \dot{v})\) 而不是 \((q, v, \tau)\)，因此无约束加速度 \(\au\) 是未知的。但是我们可以计算受约束加速度：

\[\ac = J \dot{v} + \dot{J} v \]

现在可以通过求解以下优化问题来计算逆动力学：

\[f = \arg \min_\lambda \frac{1}{2} \lambda^{T} R \lambda + \lambda^T \left( \ac - \ar \right) \\ \text{约束条件} \; \lambda \in \Omega \]

通过比较这两个凸优化问题的 KKT 条件，可以验证当满足以下条件时它们的解是一致的：

(10)\[\ac = \au + Af \]

这个关键等式本质上是牛顿第二定律在约束空间中的投影。它的推导方法是：将运动方程 (1) 中的 \(c\) 项移至右侧，从左侧乘以 \(J M^{-1}\)，两边同时加上 \(\dot{J} v\)，并代入上述 \(A, \au, \ac\) 的定义。计算 \(\dot{J} v\) 需要对约束雅可比矩阵进行时间微分，这是非平凡的。虽然该项在等式 (10) 中抵消，因此不影响前向-逆向比较，但其在前向动力学中的忽略会为任何雅可比矩阵随配置变化的约束引入速度相关的偏差。我们针对等式约束（连接和焊接）计算该项，此时雅可比微分是易处理的。对于接触，由于通过碰撞流水线对接触坐标系进行微分的复杂性，该项仍被忽略。

请注意，逆问题中的二次项由 \(R\) 加权，而不是 \(A+R\)。这是关键的结构性洞察：\(A\) 矩阵完全抵消，二次项中仅留下 \(R\)。这产生了两个后果。首先，在对应于硬约束的极限 \(R \to 0\) 中，逆动力学不再定义，正如预期那样。其次，逆问题是对角的，即它解耦为关于单个约束力的独立优化问题。由于 \(R\) 是对角的，无需矩阵求逆或分解——逆动力学无需任何优化，只需解析公式。唯一的耦合来自约束集 \(\Omega\)，但该集在前文讨论的概念性约束中也是解耦的。事实证明，所有这些独立的优化问题都可以解析求解。唯一非平凡的情况是椭圆摩擦锥模型；我们已在上述参考论文中展示了如何处理它。这需要 \(R\) 对角值的一定耦合，MuJoCo 会自动强制执行此操作，以便为每个模型实现精确的解析逆解。

一旦计算出前向动力学，逆动力学在计算上几乎是免费的。这是因为前向动力学需要逆问题中使用的所有量，因此唯一的额外步骤就是解析公式。这使得在 MuJoCo 中实现自动正确性检查成为可能。当 mjModel.opt.enableflags 中的 fwdinv 标志开启时，前向和逆向动力学会自动在每个时间步结束时进行比较，差异记录在 mjData.solver_fwdinv 中。差异表明前向求解器（它是数值求解且通常提前终止）收敛性不佳。当然，无需先计算前向动力学，逆动力学本身也很有用。

算法

这里我们描述用于求解上述凸优化问题的数值算法（或“求解器”）。牛顿求解器和 CG 求解器使用降维原公式 (8)，而 PGS 求解器使用对偶公式 (9)。注意，数值求解器仅在前向动力学中需要。逆动力学则通过解析方式处理。

每种求解算法都可用于棱锥和椭圆摩擦锥，以及约束雅可比矩阵及相关矩阵的稠密和稀疏表示。

CG共轭梯度法

该算法使用带有 Polak-Ribiere-Plus 公式（非负 \(\beta\)）的非线性共轭梯度法。直线搜索是精确的，在分段二次代价函数上使用一维牛顿法和解析二阶导数。CG 没有设置成本。

Newton牛顿法

该算法实现了精确牛顿法，具有解析二阶导数和 Hessian 矩阵的 Cholesky 分解。直线搜索与 CG 方法相同。当约束状态在迭代间发生变化（例如，约束从二次过渡到线性）时，通过秩 1 Cholesky 更新增量更新 Hessian 分解，避免了完全重新分解。它是默认求解器。

PGS投影高斯-塞德尔法

这是物理模拟器中最常用的算法，也是 MuJoCo 曾经的默认算法，直到我们开发出在各方面看起来都更好的牛顿法。PGS 使用对偶公式。与沿斜向改进解的基于梯度的方法不同，高斯-塞德尔法一次处理一个标量分量，并根据所有其他分量的当前值将其设置为最优值。PGS 的一次扫描具有与一次矩阵向量乘法相当的计算复杂度（尽管常数较大）。它具有一阶收敛速度，但在几次迭代中仍能取得快速进展。

当使用棱锥摩擦锥时，问题涉及 PGS 传统上应用的盒约束。如果我们直接将 PGS 应用于椭圆摩擦锥产生的圆锥约束，它会陷入连续的局部极小值中；参见左图。这是因为它只能沿坐标轴取得进展。右图展示了我们解决该问题的方法。我们仍然一次更新一个接触点，但在接触点内，我们沿适应约束表面的非正交轴进行更新，如下所示。首先，我们沿从锥尖穿过当前解的射线优化二次代价。然后，我们用通过当前解且垂直于接触法线的超平面切割圆锥。这产生了一个椭圆，根据我们的接触模型，它可以达到 5 维。现在我们在该椭圆内优化二次代价。这是二次约束二次规划 (QCQP) 的一个实例。由于只有一个标量约束（尽管可能是非线性的），其对偶是一个关于未知拉格朗日乘子的标量优化问题。我们使用牛顿法求解此问题直至收敛——实践中这需要不到 10 次迭代，并涉及小矩阵。总体而言，该算法对于棱锥摩擦锥的行为与 PGS 相似，但它无需近似即可处理椭圆锥。虽然它每个接触点做的工作更多，但接触维度较小，这两个因素大致相互抵消。

NoSlip后处理通道

这并不是一个独立的求解器，而是一个后处理步骤，通过将 选项中的 noslip_iterations 设置为正值来启用。在主求解器（牛顿法、CG 或 PGS）收敛后，NoSlip 求解器仅在摩擦维度上重新求解，在这些维度中使用 \(R = 0\)（即硬约束）的 PGS 扫描。这抑制了软约束模型固有的接触滑动。然而，这种优化步骤的级联不再求解单个定义明确的优化问题；这是一种临时修正，有时可能会导致具有复杂多接触相互作用的模型不稳定。

热启动 (Warmstart)

在求解之前，求解器使用前一个时间步的约束力进行热启动。它评估热启动力的成本，并将其与零力（即无约束解 qacc_smooth）的成本进行比较。使用成本较低的初始化。这种对偶热启动策略是稳健的：当约束跨越时间步持续存在时，它能快速引导求解器，但避免从已消失的约束中携带过时的力。

约束孤岛

考虑由自由度（DOFs）和约束定义的抽象图。顶点是单个运动学树中的所有自由度；边是属于不同树的两个物体之间的约束（接触、等式或肌腱限制）。约束孤岛 (Constraint island) 是一个可以独立求解的不相交子图，因为约束力不能在孤岛之间传播。约束孤岛的发现和构建（“islanding”）包括查找这些不相交子图，并重新排序自由度和约束，使它们在内存中连续。这相当于约束雅可比矩阵 \(J\) 的分块对角化，如图所示。左侧是大小为 \(\nc \times \nv\) 的整体雅可比矩阵，我们使用了来自 MuJoCo 数据结构 mjData.nefc 和 mjModel.nv 的对应大小名称。右侧是分块对角化的雅可比矩阵，包含 3 个可以独立求解的孤岛。请注意，islanding 还会识别无约束自由度，因此所有孤岛中的自由度总数 mjData.nidof 可能小于 mjModel.nv。虽然 islanding 并非免费（请参阅 engine_island.c 中的实现），但它非常值得。

• 不同的孤岛需要不同数量的迭代才能收敛，整体求解将运行最慢孤岛所需的时间。

• 无约束自由度完全不被求解器触及，求解器无需发现它们不受影响。

• 求解单独的孤岛可以多线程化。

参数

在此我们解释如何从模型参数计算量 \(R, \ar\)。为了使选定的参数化有意义，我们首先需要了解这些量如何影响动力学。我们关注 (9) 的无约束极小化问题，即：

\[f^+ = (A+R)^{-1} (\ar - \au) \]

如果恰好 \(f^+ \in \Omega\)，那么 \(f^+ = f\) 就是我们模型产生的实际约束力。我们关注这种情况是因为它很常见，即 \(\Omega\) 中在任何给定时间处于活动状态的约束子集通常很小，而且这也是我们唯一能够真正分析的情况。将 \(f^+\) 代入约束动力学 (10) 并重排项得到：

\[\ac = A(A+R)^{-1} \ar + R (A+R)^{-1} \au \]

因此，受约束的加速度在无约束加速度和参考加速度之间进行插值。特别是，在极限 \(R \to 0\) 时，我们有硬约束且 \(\ac = \ar\)；而在极限 \(R \to \infty\) 时，我们有无限软的约束（即无约束）且 \(\ac = \au\)。因此，引入一个直接控制插值的模型参数是很自然的。我们将此参数称为阻抗 (impedance)，记为 \(d\)。它是一个维度为 \(\nc\) 的向量，满足逐元素 \(0<d<1\)。一旦指定，我们计算正则化项的对角元素为：

(11)\[R_{ii} = \frac{1-d_i}{d_i} \hat{A}_{ii}\]

注意我们使用的不是实际 \(A\) 矩阵的对角线，而是它的一个近似。这是因为我们不想在稀疏求解器或逆动力学中计算 \(A\)。该近似（仅限于对角线）是使用模型处于初始配置 mjModel.qpos0 时所有物体、关节和肌腱的“末端执行器”惯性构建的。这些量由编译器计算。如果我们的近似恰好是精确的，且 \(A\) 本身恰好是对角的，那么每个标量约束的加速度将满足：

\[\aci = d_i \ari + (1-d_i) \aui \]

因此我们将实现期望的插值效果。当然，这通常不完全成立，但这里的目标是构建一个合理且直观的约束模型参数化，并获得正确的缩放比例。

对角近似：该近似有三个误差来源：(i) 它固定在 qpos0 而不是在当前配置下评估；(ii) 它将方向性逆惯性平均为标量，假设各向同性；(iii) 它将不同物体的贡献视为独立的，忽略了通过共享自由度的运动学耦合。这些误差通常较小，但对于具有高度各向异性惯性或远离 qpos0 运行的长运动学链的模型，可能会变得显著。在严重情况下——特别是当平均惯性在方向性惯性有限的情况下趋于零时——正则化项 \(R\) 趋于零，使得约束无限硬并导致发散。diagexact 标志用精确对角线 \(A_{ii} = \|Y_i\|^2\) 替换近似，其中 \(Y = J M^{-1/2}\) 是白化雅可比矩阵，在当前配置下计算。这消除了所有三个误差来源，且运行成本适中：计算 \(Y\) 需要对每个活动约束行使用质量矩阵的 Cholesky 因子进行回代；如果使用对偶求解器（PGS 或 NoSlip），成本可以忽略不计，因为 \(Y\) 无论如何都会被计算。

接下来我们解释参考加速度是如何计算的。如前所述，我们使用由阻尼和刚度系数逐元素参数化的弹簧-阻尼器模型：

(12)\[\ari = -b_i (J v)_i - k_i r_i\]

回想一下，\(r\) 是位置残差，而 \(J v\) 是投影在约束空间中的关节速度；索引符号引用投影速度向量的一个分量。对于椭圆锥的摩擦损失和摩擦维度，\(r \equiv 0\)，因此 \(k=0\)，所以参考加速度简化为纯阻尼：\(\ari = -b_i (J v)_i\)。更多细节请参阅建模章节的摩擦部分。

总而言之，约束行为由三个约束参数确定：阻抗 \(0<d<1\)、阻尼 \(b > 0\) 和刚度 \(k \geq 0\)。这些参数由 solimp 和 solref 属性计算，如建模章节的求解器参数部分所述，该部分还提供了额外的自动化功能（例如实现临界阻尼，或随距离变化 \(d\) 以模拟软接触层）。量 \(R, \ar\) 随后根据 (11) 和 (12) 计算，并应用选定的优化算法来求解问题 (9)。

由 \(R\) 和 \(\ar\) 组合产生的闭环约束动力学在建模章节的求解器参数部分进行了详细分析。简而言之，每个标量约束的表现大致为一个阻尼二阶系统，其时间常数和阻尼比由 solref 属性设置，强度由通过 solimp 设置的阻抗 \(d\) 控制。当临界阻尼时（\(\text{dampratio} = 1\)），恒定外部载荷下的稳态穿透力与约束空间中的有效质量无关——这是阻抗缩放参数化的结果。

摩擦锥

如上所述，MuJoCo 允许使用椭圆摩擦锥及其棱锥近似；所选的求解器决定使用哪种类型的摩擦锥。棱锥近似具有 \(2 (n-1)\) 条棱边，其中 \(n\) 是由 condim 指定的接触空间维度。我们可以添加更多的棱边以获得对底层椭圆锥更好的近似，但这毫无意义，因为由此产生的求解器将比椭圆求解器更慢。

人们可能预期，如果我们增加棱锥近似中的棱边数量，我们优化问题 (7) 的解会收敛到椭圆锥的解。在硬接触的极限下确实如此。然而，对于软接触，事实并非如此。这个令人惊讶的事实不仅仅是一个数学上的好奇心；它会对动力学产生可见的影响，在 MuJoCo 的早期版本中，这使得用棱锥近似实现精细的抓取行为变得困难。为了理解这种现象，考虑固定问题 (7) 中的加速度变量 \(x\) 并优化掉变形变量 \(y\)。可以证明，由此产生的关于 \(x\) 的优化问题等价于受约束优化的惩罚方法，其中惩罚是从约束边界开始的半二次函数。可以把它想象成边界投下的“影子”。无论近似有多精确，这种阴影的形状对于椭圆锥和它们的棱锥近似都是不同的。下图展示了二维接触下的这种效应，其中棱锥甚至不是近似，而是与椭圆锥代表相同的约束集。我们绘制了棱锥（红色）和椭圆（虚线蓝色）锥在不同摩擦系数下的惩罚/阴影轮廓。在数学上，棱锥情况下的惩罚是一个二次样条函数，而椭圆情况下的惩罚包含二次函数减去二次函数平方根的部分——从而允许锥尖周围的圆形轮廓。

总之，椭圆摩擦锥和棱锥摩擦锥定义了不同的软接触动力学（尽管它们通常非常接近）。椭圆模型更有原则且更符合物理直觉，相应的求解器也非常高效，但根据模型不同，可能不如棱锥求解器高效。

碰撞检测

碰撞检测作用于几何体（geom），这些几何体是刚性附着在底层物体上的几何实体。碰撞检测的输出是活动接触列表，定义为接触距离小于其边界参数的接触。它们存储在全局数组 mjData.contact 中，随后用于构建约束雅可比矩阵并计算约束力。下面我们将解释如何选择几何体对进行碰撞检查，如何执行碰撞检查，以及如何确定由此产生的接触参数。

选择

如果模型有 \(n\) 个几何体，则有 \(n (n-1)/2\) 个几何体对可能发生碰撞。详细检查所有这些对（也称为近相碰撞检测）对于大型系统来说代价高昂。幸运的是，其中一些潜在碰撞是不可取的，因此用户可以在建模阶段将其排除，而另一些则无需详细检查即可快速剪枝。MuJoCo 有灵活的机制来决定详细检查哪些几何体对。决策过程涉及两个阶段：生成和过滤。

生成

首先，我们通过合并两个来源生成候选几何体对列表：可能包含碰撞几何体的物体对，以及在 MJCF 中使用 pair 元素定义的显式几何体对列表。

物体对是通过基于改进的扫描和剪枝（sweep-and-prune）算法的宽相碰撞检测生成的。改进之处在于，排序轴选择为所有几何体中心的协方差矩阵的主特征向量——这最大化了分布。然后，对于每个物体对，使用轴对齐包围盒 (AABB) 的静态包围体层次结构 (BVH 二叉树) 执行中相碰撞检测。每个物体都配备了一个其几何体的 AABB 树，分别与物体惯性或几何体框架对齐。

最后，用户可以使用 MJCF 中的 exclude 元素显式排除某些物体对。在此步骤结束时，我们得到的几何体对列表通常远小于 \(n (n-1)/2\)，但在详细碰撞检查之前仍可进一步剪枝。

过滤

接下来，我们对前一步生成的列表应用四个过滤器。过滤器 1 和 2 应用于所有几何体对。过滤器 3 和 4 仅应用于由物体对机制生成的对，从而允许用户通过显式指定几何体对来绕过这些过滤器。

1. 两个几何体的类型必须对应于能够执行详细检查的碰撞函数。通常是这种情况，但也有例外（例如不支持平面-平面碰撞），此外用户可以使用 NULL 指针覆盖默认的碰撞函数表，从而有效地禁用某些几何体类型之间的碰撞。

2. 应用包围球测试，同时考虑接触余量（margin）。如果对中的一个几何体是平面，则变为平面-球体测试。

3. 两个几何体不能属于同一个物体。此外，它们不能属于父物体和子物体，除非父物体是世界物体。动机是避免物体和关节内部的永久接触。请注意，如果多个物体在它们之间没有关节的意义上焊接在一起，它们在进行此测试时被视为单个物体。父过滤测试可以由用户禁用，而同体测试则不能禁用。

4. 两个几何体必须在以下意义上是“兼容”的。每个几何体都有整数参数 contype 和 conaffinity。以下布尔表达式必须为真才能通过测试：

   (contype1 & conaffinity2) || (contype2 & conaffinity1)

   这要求一个几何体的 contype 和另一个几何体的 conaffinity 有一个共同的位设置为 1。这是从 Open Dynamics Engine 借用的一种强大机制。所有几何体的默认设置为 contype = conaffinity = 1，这总是通过测试，所以如果一开始觉得困惑，用户可以忽略此机制。

检查

详细碰撞检查，也称为近相或窄相 (narrow-phase) 碰撞检测，由取决于对中几何体类型的函数执行。窄相碰撞函数表可以在 engine_collision_driver.c 的顶部查阅，并作为 mjCOLLISIONFUNC 暴露给希望安装自定义碰撞器的用户。MuJoCo 支持多种原始几何形状：平面、球体、胶囊体、圆柱体、椭圆体和盒体。它还支持三角网格和高度图 (height-fields)。

除了SDF 插件（参见相关文档）这一显著例外，碰撞检测仅限于凸几何体。所有原始类型都是凸的。高度图不是凸的，但在内部它们被视为三角棱柱的集合（使用超出上述过滤器之外的自定义碰撞剪枝）。用户指定的网格可以是凹的，并按原样渲染。但出于碰撞目的，它们会被其凸包取代（在 simulate 中用“H”键可视化），由 qhull 库计算。

凸碰撞

所有涉及没有解析碰撞器的几何体对（例如网格）的碰撞，都由两个通用凸碰撞检测 (CCD) 流水线之一处理：

原生流水线（默认）

原生 CCD 流水线 (“nativeccd”) 在 MuJoCo 中原生实现，基于 GJK 和 EPA 算法 (GJK / EPA)。原生流水线比基于 MPR 的流水线更快且更稳健。

libccd 流水线（遗留）

此遗留流水线基于 libccd 库，并使用 Minkowski Portal Refinement (MPR)。它通过禁用 nativeccd 标志来激活。

两个流水线都由容差（距离单位）和最大迭代参数控制，分别暴露为 mjOption.ccd_tolerance (ccd_tolerance) 和 mjOption.ccd_iterations (ccd_iterations)。

多接触

某些碰撞器可以为每个碰撞对返回多个接触点，以建模边缘或表面接触，例如当两个扁平物体接触时。例如，胶囊体-平面和盒体-平面碰撞器分别最多可以返回两个或四个接触点。像 MPR 和 GJK/EPA 这样的标准通用凸碰撞算法总是返回单个接触点，这对于表面接触场景（例如盒体堆叠）是有问题的。MuJoCo 的两个 CCD 流水线都可以为每个接触对返回多个点（“multiccd”）。此行为由 multiccd 标志控制，但以不同的方式和不同的权衡实现：

多次运行流水线（遗留）

多个接触点通过将两个几何体沿切向轴旋转 ±1e-3 弧度并重新运行碰撞程序来找到。如果检测到新接触，则将其添加，最多可增加 4 个额外的接触点。此方法有效，但会将每次碰撞调用的成本增加 5 倍。当 nativeccd 标志被禁用时，以及在涉及圆柱体和胶囊体或具有正接触余量的几何体碰撞时，会使用此方法。

单次流水线

单次流水线与原生 CCD 流水线结合使用，即当 nativeccd 标志启用时。由于该流水线是一次性的，且大部分几何体分析在编译时完成，因此性能开销极小。支持的几何体是盒体和没有正接触余量的网格。

几何体距离

上述描述的窄相碰撞函数驱动 mj_geomDistance 函数及相关的 碰撞传感器。由于 MPR 的局限性，遗留流水线会返回错误值（顶部），除非在相对于几何体尺寸极小的距离处，因此不建议用于此用例。相反，基于 GJK 的原生流水线（底部）在所有距离上都能计算出正确的值。

凸分解

为了建模高度图以外的凹物体，用户必须将其分解为凸几何体的联合（可以是原始形状或网格）并将其附着在同一物体上。此规则的另一个例外（高度图除外）是符号距离函数 (SDF)（详见相关文档），在某些情况下（例如 解析 SDFs）可以高效，但有其他要求和限制。

像 CoACD 库这样的开源网格分解工具可以在 MuJoCo 外部使用以自动化此过程。最后，所有内置碰撞函数都可以替换为自定义回调。例如，这可用于合并通用“三角形汤 (triangle soup)”碰撞检测器。但是我们不建议这样做。预处理几何体并将其表示为凸几何体的联合需要一些工作，但这在运行时会获得回报，并产生更快、更稳定的模拟。

成对碰撞器

下表提供了有关用于不同几何体对的碰撞器的信息。这些值可以由 mj_maxContact 函数动态计算。使用切换开关查看通过参数 nativeccd、multiccd 和 margin 返回的最大接触点数。

nativeccd

multiccd

with margin

	球体	胶囊体	椭圆体	圆柱体	盒体	网格	SDF
平面	原始形状 1	原始形状 2	原始形状 1	原始形状 4	原始形状 4	原始形状 3	原始形状 1
高度图	高度图CCD mjMAXCONPAIR	高度图CCD mjMAXCONPAIR	高度图CCD mjMAXCONPAIR	高度图CCD mjMAXCONPAIR	高度图CCD mjMAXCONPAIR	高度图CCD mjMAXCONPAIR	高度图SDF sdf_initpoints
球体	原始形状 1	原始形状 1	CCD 1	原始形状 1	原始形状 1	CCD 1	SDF sdf_initpoints
胶囊体		原始形状 2	CCD 1	CCD 1 5 5	原始形状 2	CCD 1 5 5	SDF sdf_initpoints
椭圆体			CCD 1	CCD 1	CCD 1	CCD 1	SDF sdf_initpoints
圆柱体				CCD 1 5 5	CCD 1 5 5	CCD 1 5 5	SDF sdf_initpoints
盒体					原始形状 8	CCD 1 4 5	SDF sdf_initpoints
网格						CCD 1 4 5	网格SDF sdf_initpoints
SDF							SDF sdf_initpoints

睡眠孤岛

睡眠是一种性能优化，通过该优化，检测到处于静止状态的模拟可移动元素被暂时从流水线中移除（“进入睡眠”）。这种优化在模型包含大量被动物体时最有用。可以睡眠或唤醒的最小单元是运动学树，然而树总是与它们通过约束连接到的其他树一起睡眠，因此称为“睡眠孤岛”。

右侧视频展示了睡眠的几个方面。首先我们展示 多米诺骨牌 模型，它模拟了传统的倒塌“连锁反应”。除一块外，所有骨牌都处于稳定平衡状态并很快进入睡眠，但第一块骨牌处于不稳定状态，保持清醒并开始下落。每当清醒骨牌与睡眠骨牌之间发生接触时，后者会自动被唤醒。在地板上稳定后，骨牌堆再次进入睡眠，其相关接触消失。在启用孤岛可视化的情况下重复此序列，根据孤岛的第一个自由度重新为几何体着色，睡眠时使用较暗的颜色。在子剪辑的最后，睡眠被切换关闭和开启，展示了睡眠带来的速度提升（右下）。第二个子剪辑显示了 100 个仿人机器人 模型的变体，其中所有仿人机器人初始化为睡眠。初始化为睡眠的树可以处于任何配置，包括悬浮在半空中、深穿透等。其中一个仿人机器人被用户直接扰动手动唤醒，然后拖动以唤醒它接触到的任何其他仿人机器人。

虽然平滑动力学受益于睡眠，但最大的加速来自于减少的接触数量。在碰撞检测方面，睡眠孤岛表现得像静态物体：跳过孤岛内部以及孤岛与静态物体之间的所有接触。在静态物体堆的情况下，跳过的接触数量可能很高，从而导致显著的速度提升。允许睡眠孤岛与清醒树之间的接触，实际上它们是唤醒的主要自动触发器，但也支持手动唤醒。由于唤醒发生在流水线的位置阶段，它是瞬时完成的，被唤醒的孤岛将表现得就像一直清醒一样。

睡眠默认关闭，并使用 sleep 标志启用。睡眠机制的详细描述在模拟章节中提供，但这里我们提供一个简要概述。

睡眠可以通过两种方式之一发生：

• 自动：如果一棵树绝对值的最大速度在 mjMINAWAKE 时间步内小于 容差，则标记为“准备睡眠”。如果孤岛中的所有树都准备睡眠，它们将在状态推进期间进入睡眠。

• 初始化睡眠：通过将树根的 body/sleep 属性设置为“init”，它被标记为“初始化睡眠”，并在 mjData 初始化期间进入睡眠。

模拟流水线

在此，我们总结了前向动力学和逆动力学各自涉及的计算序列。其中大多数步骤之前已经描述过。请记住，mjModel.opt.disableflags 和 mjModel.opt.enableflags 中的位标志可分别用于跳过默认步骤和启用可选步骤。回调函数不在此处显示。

前向动力学

源文件 engine_forward.c 包含了高层前向动力学流水线。

顶层

• 顶层函数 mj_step 调用了下文 1-26 阶段的整个序列。

• mj_forward 仅调用 2-23 阶段，计算连续时间的前向动力学，并以加速度 mjData.qacc 结束。

• mj_step1 调用阶段 1-19，mj_step2 调用阶段 20-26，将 mj_step 分解为两个不同的阶段。这允许用户编写依赖于位置和速度推导量（但不依赖于力，因为力尚未计算）的控制器。请注意，mj_step1 → mj_step2 流水线不支持龙格-库塔 (Runge Kutta) 积分器。

• mj_fwdPosition 调用阶段 2-11，即流水线中依赖于位置的部分。

前向动力学流水线示意图

顶层函数	mj_step
	mj_step1						mj_step2
	mj_forward
组件 / 描述		fwdPosition			fwdVelocity
		fwdKinematics	惯性，碰撞				加速度		advance
阶段	1	2-5	6-11	12	13-18	19	20-23	24,25	26

阶段

下面我们描述流水线阶段以及对应于每个阶段的 API 函数。所有函数都将其输出写入 mjData 的属性中。将下表与 mjData 结构体定义进行比较非常有用，其中属性块上方的注释指定了计算它们的函数。请注意，每个阶段都依赖于之前部分或全部阶段计算出的值。

1. 检查位置和速度是否存在指示发散的无效或不可接受的巨大实数值。如果检测到发散，状态将自动重置并引发相应的警告：mj_checkPos, mj_checkVel

位置

以下阶段计算依赖于广义位置 mjData.qpos 的量。

2. 计算正向运动学。这将产生所有物体、几何体、位点、相机和灯光的全局位置和方向。它还会归一化所有四元数：mj_kinematics, mj_camlight

3. 计算物体惯性和关节轴，这些惯性和轴位于以相应运动学子树的质心为中心的全局坐标系中：mj_comPos

4. 计算与 flex 对象相关的量：mj_flex

5. 计算肌腱长度和力臂。这包括空间肌腱最小长度路径的计算：mj_tendon

6. 计算复合刚体惯性和关节空间惯性矩阵：mj_makem

7. 计算关节空间惯性矩阵的稀疏分解：mj_factorm

8. 构建活动接触列表。这包括粗略阶段和精确阶段的碰撞检测：mj_collision

9. 构建约束雅可比矩阵，计算约束残差，构建岛（islands）：mj_makeConstraint, mj_island（API 中尚未公开）

10. 计算执行器长度和力臂：mj_transmission

11. 计算约束求解器所需的矩阵和向量：mj_projectConstraint

12. 计算仅依赖于位置的传感器数据，并在启用时计算势能：mj_sensorPos, mj_energyPos

速度

以下阶段计算依赖于广义速度 mjData.qvel 的量。由于流水线的顺序依赖结构，实际依赖关系同时取决于 qpos 和 qvel。

13. 计算肌腱、flex 边缘和执行器的速度：mj_fwdVelocity

14. 计算物体速度和关节轴的变化率，同样是在以子树质心为中心的全局坐标系中：mj_comVel

15. 计算被动力——关节和肌腱中的弹簧阻尼器以及流体力：mj_passive

16. 计算依赖于速度的传感器数据，并在启用时计算动能（如果传感器需要，请调用 mj_subtreeVel）：mj_sensorVel

17. 计算参考约束加速度：mj_referenceConstraint

18. 计算科里奥利力、离心力和重力向量：mj_rne

控制回调

19. 如果定义了用户自定义的控制回调，则调用它：mjcb_control

力/加速度

以下阶段计算依赖于 用户输入 的量。由于流水线的顺序特性，实际依赖关系取决于整个 积分状态。

20. 计算执行器力以及定义的激活动力学：mj_fwdActuation

21. 计算除了（仍未知的）约束力之外的所有力产生的关节加速度：mj_fwdAcceleration

22. 使用所选求解器计算约束力，并更新关节加速度以考虑约束力。这产生 mjData.qacc 向量，它是前向动力学的主要输出：mj_fwdConstraint

23. 如果在启用时计算依赖于力和加速度的传感器数据（如果传感器需要，请调用 mj_rnePostConstraint）：mj_sensorAcc

24. 检查加速度是否存在无效或不可接受的巨大实数值。如果检测到发散，状态将自动重置并引发相应的警告：mj_checkAcc

25. 比较前向和逆动力学的结果，以诊断前向动力学中求解器收敛不良的问题。这是一个可选步骤，仅在启用时执行：mj_compareFwdInv

26. 使用所选积分器将仿真状态推进一个时间步长。请注意，龙格-库塔积分器会额外重复上述序列三次，但仅执行一次可选计算：mj_Euler, mj_RungeKutta, mj_implicit 中的一种

mjData 中的一致性

MuJoCo 的计算流水线完全是命令式的，没有任何事情会自动发生。这导致的行为对于熟悉其他范式的用户来说可能显得出乎意料。以下是两个有时可能会令人惊讶的预期行为示例

• 设置 状态 后，派生自状态的量不会自动对应于新状态。必须手动调用所需的阶段。例如，在设置广义位置 mjData.qpos 后，如果不先调用 mj_kinematics，笛卡尔坐标下的位置和方向将不会与 qpos 一致。

• 在 mj_step（在更新状态后立即终止）之后，mjData 中的量对应于“先前”状态（或者更准确地说，对应于先前状态和当前状态之间的“转换”）。特别地，所有依赖于位置的传感器值以及依赖于位置的计算（如运动学 雅可比矩阵）将相对于“先前位置”进行。

可重复性

MuJoCo 的仿真流水线是完全确定性和可重复的——如果保存并重新加载轨迹中的一个 状态 并再次调用 mj_step，得到的结果状态将是相同的。但是，有一些重要的注意事项

• 保存所有必需的 积分状态 组件。特别是 热启动加速度 (warmstart accelerations) 对下一个状态的影响非常小，但如果需要逐位相等，则应保存它们。

• 状态之间的任何数值差异（无论多么小）在积分后都会变得显著，尤其是对于有接触的系统。接触事件具有较高的 Lyapunov 指数；这是任何刚体仿真器（实际上也是 现实物理世界）的属性，并非 MuJoCo 特有的。

• 精确的可重复性仅在 单一版本 且在 相同架构 下得到保证。不同版本发布之间通常存在小的数值差异，例如由于代码优化。这意味着当保存初始状态和开环控制序列时，得到的滚动轨迹在同一版本内将是相同的，但在不同的 MuJoCo 版本或不同的操作系统之间可能会有所不同。

逆动力学

顶层函数 mj_inverse 调用以下计算序列。上述关于 一致性 和 可重复性 的说明也适用于此处。

1. 计算正向运动学。

2. 计算物体惯性和关节轴。

3. 计算肌腱长度和力臂。

4. 计算执行器长度和力臂。

5. 计算复合刚体惯性并形成关节空间惯性矩阵。

6. 计算关节空间惯性矩阵的稀疏分解。

7. 构建活动接触列表。

8. 构建约束雅可比矩阵并计算约束残差。

9. 计算仅依赖于位置的传感器数据，并在启用时计算势能。

10. 计算肌腱和执行器的速度。

11. 计算物体速度和关节轴的变化率。

12. 计算依赖于速度的传感器数据，并在启用时计算动能。如果传感器需要，请调用 mj_subtreeVel。

13. 计算所有被动力。

14. 计算参考约束加速度。

15. 如果设置了 invdiscrete 标志且 积分器 不是 RK4，则将输入加速度从离散时间转换为连续时间。

16. 计算约束力。这是通过分析完成的，不使用数值求解器。

17. 计算无约束系统的逆动力学。

18. 如果启用，计算依赖于力和加速度的传感器数据。如果传感器需要，请调用 mj_rnePostConstraint。

19. 通过合并所有结果来计算 mjData.qfrc_inverse 向量。这是逆动力学的主要输出。它等于外部力和驱动力的总和。

导数

MuJoCo 的整个计算流水线（包括其约束求解器）原则上都是可解析微分的。编写这些导数的高效实现是开发团队的一个长期目标。平滑动力学（不含约束）相对于速度的解析导数已经被计算出来，并支持两个 隐式积分器。

请注意，求解器阻抗 的默认值使得接触在默认情况下是不可微的，需要 设置为 0 才能使接触力的发生平滑。

目前有两个函数可以使用高效的有限差分法来计算动力学雅可比矩阵

mjd_transitionFD:

为离散时间前向动力学（mj_step）计算状态转换和控制转换雅可比矩阵。参见 文档。

mjd_inverseFD:

为连续或离散时间逆动力学（mj_inverse）计算雅可比矩阵。参见 文档。

这些导数的高效得益于 MuJoCo 可配置的计算流水线，使得不需要的量不会被重复计算。例如，当对控制进行差分时，仅依赖于位置和速度的量不会被重新计算。此外，求解器热启动、四元数和控制钳位（clamping）均得到正确处理。同时支持前向差分和中心差分。

参考文献

在此，我们提供了一份简短的带注释的参考文献列表，并将它们与正文联系起来。

计算机器人运动学和动力学的递归算法有着悠久的历史。Featherstone 的书是一个标准的参考资料。我们对 RNE 和 CRB 算法的实现以及稀疏惯性分解都是基于此书的。

18. Featherstone. Rigid Body Dynamics Algorithms. Springer, 2008.

我们用于凸网格碰撞的 MPR 算法是由 Snethen 引入的。

7. Snethen. Complex collision made simple, Game Programming Gems 7, 165-178, 2008.

Stewart 和 Trinkle 引入了线性互补问题 (LCP) 方法用于接触建模，我们在前文中讨论过该方法但未在此处实际使用。请注意，这是一个非常成熟的领域，有许多更新的论文对此进行研究。

D. Stewart and J. Trinkle. An implicit time-stepping scheme for rigid-body dynamics with inelastic collisions and coulomb friction. International Journal Numerical Methods Engineering, 39:2673-2691, 1996.

我们现在解决与我们的约束模型相关的前期工作及其在高斯原理中的根源。Udwadia 和 Kalaba 指出了推广高斯原理的可能性，从而重新激发了人们对高斯原理的兴趣。

6. Udwadia and R. Kalaba. A new perspective on constrained motion. Proceedings of the Royal Society, 1992.

第一个与接触建模相关的此类推广由 Redon 等人完成，他们将高斯原理扩展到包括加速度上的不等式约束，并将其用于模拟无摩擦接触。这产生了一个凸二次规划 (QP) 问题。

S. Redon, A. Kheddar and S. Coquillart. Gauss’s least constraint principle and rigid body simulations. IEEE International Conference on Robotics and Automation, 2002.

为了用一个更容易处理的问题来近似 LCP 问题，Anitescu 提出了一个基于加速度的 QP，它本质上是我们在此开发的接触模型的硬限制。与 Redon 等人的早期模型不同之处在于，Anitescu 使用了多个不等式形成一个金字塔，而不是每个接触仅使用一个不等式（仅在法线方向）。这就是在凸互补自由模型中从无摩擦接触过渡到摩擦接触所需的全部内容。

M. Anitescu. Optimization-based simulation of nonsmooth rigid multibody dynamics. Math. Program. Ser. A, 105:113-143, 2006.

Drumwright 和 Shell 提出了一个基于接触力的 QP，它是 Anitescu 之前开发的 QP 的对偶，也仅限于硬接触。

E. Drumwright and D. Shell, Modeling contact friction and joint friction in dynamic robotic simulation using the principle of maximum dissipation. International Workshop on the Algorithmic Foundations of Robotics, 2010.

我们当前模型的第一版是在下面的论文中开发的。这又是一个凸优化问题，但它允许软接触和其他约束，并且有一个唯一定义的逆。

E. Todorov. A convex, smooth and invertible contact model for trajectory optimization. IEEE International Conference on Robotics and Automation, 2011.

正如我们在本章中所做的那样，这些摩擦接触的凸模型中没有一个是系统地从高斯原理导出的。此处开发的增强动力学是新的。连续时间公式也是新的，背离了依赖于离散时间“速度步进”方案的现代接触求解器。

我们获得软约束模型的方式让人联想到 Open Dynamics Engine (ODE) 中的约束力混合 (CFM) 参数，尽管 ODE 基于 LCP 形式主义并且解决的是一个不同的问题。

18. Smith. Open Dynamics Engine user guide. 2006.

Lacoursiere 引入了“鬼变量 (ghost variables)”，这些变量似乎与我们的变形动力学有关。然而，它们相当难以解释（正如它们的名字所暗示的那样），与我们模型的精确关系仍有待澄清。

C. Lacoursiere. Ghosts and machines: Regularized variational methods for interactive simulations of multibodies with dry frictional contacts. PhD Thesis, Umea University, 2007.