计算#

简介#

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

Analytically-invertible dynamics with contacts and constraints: Theory and implementation in MuJoCo E. Todorov (2014).

软接触模型#

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

物理真实性和软接触#

我们接触模型的许多优点可以追溯到这样一个事实：我们放弃了LCP公式核心的严格互补性约束。我们将这类模型称为凸模型；相关工作请参见参考文献。对于无摩擦接触，放弃显式互补性约束没有区别，因为所得凸二次规划的Karush-Kuhn-Tucker（KKT）最优性条件等价于LCP。但对于有摩擦的接触，则存在差异。

如果将凸模型视为LCP的近似，那么一个逻辑上的问题是这种近似有多好。然而，我们不这么看。相反，我们认为LCP模型和凸模型都是对物理现实的不同近似，各有其优缺点。放弃严格互补性并用代价函数取而代之的直接后果是，互补性可能被违反——这意味着接触法向的力和速度可以同时为正，并且摩擦力可能不是最大耗散的。一个相关的现象是，启动滑动的唯一方法是在法向产生一些运动。这些效应在数值上很小，但并不理想。然而，这个缺点几乎没有实际意义，因为它以硬接触为前提。然而，所有物理材料都允许一定的变形。这在机器人技术中尤其重要，因为机器人与环境接触的部分通常被设计成柔软的。对于软接触，互补性必须被违反：当存在穿透且材料正在将接触体推开时，法向力和速度都为正。此外，如果一个物体停在一个有一定穿透的软表面上，我们横向推动它，我们预计它在开始滑动时会稍微向上移动。因此，与LCP的偏离实际上增加了软接触存在时的物理真实性。

当然，并非每个软模型都是可取的；例如，弹簧-阻尼器模型是软的，但饱受不稳定性之苦。同时，不同材料有不同的特性，因此与硬接触模型不同，软模型必须有足够丰富的参数化，才能适应多个感兴趣的系统。这反过来又有助于接触模型参数的系统辨识。

计算效率#

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

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

连续时间#

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

过去，摩擦接触的凸模型也依赖于离散时间近似，但这并非必要。目前的模型是在连续时间内定义的，用力和加速度来表示。鉴于现实世界中的时间是连续的，这更为自然。它也是控制文献中首选的表述方式，我们确实希望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)\)	\(\nq \times \nv\)	约束雅可比矩阵	`mjData.efc_J`
\(r(q)\)	\(\nq\)	约束残差	`mjData.efc_pos`
\(f(q, v,\tau)\)	\(\nq\)	约束力	`mjData.efc_force`

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

当使用四元数表示三维方向时，位置坐标数 \(\nq\) 大于自由度数 \(\nv\)。当模型包含球关节或自由关节时（即在大多数模型中），就会出现这种情况。在这种情况下，\(\dot{q}\) 不等于 \(v\)，至少在通常意义上是这样。相反，必须考虑刚体方向群 \(SO(3)\)——它具有四维空间中单位球的几何形状。速度存在于该球体的三维切空间中。所有内部计算都考虑到了这一点。对于自定义计算，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\) 包括科里奥利力、离心力和重力。它们的总和使用递归牛顿-欧拉（RNE）算法计算，并将加速度设置为0。
关节空间惯性矩阵 \(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 坐标系上的力和力矩。

如果一个 site 传动定义了可选的 refsite 属性，力和力矩将在参考 site 的坐标系中施加，而不是在 site 自身的坐标系中。如果定义了参考 site，驱动器的长度将非零，并对应于两个 site 的位姿差，投影到参考坐标系中选定的方向上。这个长度随后可以用 position 驱动器来控制，从而实现笛卡尔末端执行器控制。更多细节请参见 refsite 文档。

有状态驱动器#

一些驱动器，如气动和液压缸以及生物肌肉，具有一个称为“激活”的内部状态。这是一个真实的动态状态，超出了关节位置 \(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.
重力补偿力。详细信息请参见 body gravcomp 属性。
由周围介质施加的流体力。详情请参见流体力章节。

数值积分#

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

提供了四种数值积分器，三种单步积分器和多步四阶龙格-库塔积分器。在描述积分器之前，我们首先对单步欧拉积分器进行一般性描述：显式、半隐式和速度隐式。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}\]

积分器#

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

带隐式关节阻尼的半隐式方法 (Euler): 对于这种方法，\(D\) 只包含关节阻尼的导数。注意，在这种情况下，\(D\) 是对角的，\(\widehat{M}\) 是对称的，所以可以使用 \(L^TL\) 分解（Cholesky的一种变体）。这个分解存储在 mjData.qLD 中。如果模型没有关节阻尼或者设置了 eulerdamp 禁用标志，隐式阻尼将被禁用，并使用半隐式更新 (3) 而不是 (6)，从而避免了对 \(\widehat{M}\) 的额外分解（额外是因为 \(M\) 已经为加速度更新 (5) 分解了）。
速度隐式方法 (implicit): 对于这种方法，\(D\) 包含除约束力 \(J^T f(v)\) 之外所有力的导数。这些目前被忽略，因为尽管计算它们是可能的，但这很复杂，而且数值测试表明包含它们并不能带来太多好处。话虽如此，约束力的解析导数计划在未来版本中实现。此外，为了计算效率，我们限制 \(D\) 具有与 \(M\) 相同的稀疏模式。这个限制会排除连接在运动树不同分支上的物体的肌腱阻尼。由于 \(D\) 不对称，我们不能使用 Cholesky 分解，但因为 \(D\) 和 \(M\) 具有与运动树拓扑对应的相同稀疏模式，所以 \(\widehat{M}\) 的逆序 \(LU\) 分解保证没有填充。这个分解存储在 mjData.qLU 中。
快速速度隐式方法 (implicitfast): 对于这种方法，\(D\) 包含隐式方法中使用的所有力的导数，但由RNE算法计算的向心力和科里奥利力 \(c(v)\) 除外。此外，它被对称化 \(D \leftarrow (D + D^T)/2\)。去掉RNE导数的一个原因是它们计算成本最高。其次，这些力只有在复杂摆和旋转体高速旋转时才会迅速变化，这些场景不常见，并且已经可以由龙格-库塔积分器（见下文）很好地处理。因为RNE导数也是 \(D\) 不对称的主要来源，通过去掉它们并对称化，我们可以使用更快的 \(L^TL\) 分解而不是 \(LU\) 分解。
四阶龙格-库塔方法 (RK4): 我们连续时间公式的一个优点是我们可以使用更高阶的积分器，如龙格-库塔或多步法。目前唯一实现的此类积分器是固定步长的四阶龙格-库塔方法，不过用户可以轻松地通过调用 mj_forward 并自行积分加速度来实现其他积分器。我们观察到，对于能量守恒系统（示例），RK4 在稳定性和准确性方面都定性地优于单步方法，即使时间步长减少了4倍（因此计算量相同）。在存在大的速度相关力的情况下，如果所选的单步方法对这些力进行隐式积分，单步方法可以比RK4稳定得多。

选择时间步长和积分器

时间步长

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

积分器

总结：推荐的积分器是 implicitfast，它通常在稳定性和性能之间有最好的权衡。

Euler:: 为了与旧模型兼容，请使用 Euler。
implicitfast:: implicitfast 积分器的计算成本与 Euler 相似，但提供了更高的稳定性，因此是一个严格的改进。它是大多数模型的推荐积分器。
implicit:: 相对于 implicitfast 的好处是科里奥利力和向心力（包括陀螺力）的隐式积分。隐式积分这类力带来显著改善的最常见情况是，当具有不对称惯性的自由物体快速旋转时。gyroscopic.xml 展示了一个在斜面上滚动的椭球体，它在使用 implicitfast 时会迅速发散，但在使用 implicit 时是稳定的。
RK4:: 该积分器最适用于能量守恒或接近能量守恒的系统。pendulum.xml 展示了一个复杂的摆锤机制，使用 Euler 或 implicitfast 时会迅速发散，但在 RK4 下能很好地保持能量守恒。请注意，在 implicit 下，该模型不会发散，而是会损失能量。

状态#

为了完成我们对通用框架的描述，我们现在将讨论状态的概念。MuJoCo 有一个紧凑、定义明确的内部状态，它与确定性计算流程一起，意味着像重置状态和计算动力学导数这样的操作也是定义明确的。

状态完全封装在 mjData 结构体中，并由几个组件组成。这些组件在 mjtState 中以位标志的形式枚举，以及几个常见的组合，对应于下面的分组。可以使用 mj_getState 和 mj_setState 方便地从 mjData 中读取和写入连接的状态向量。

物理状态#

物理状态 (mjSTATE_PHYSICS) 包含了在步进过程中进行时间积分的主要量。这些是 mjData.{qpos, qvel, act}

位置：qpos: 广义坐标下的构型，上文表示为 \(q\)。
速度：qvel: 广义速度，上文表示为 \(v\)。
驱动器激活：act: mjData.act 包含有状态驱动器的内部状态，上文表示为 \(w\)。

完整物理状态#

完整物理状态 (mjSTATE_FULLPHYSICS) 包含物理状态和两个额外组件：

时间：time: 仿真时间由标量 mjData.time 给出。由于物理是时不变的，它被排除在物理状态之外；例外情况包括依赖时间的用户回调和插件（例如，开环控制器），在这种情况下应包含时间。
插件状态：plugin_state: mjData.plugin_state 是由引擎插件声明的状态。更多详情请参见插件状态部分。

用户输入#

这些输入字段 (mjSTATE_USER) 由用户设置并影响物理仿真，但模拟器不会修改它们。除MoCap位姿外，所有输入字段默认为0。

控制：ctrl: 控件由XML的actuator部分定义。mjData.ctrl 的值要么直接产生广义力（无状态驱动器），要么影响 mjData.act 中的驱动器激活，然后产生力。
辅助控制：qfrc_applied 和 xfrc_applied: mjData.qfrc_applied 是直接施加的广义力。

mjData.xfrc_applied 是施加在单个物体质心上的笛卡尔扳手。例如，原生查看器使用该字段来施加鼠标扰动。

请注意，qfrc_applied 和 xfrc_applied 的效果通常可以通过适当的驱动器定义来重现。
MoCap 位姿: mocap_pos 和 mocap_quat: mjData.mocap_pos 和 mjData.mocap_quat 是此处描述的特殊可选运动学状态，允许用户实时设置静态物体的位置和方向，例如从运动捕捉设备流式传输6D位姿时。由 mj_resetData 设置的默认值是物体在默认配置下的位姿。
等式约束切换：eq_active: mjData.eq_active 是一个字节值数组，允许用户在运行时切换等式约束的状态。该数组的初始值是 mjModel.eq_active0，可以在 XML 中使用等式约束的 active 属性来设置。
用户数据：userdata: mjData.userdata 作为一个用户定义的内存空间，引擎不会触及。例如，它可以被回调函数使用。这在编程章节中有更详细的描述。

热启动加速度#

qacc_warmstart

mjData.qacc_warmstart 是用于热启动约束求解器的加速度，从上一步保存。当使用像PGS这样收敛缓慢的约束求解器时，这些可以减少收敛所需的迭代次数，从而加速仿真。但请注意，默认的牛顿求解器收敛速度非常快（通常2-3次迭代），以至于热启动通常对速度没有影响，可以禁用。

不同的热启动对动力学没有可察觉的影响，但如果加载非初始状态时需要完美的数值再现性，则应保存它们。请注意，尽管它们对物理的影响可以忽略不计，但许多物理系统在时间步进时会指数级地累积微小差异，迅速导致不同热启动的轨迹发散。

积分状态#

积分状态 (mjSTATE_INTEGRATION) 是上述所有 mjData 字段的并集，构成了正向动力学的全部输入。在逆向动力学的情况下，mjData.qacc 也被视为一个输入变量。所有其他 mjData 字段都是积分状态的函数。

请注意，由 mjSTATE_INTEGRATION 给出的完整积分状态是最大化的，包含了通常未使用的字段。如果需要较小的状态大小，避免保存未使用的字段可能是明智的。特别是 xfrc_applied 可能相当大 (6 x nbody)，但通常未使用。

仿真状态：`mjData`#

仿真状态是整个 mjData 结构体及其相关内存缓冲区。该状态包括在动力学计算期间计算的所有派生量。因为 mjData 缓冲区是为最坏情况预先分配的，所以从积分状态重新计算派生量通常比使用 mj_copyData 要快得多。

约束模型#

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

等式#

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

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

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

connect3

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

weld6

此约束将两个物体焊接在一起，抑制它们之间所有的相对自由度。约束求解器强制执行的相对物体位置和方向是 mjModel 中的参数。编译器根据定义模型的初始配置（即 mjModel.qpos0）计算它们，但用户可以稍后更改。6D残差有一个与连接约束相同的三维位置分量，后面跟着一个三维方向分量。后者定义为 \(\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 中的原因。通常，mjModel 中所有以0结尾的字段名都是编译器在初始模型配置 mjModel.qpos0 中计算的量。

distance1

注意

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

摩擦损失#

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

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

joint1, 3 or 6: 摩擦损失不仅可以为标量关节（滑动和铰链）定义，也可以为具有3个自由度的球关节和具有6个自由度的自由关节定义。当定义时，它独立地应用于受影响关节的所有自由度。frictionloss参数具有与关节兼容的隐含单位（线性或角度）。自由关节是一个例外，因为它们既有线性的也有角度的分量，而MJCF模型格式允许每个关节只有一个frictionloss参数。在这种情况下，同一个参数用于线性和角度分量。有人可能会说，自由关节中的摩擦损失应该是不允许的。然而，我们允许它，因为它可以模拟有用的非物理效应，例如将一个物体固定在原位，直到有东西用足够的力推动它。
tendon1: 肌腱是标量，因此为肌腱定义摩擦损失总是增加一个标量约束。对于空间肌腱，这可以用来模拟肌腱与其缠绕的表面之间的摩擦。不过，这种摩擦将是与载荷无关的。要构建这种现象的更详细模型，可以创建几个小的浮动球体，并用肌腱串联起来。然后，球体与周围表面之间的接触将具有与载荷相关的（即库仑）摩擦，但这模拟起来效率较低。

限制#

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

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

joint1 or 2: 可以为标量关节（铰链和滑动）以及球关节定义限制。标量关节按上述方式处理。球关节限制应用于关节四元数的指数映射或角轴表示，即向量 \((\theta x, \theta y, \theta z)\)，其中 \(\theta\) 是旋转角度，\((x, y, z)\) 是对应于旋转轴的单位向量。限制应用于旋转角度 \(\theta\) 的绝对值。在运行时，限制由两个范围参数中较大的一个决定。然而，为了语义清晰，应该使用第二个范围参数来指定限制，并将第一个范围参数设置为0。编译器强制执行此规则。
tendon1 or 2: 肌腱是标量，其限制按上述方式处理。请注意，固定肌腱（它是标量关节位置的线性组合）可以有正的“长度”和负的“长度”，因为关节位置是相对于关节参考定义的，可以是正的也可以是负的。然而，空间肌腱具有真实的长度，不能为负。在为肌腱限制设置范围和边距时请记住这一点。

接触#

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

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

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

参数	描述
`condim`	接触力和力矩在接触坐标系中的维度。可以是 1、3、4 或 6。
`friction`	维度为 `condim-1` 的摩擦系数向量。具体系数的语义见下文。
`margin`	用于确定接触是否应包含在全局接触数组 `mjData.contact` 中的距离边界。
`gap`	对于自定义计算，有时将接触包含在 `mjData.contact` 中但不产生接触力会很方便。这就是 `gap` 的作用：只有当法线距离低于（margin - gap）时，才会产生接触力。
`solref` 和 `solimp`	求解器参数，稍后解释。

接触摩擦锥可以是椭圆锥或棱锥。这是一个全局设置，由所选的约束求解器决定：椭圆求解器使用椭圆锥，而棱锥求解器使用棱锥，具体定义见后文。condim 参数决定了接触类型，其含义如下：

condim = 1椭圆锥为 1，棱锥为 1: 这对应于无摩擦接触，只增加一个标量约束。回想一下，接触坐标系的第一个轴是接触法线。无摩擦接触只能沿法线方向产生力。这与关节或肌腱的限位非常相似，但它应用于两个几何体之间的距离。
condim = 3椭圆锥为 3，棱锥为 4: 这是一个常规的摩擦接触，可以产生法向力以及抵抗滑动的切向摩擦力。对这个数字的一种解释是，一个平面物体在重力作用下开始滑动的表面的斜率。
condim = 4椭圆锥为 4，棱锥为 6: 除了法向力和切向力，这种接触还可以产生抵抗绕接触法线旋转的扭转摩擦力矩，对应于接触面片产生的力矩。这对于模拟柔软的手指很有用，并且可以显著提高模拟抓取的稳定性。扭转摩擦系数的单位是长度，可以解释为接触面片的直径。
condim = 6椭圆锥为 6，棱锥为 10: 这种接触可以抵抗两个几何体之间所有相对自由度的运动。特别是，它增加了滚动摩擦，例如可以用来阻止一个球在平面上无限滚动。现实世界中的滚动摩擦是由于接触点附近的局部变形耗散能量而产生的。它可以用来模拟轮胎和道路之间的滚动摩擦，以及普遍用于稳定接触。滚动摩擦系数的单位也是长度，可以解释为能量耗散的局部变形的深度。

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

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

\[\begin{aligned} \text{椭圆锥 (elliptic cone)}: & & \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{棱锥 (pyramidal cone)}: & & \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\)-by-\(\nv\) 矩阵 \(S\)，它将关节速度 \(v\) 映射到在接触坐标系中表示的空间速度 \(S v\)。这是通过将接触点视为属于其中一个几何体，计算其空间雅可比矩阵，然后将这两个雅可比矩阵相减得到 \(S\) 来完成的。我们使用的约定是接触力从第一个几何体作用到第二个几何体，因此第一个几何体的空间雅可比矩阵为负号。接触雅可比矩阵然后是 \(E^T S\)。与所有其他约束一样，该矩阵被插入到系统级雅可比矩阵 \(J\) 中。

约束求解器#

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

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

原始公式基于高斯最小约束原理的推广。在其基本形式中，高斯原理指出，如果我们有无约束动力学 \(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\) 的最小可能偏差，其中测量关节坐标偏差的度量由惯性矩阵给出。这个原理已知等价于拉格朗日-达朗贝尔约束运动原理。在这里，我们将用它来获得一个丰富而有原则的软约束模型。这将通过推广高斯原理中的成本函数和约束来完成。

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

符号	大小	描述
\(z\)	\(\nq\)	约束变形
\(\omega\)	\(\nq\)	约束变形的速度
\(k\)	\(\nq\)	虚拟约束刚度
\(b\)	\(\nq\)	虚拟约束阻尼
\(d\)	\(\nq\)	约束阻抗
\(A(q)\)	\(\nq \times \nq\)	约束空间中的逆惯量
\(R(q)\)	\(\nq \times \nq\)	约束空间中的对角正则化项
\(\ar\)	\(\nq\)	约束空间中的参考加速度
\(\au(q, v, \tau)\)	\(\nq\)	约束空间中的无约束加速度
\(\ac(q, v, \dot{v})\)	\(\nq\)	约束空间中的有约束加速度
\(\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\)。这是因为我们的优化问题依赖于约束空间加速度的差值，所以即使我们计算了，这个项也会被抵消掉。

请注意，逆问题中的二次项由 \(R\) 而不是 \(A+R\) 加权。这告诉我们两件事。首先，在 \(R \to 0\) 的极限下（对应于硬约束），逆问题不再定义，正如所预期的那样。其次，更有用的是，逆问题是对角的，即它解耦为关于单个约束力的独立优化问题。唯一剩下的耦合是由于约束集 \(\Omega\)，但该集在前面讨论的概念性约束上也是解耦的。事实证明，所有这些独立的优化问题都可以解析求解。唯一不平凡的情况是椭圆摩擦锥模型；我们已在上述引用的论文中展示了如何处理它。它需要对 \(R\) 的对角值进行一定的耦合，MuJoCo 会自动强制执行这种耦合，以便为每个模型提供精确的解析逆解。

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

算法#

这里我们描述用于解决上述凸优化问题的数值算法（或称“求解器”）。Newton 和 CG 求解器使用简化原始公式 (8)，而 PGS 求解器使用对偶公式 (9)。请注意，数值求解器仅在正向动力学中需要。逆向动力学是解析处理的。

每个求解器算法都可以与棱锥和椭圆摩擦锥一起使用，并且可以使用约束雅可比矩阵和相关矩阵的稠密和稀疏表示。

CG共轭梯度法

该算法使用非线性共轭梯度法，采用 Polak-Ribiere-Plus 公式。线搜索是精确的，在一维中使用牛顿法，并带有解析的二阶导数。

Newton牛顿法

该算法实现了精确的牛顿法，具有解析的二阶导数和海森矩阵的 Cholesky 分解。线搜索与 CG 方法中的相同。它是默认的求解器。

PGS投影高斯-赛德尔法

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

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

约束岛#

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

不同的岛需要不同数量的迭代才能收敛，而整体求解将运行最慢的岛所需的次数。
无约束的自由度完全不受求解器影响，否则求解器需要发现它们不受影响。
求解独立的岛可以进行多线程处理。

已知问题

PGS 求解器尚不支持岛化。

参数#

这里我们解释如何从模型参数计算量 \(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\)。因此，引入一个直接控制插值的模型参数是很自然的。我们称这个参数为*阻抗*，并用 \(d\) 表示它。它是一个维度为 \(\nq\) 的向量，按元素满足 \(0。一旦指定了它，我们就计算正则化器的对角元素为

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

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

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

因此，我们将达到期望的插值效果。当然，这在一般情况下并不完全成立，但这里的目标是构建一个合理且直观的约束模型参数化，并正确设置缩放比例。

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

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

回想一下，\(r\) 是位置残差（对于摩擦损失和椭圆锥的摩擦维度，它为零），而 \(J v\) 是投影在约束空间中的关节速度；索引符号指的是投影速度向量的一个分量。

总而言之，用户指定阻抗系数向量 \(0、阻尼系数向量 \(b > 0\) 和刚度系数向量 \(k > 0\)。然后 MuJoCo 如上所示计算量 \(R, \ar\)，并应用选定的优化算法来解决问题 (9)。如建模章节的求解器参数部分所解释，MuJoCo 提供了额外的自动化功能来设置 \(d, b, k\)，以实现临界阻尼，或通过随距离改变 \(d\) 来模拟软接触层。

摩擦锥#

如上所述，MuJoCo 同时允许椭圆摩擦锥和它们的棱锥近似；所选的求解器决定了使用哪种类型的摩擦锥。棱锥近似有 \(2 (n-1)\) 条边，其中 \(n\) 是由 condim 指定的接触空间维度。我们可以增加更多的边来更好地近似底层的椭圆锥，但这毫无意义，因为得到的求解器会比其椭圆对应物更慢。

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

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

碰撞检测#

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

选择#

如果一个模型有 \(n\) 个几何体，那么有 \(n (n-1)/2\) 个可能碰撞的几何体对。对所有这些对进行详细检查（也称为近相碰撞检测）对于大型系统来说成本高得令人望而却步。幸运的是，其中一些潜在的碰撞是不希望发生的，因此在建模阶段被用户排除，而其他一些则可以在没有详细检查的情况下被快速剪枝。MuJoCo 有灵活的机制来决定哪些几何体对被详细检查。这个决策过程包括两个阶段：生成和过滤。

生成

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

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

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

过滤

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

两种几何体的类型必须对应于一个能够执行详细检查的碰撞函数。这通常是成立的，但也有例外（例如不支持平面-平面碰撞），此外，用户可以用 NULL 指针覆盖默认的碰撞函数表，从而有效地禁用某些几何体类型之间的碰撞。
应用包围球测试，并考虑接触边界。如果对中的一个几何体是平面，则变为平面-球体测试。
两个几何体不能属于同一个物体。此外，它们不能属于父物体和子物体，除非父物体是世界物体。其动机是为了避免物体内部和关节内的永久接触。请注意，如果多个物体被焊接在一起，即它们之间没有关节，那么在本测试中它们被视为单个物体。父子过滤测试可以由用户禁用，而同物体测试不能被禁用。
两个几何体必须在以下意义上“兼容”。每个几何体都有整数参数 contype 和 conaffinity。下面的布尔表达式必须为真，测试才能通过：

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

这要求一个几何体的 contype 和另一个几何体的 conaffinity 有一个共同的位被设置为 1。这是从开放动力学引擎（Open Dynamics Engine）借鉴的一个强大机制。所有几何体的默认设置是 contype = conaffinity = 1，这总是能通过测试，所以如果用户一开始觉得这个机制很困惑，可以忽略它。

检查#

详细的碰撞检查，也称为*近相*或窄相碰撞检测，由依赖于对中几何体类型的函数执行。窄相碰撞函数表可以在 engine_collision_driver.c 的顶部查看，并向希望安装自己的碰撞器的用户公开为 mjCOLLISIONFUNC。MuJoCo 支持多种基本几何形状：平面、球体、胶囊体、圆柱体、椭球体和长方体。它还支持三角网格和高度场。

除了 SDF 插件（参见其中的文档）这一显著例外，碰撞检测仅限于*凸*几何体。所有基本类型都是凸的。高度场不是凸的，但在内部它们被视为一组三棱柱（使用超出上述过滤器的自定义碰撞剪枝）。用户指定的网格可以是非凸的，并且也按此方式渲染。然而，为了碰撞目的，它们被替换为其凸包（在 simulate 中用“H”键可视化），由 qhull 库计算。

凸体碰撞#

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

原生流水线（默认）: 原生 CCD 流水线（“nativeccd”）是在 MuJoCo 中原生实现的，基于 Gilbert-Johnson-Keerthi 和 Expanding Polytope 算法（GJK / EPA）。原生流水线比基于 MPR 的流水线更快、更鲁棒。
libccd 流水线（传统）: 这个传统流水线基于 libccd 库，并使用 Minkowski Portal Refinement（MPR）。通过禁用 nativeccd 标志来激活它。

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

多点接触#

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

libccd 流水线（传统）: 通过将两个几何体绕切向轴旋转 ±1e-3 弧度并重新运行碰撞程序来找到多个接触点。如果检测到新的接触点，则将其添加，最多允许 4 个额外的接触点。这种方法是有效的，但将每次碰撞调用的成本增加了 5 倍。
原生流水线: 原生 multiccd 通过对解处的接触表面进行新颖的分析来发现多个接触，避免了碰撞程序的完全重新运行，因此实际上是“免费的”。请注意，原生 multiccd 目前不支持正的接触边界。如果两个几何体中有一个具有正的边界，原生 multiccd 将回退到传统算法。

几何体距离#

上面描述的窄相碰撞函数驱动着 mj_geomDistance 函数和相关的碰撞传感器。由于 MPR 的局限性，传统流水线将返回不正确的值（上图），除非在相对于几何体尺寸非常小的距离处，因此不鼓励用于此用例。相比之下，基于 GJK 的原生流水线（下图）在所有距离处计算正确的值。

凸分解#

为了模拟除高度场之外的非凸物体，用户必须将其分解为多个凸几何体（可以是基本形状或网格）的并集，并将它们附加到同一个物体上。此规则的另一个例外（除了高度场）是有符号距离函数（请参阅其中的文档），在某些情况下（例如解析 SDF）可以很高效，但有其他要求和限制。

开源的网格分解工具，如 CoACD 库，可以在 MuJoCo 之外使用来自动化这个过程。最后，所有内置的碰撞函数都可以用自定义回调函数替换。例如，这可以用于集成一个通用的“三角汤”碰撞检测器。但是，我们不推荐这种方法。预处理几何体并将其表示为凸几何体的并集需要一些工作，但在运行时会得到回报，并产生更快、更稳定的模拟。

休眠岛#

休眠是一种性能优化，通过将被检测为静止的模拟可移动元素暂时从流水线中移除（“使其休眠”）。当模型包含大量被动对象时，这种优化最为有用。可以休眠或唤醒的最小单位是运动学树，然而，树总是与通过约束连接到它们的其他树一起进入休眠状态，因此有了“休眠岛”这个术语。

右侧的视频演示了休眠的几个方面。首先，我们展示了多米诺骨牌模型，它模拟了传统的多米诺骨牌倒下的“链式反应”。除了一个骨牌外，所有骨牌都处于稳定平衡状态并迅速进入休眠，但第一个不稳定的骨牌保持唤醒状态并开始倒下。每当唤醒和休眠的骨牌之间发生接触时，后者会自动被唤醒。在地面上稳定后，一堆堆的骨牌再次进入休眠状态，其相关的接触也随之消失。这个序列在启用岛屿可视化的情况下重复，该可视化根据岛屿的第一个自由度重新着色几何体，如果处于休眠状态则使用较深的颜色。在子剪辑的末尾，休眠被关闭和打开，展示了休眠带来的速度提升（右下角）。第二个子剪辑显示了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 流水线不支持龙格-库塔积分器。
mj_fwdPosition 调用阶段 2-11，即流水线中依赖于位置的部分。

正向动力学流水线示意图分解

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

阶段#

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

检查位置和速度是否存在无效或过大的实数值，这表明发生了发散。如果检测到发散，状态将自动重置并引发相应的警告：mj_checkPos, mj_checkVel

位置#

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

计算正向运动学。这将产生所有物体、几何体、站点、相机和灯光的全局位置和方向。它还对所有四元数进行归一化：mj_kinematics, mj_camlight
计算物体惯量和关节轴，在以相应运动学子树质心为中心的全局坐标系中：mj_comPos
计算与柔性体对象相关的量：mj_flex
计算肌腱长度和力臂。这包括计算空间肌腱的最小长度路径：mj_tendon
计算复合刚体惯量和关节空间惯性矩阵：mj_makeM
计算关节空间惯性矩阵的稀疏分解：mj_factorM
构建活动接触列表。这包括宽相和近相碰撞检测：mj_collision
构建约束雅可比矩阵，计算约束残差，构建岛：mj_makeConstraint, mj_island (尚未在 API 中公开)
计算执行器长度和力臂：mj_transmission
计算约束求解器所需的矩阵和向量：mj_projectConstraint
计算仅依赖于位置的传感器数据，以及如果启用则计算势能：mj_sensorPos, mj_energyPos

速度#

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

计算肌腱、柔性体边和执行器的速度：mj_fwdVelocity
计算物体速度和关节轴的变化率，同样是在以子树质心为中心的全局坐标系中：mj_comVel
计算被动力——关节和肌腱中的弹簧-阻尼器，以及流体力：mj_passive
计算依赖于速度的传感器数据，以及如果启用则计算动能（如果传感器需要，调用 mj_subtreeVel）：mj_sensorVel
计算参考约束加速度：mj_referenceConstraint
计算科里奥利力、离心力和重力的向量：mj_rne

控制回调#

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

力/加速度#

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

如果定义了，则计算执行器力和激活动态：mj_fwdActuation
计算除（仍然未知）约束力之外的所有力产生的关节加速度：mj_fwdAcceleration
使用所选求解器计算约束力，并更新关节加速度以计入约束力。这将产生向量 mjData.qacc，它是正向动力学的主要输出：mj_fwdConstraint
如果启用，则计算依赖于力和加速度的传感器数据（如果传感器需要，则调用 mj_rnePostConstraint）：mj_sensorAcc
检查加速度是否存在无效或不可接受的大实数值。如果检测到发散，状态将自动重置并引发相应的警告：mj_checkAcc
比较正向和逆向动力学的结果，以便诊断正向动力学中求解器收敛性差的问题。这是一个可选步骤，仅在启用时执行：mj_compareFwdInv
使用所选的积分器将仿真状态推进一个时间步。请注意，龙格-库塔积分器会额外重复上述序列三次，但可选计算仅执行一次：mj_Euler、mj_RungeKutta、mj_implicit 中的一个

`mjData` 中的一致性#

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

设置状态后，从状态派生的量不会自动与新状态对应。必须手动调用所需的一个或多个阶段。例如，在设置广义位置 mjData.qpos 之后，如果不首先调用 mj_kinematics，笛卡尔位置和方向将与 qpos 不一致。
在 mj_step 之后，它会在更新状态后立即终止，mjData 中的量对应于*上一个*状态（或者更准确地说，是*上一个*和*当前*状态之间的*过渡*）。特别是，所有与位置相关的传感器值和与位置相关的计算（如运动学雅可比矩阵）都将是相对于*之前的位置*。

可复现性#

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

保存所有必需的积分状态组件。特别是，热启动加速度对下一个状态的影响非常小，但如果需要比特级的相等性，则应保存它们。
状态之间的任何数值差异，无论多小，在积分后都会变得显著，特别是对于有接触的系统。接触事件具有很高的李雅普诺夫指数；这是任何刚体模拟器（乃至真实世界物理）的特性，并非 MuJoCo 特有。
精确的可复现性仅在**单一版本**、**相同架构**上得到保证。在不同版本发布之间，微小的数值差异是很常见的，例如由于代码优化。这意味着，当保存一个初始状态和一个开环控制序列时，所得到的展开轨迹在同一版本内将是相同的，但在不同的 MuJoCo 版本或不同的操作系统之间可能会有所不同。

逆向动力学#

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

计算正向运动学。
计算物体惯性和关节轴。
计算肌腱长度和力臂。
计算执行器长度和力臂。
计算复合刚体惯性并形成关节空间惯性矩阵。
计算关节空间惯性矩阵的稀疏分解。
构建活动接触列表。
构建约束雅可比矩阵并计算约束残差。
如果启用，计算仅依赖于位置的传感器数据和势能。
计算肌腱和执行器速度。
计算物体速度和关节轴变化率。
如果启用，计算依赖于速度的传感器数据和动能。如果传感器需要，调用 mj_subtreeVel。
计算所有被动力。
计算参考约束加速度。
如果 invdiscrete 标志被设置，且积分器不是 RK4，则将输入加速度从离散时间转换为连续时间。
计算约束力。这是通过解析方式完成的，不使用数值求解器。
计算无约束系统的逆向动力学。
如果启用，计算依赖于力和加速度的传感器数据。如果传感器需要，调用 mj_rnePostConstraint。
通过组合所有结果计算向量 mjData.qfrc_inverse。这是逆向动力学的主要输出。它等于外力和驱动力之和。

导数#

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

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

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

mjd_transitionFD:: 为离散时间正向动力学（mj_step）计算状态转换和控制转换雅可比矩阵。请参阅文档。
mjd_inverseFD:: 为连续或离散时间逆向动力学（mj_inverse）计算雅可比矩阵。请参阅文档。

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

参考文献#

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

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

Featherstone. Rigid Body Dynamics Algorithms. Springer, 2008.

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

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

我们讨论过但实际上并未使用过的线性互补（LCP）接触建模方法是由 Stewart 和 Trinkle 引入的。请注意，这是一个发展成熟的领域，有更多关于它的近期论文。

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 通过指出其推广的可能性，重新激发了人们对高斯原理的兴趣。

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.

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

我们获得软约束模型的方式让人联想到开放动力学引擎（ODE）中的约束力混合（CFM）参数，尽管 ODE 基于 LCP 形式主义并解决一个不同的问题。

Smith. Open Dynamics Engine user guide. 2006.

Lacoursiere 引入了“幽灵变量”，这似乎与我们的形变动力学有关。然而，它们有些难以解释（正如其名所示），与我们模型的具体关系仍有待阐明。

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

计算#

简介#

软接触模型#

物理真实性和软接触#

计算效率#

连续时间#

逆动力学与优化#

通用框架#

驱动模型#

传动#

有状态驱动器#

力生成#

被动力#

数值积分#

积分器#

状态#

物理状态#

完整物理状态#

用户输入#

热启动加速度#

积分状态#

仿真状态：mjData#

约束模型#

等式#

摩擦损失#

限制#

接触#

约束求解器#

原始问题#

简化原始问题#

对偶问题#

算法#

约束岛#

参数#

摩擦锥#

碰撞检测#

选择#

检查#

凸体碰撞#

多点接触#

几何体距离#

凸分解#

休眠岛#

模拟流水线#

正向动力学#

顶层#

阶段#

位置#

速度#

控制回调#

力/加速度#

mjData 中的一致性#

可复现性#

逆向动力学#

导数#

参考文献#

仿真状态：`mjData`#

`mjData` 中的一致性#