计算

引言

本章介绍 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 模型和凸模型都视为对物理现实的不同近似，各有优缺点。放弃严格互补并将其替换为成本函数带来的直接结果是，互补性可能被违反——这意味着接触法向方向上的力和速度可以同时为正，并且摩擦力可能不是最大耗散的。一个相关的现象是，启动滑动的唯一方法是在法向方向上产生一些运动。这些影响在数值上很小但不理想。然而，这个缺点几乎没有实际相关性，因为它基于硬接触的假设。然而，所有物理材料都允许一定的变形。这在机器人学中尤其重要，机器人接触环境的部件通常设计成柔软的。对于软接触，必须违反互补性：当发生穿透且材料将接触体推开时，法向力和速度都为正。此外，如果一个物体以一定穿透 resting 在柔软表面上，我们将其侧向推动，我们预期它在开始滑动时会稍微向上移动。因此，LCP 的偏差实际上增加了软接触下的物理真实性。

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

计算效率

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

计算效率的要求因用例而异。如果只需要实时仿真，现代计算机的速度足够处理大多数感兴趣的机器人系统，即使使用低效的求解器。然而，在优化的上下文中，不存在足够快的仿真。如果目标函数及其导数可以计算得更快，这意味着更大的搜索范围或训练集或样本量，从而提高性能。这就是我们投入大量精力开发高效求解器的原因。

连续时间

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

凸摩擦接触模型过去也依赖于离散时间近似，但这并非必需。当前的模型是基于力和加速度，在连续时间下定义的。考虑到现实世界的时间是连续的，这更自然。它也是控制文献中偏好的公式，事实上，我们希望 MuJoCo 能够吸引来自该领域的用户。连续时间公式的另一个优势是它们易于使用复杂的数值积分，而无需承担离散时间变分积分器（在惯性与构型相关时必然是隐式的）的计算开销。连续时间动力学在时间逆向方面也定义明确，这在某些优化算法中是必需的。

逆动力学与优化

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

逆动力学在系统辨识、估计和控制中出现的优化算法中起着关键作用。它们使得将位置序列（或其参数化表示）视为被优化的对象成为可能。然后通过对位置进行微分计算速度和加速度；逆动力学用于计算施加力和接触力；最后构建一个可以依赖于所有这些量级的目标函数。这通常互换地被称为时空优化、谱方法、直接配置法。MuJoCo 在处理接触和其他约束的情况下特别适合促进此类计算。

通用框架

我们的符号总结如下表。稍后将介绍针对约束的附加符号。如果可用，我们还展示了与数学符号对应的主数据结构mjModel和mjData的字段。

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

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

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

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{forward:} & & \dot{v} &= M^{-1} (\tau + J^T f - c) \\ \text{inverse:} & & \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\) 是一个 \(n_V\) 维的力臂向量。它决定了从标量驱动器力到关节力的映射。传动属性由驱动器附着的 MuJoCo 对象决定；可能的附着对象类型包括关节、肌腱、父体中的关节、滑块曲柄、站点和物体。

关节和肌腱

关节和肌腱传动类型按预期工作，对应于驱动器对目标对象施加力或扭矩。球关节比较特殊，详情请参阅驱动器/通用/关节文档。

父体中的关节

父体中的关节 (jointinparent) 传动是球关节和自由关节所独有的，它表明旋转应在父体坐标系而非子体坐标系中测量。

滑块曲柄

滑块曲柄传动将线性力转换为扭矩，如同活塞式内燃机中一样。此模型包含教学示例。滑块曲柄也可以通过创建 MuJoCo 物体并使用等式约束将它们耦合起来进行显式建模，但这效率较低且稳定性较差。

物体

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

站点

站点传动对应于在站点坐标系中施加笛卡尔力/扭矩。如果未定义参考站点 (refsite)（见下文），这些目标具有固定的零长度 \(l_i(q) = 0\)，适用于建模固定在站点坐标系中的力或扭矩，例如喷射器和螺旋桨。

如果站点传动定义了可选的参考站点 (refsite) 属性，力或扭矩将施加在参考站点的坐标系中，而不是站点自身的坐标系中。如果定义了参考站点，则驱动器的长度非零，并对应于两个站点的位姿差，投影到参考坐标系中的所选方向上。然后可以使用位置驱动器控制此长度，从而实现笛卡尔末端执行器控制。详情请参阅参考站点 (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)属性设置为“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{or} \; 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 可以计算三种类型的被动力

• 关节和肌腱中的弹簧-阻尼器。详情请参阅以下属性。
  关节：刚度、弹簧参考位置、阻尼、弹簧阻尼器。
  肌腱：刚度、弹簧长度、阻尼。

• 重力补偿力。详情请参阅物体重力补偿 (gravcomp)属性。

• 周围介质施加的流体作用力。详情请参阅流体作用力章节。

数值积分

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

提供四种数值积分器：三种单步积分器和多步四阶龙格-库塔积分器。在描述积分器之前，我们先对单步欧拉积分器进行一般性描述：显式、半隐式和速度隐式。MuJoCo 不支持显式欧拉法，但它具有教学价值。它可以写成

(2)\[\begin{aligned} \textrm{activation:}\quad w_{t+h} &= w_t + h \dot{w}_t \\ \textrm{velocity:}\quad v_{t+h} &= v_t + h a_t \\ \textrm{position:}\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}\) 的非线性方程，可以通过围绕 \(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\) 矩阵的定义不同，它始终是解析计算的。

带隐式关节阻尼的半隐式 (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:

对于旧模型和MJX，使用Euler以保持兼容性。特别是对于 MJX，设置欧拉阻尼 (eulerdamp)禁用标志可以提高性能。

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) 由用户设置并影响物理仿真，但不受仿真器修改。除了动作捕捉姿态外，所有输入字段默认为 0。

控制：ctrl

控制量由 XML 的驱动器部分定义。mjData.ctrl值要么直接产生广义力（无状态驱动器），要么影响mjData.act中的驱动器激活，然后激活产生力。

辅助控制：qfrc_applied 和 xfrc_applied

mjData.qfrc_applied是直接施加的广义力。

mjData.xfrc_applied是施加到各个物体质心的笛卡尔力偶。此字段例如用于原生查看器施加鼠标扰动。

注意，qfrc_applied和xfrc_applied的效果通常可以通过适当的驱动器定义重新创建。

动作捕捉姿态：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 具有非常灵活的约束模型，但仍由稍后描述的求解器以统一方式处理。这里我们概念上解释单个约束是什么，以及它们如何在维度为 \(n_C\) 的系统级向量和矩阵中布置。每个概念约束可以贡献一个或多个标量约束到总数 \(n_C\)，并且每个标量约束在约束雅可比 \(J\) 中有一行对应。活动约束按类型排序（类型顺序如下文所述），然后在每种类型内按模型元素排序。类型包括：等式、摩擦损失、限位、接触。限位被求解器视为无摩擦接触，内部不作为单独类型处理。我们在mjData中使用前缀 efc 来表示与约束相关的系统级向量和矩阵。

等式

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

除此之外，等式约束还可以用于创建“循环关节”，即无法通过运动学树建模的关节。游戏引擎都以这种方式表示所有关节。这在 MuJoCo 中也可以做到，但不推荐——因为它会导致仿真更慢、更不准确，实际上是将 MuJoCo 变成一个游戏引擎。使用等式约束表示关节的唯一原因可能是为了模拟软关节——这可以通过约束求解器完成，但无法通过运动学树完成。如果需要一些松弛，可以通过调整求解器参数来软化约束。这在计算上更高效，不仅因为它涉及一个标量约束而不是两个，还因为求解等式约束力通常更快。

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

连接 (connect)3

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

焊接 (weld)6

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

关节 (joint)1

此约束仅适用于标量关节：铰链和滑动。它可用于将一个关节锁定在恒定位置，或通过四次多项式耦合两个关节。锁定关节通过移除关节能更好地实现，但在特殊情况下（例如通过软等式约束建模间隙）可能有用。耦合两个关节对于建模螺旋关节或其他形式的机械耦合很有用。四次多项式模型定义如下。

\[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\)。

肌腱 (tendon)1

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

距离 (distance)1

注意

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

摩擦损失

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

摩擦损失与其他所有约束类型不同，没有与之关联的位置残差；因此我们正式地将 \(r(q)\) 的相应分量设为零。确实，我们稍后会看到我们的约束求解器公式需要以不同寻常的方式扩展以纳入此约束。然而，受影响关节或肌腱的速度充当速度“残差”——因为约束的作用是减小此速度，理想情况下保持为零。因此，约束雅可比中对应的块就是关节位置（或肌腱长度）相对于 \(q\) 的雅可比。对于标量关节，它是在关节地址处为 1 的零向量。对于肌腱，这被称为力臂向量。

关节 (joint)1, 3 或 6

摩擦损失不仅可以为标量关节（滑动和铰链）定义，还可以为具有 3 个自由度的球关节和具有 6 个自由度的自由关节定义。定义时，它独立应用于受影响关节的所有自由度。frictionloss 参数具有与关节兼容的隐式单位（线性的或角度的）。自由关节是例外，因为它们同时具有线性和角分量，而 MJCF 模型格式允许每个关节使用单个 frictionloss 参数。在这种情况下，同一参数用于线性和角分量。可以论证自由关节不应该允许摩擦损失。然而，我们允许它，因为它可以建模有用的非物理效应，例如将物体固定在位，直到有足够大的力推动它。

肌腱 (tendon)1

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

限位

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

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

关节 (joint)1 或 2

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

tendon1 或 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\) 乘 \(n_V\) 的矩阵 \(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{subject to} \; J x = \ar \]

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

除了前面介绍的符号外，我们将使用以下符号

符号	尺寸	描述
\(z\)	\(n_C\)	约束变形
\(\omega\)	\(n_C\)	约束变形速度
\(k\)	\(n_C\)	虚拟约束刚度
\(b\)	\(n_C\)	虚拟约束阻尼
\(d\)	\(n_C\)	约束阻抗
\(A(q)\)	\(n_C \times n_C\)	约束空间中的逆惯性
\(R(q)\)	\(n_C \times n_C\)	约束空间中的对角线正则项
\(\ar\)	\(n_C\)	约束空间中的参考加速度
\(\au(q, v, \tau)\)	\(n_C\)	约束空间中的无约束加速度
\(\ac(q, v, \dot{v})\)	\(n_C\)	约束空间中的约束加速度
\(\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{subject to} \; 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)\) 扮演软约束惩罚的角色。可以证明它是凸的且一次连续可微。对于锥形摩擦锥，它是一个二次样条。

约化公式的另一个吸引人的特点是，逆向动力学可以轻松计算。由于上述问题是无约束且凸的，唯一的全局最小值使得梯度为零。

这得到恒等式

这是在软约束存在下的解析逆向动力学。与运动方程 (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{subject to} \; \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{subject to} \; \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牛顿法

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

PGS投影高斯-赛德尔法

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

使用锥形摩擦锥时，问题涉及箱式约束，PGS 传统上应用于此。如果我们将 PGS 直接应用于椭圆摩擦锥产生的锥形约束，它将陷入连续的局部最小值；参见左图。这是因为它只能沿着坐标轴取得进展。右图展示了我们解决此问题的方法。我们仍然一次更新一个接触，但在一个接触内，我们沿着适应约束表面的非正交轴进行更新，如下所示。首先，我们沿着从锥尖穿过当前解的射线优化二次成本。然后我们用一个穿过当前解且垂直于接触法线的超平面切开锥。这产生一个椭球体——考虑到我们的接触模型，它可以高达 5 维。现在我们在此椭球体内部优化二次成本。这是一个二次约束二次规划（QCQP）的例子。由于只有一个标量约束（无论它多么非线性），对偶是关于未知拉格朗日乘子的标量优化问题。我们使用牛顿法求解此问题直至收敛——在实践中需要少于 10 次迭代，并且涉及小型矩阵。总的来说，该算法对于锥形锥具有与 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\)。它是一个维度为 \(n_C\) 的向量，满足逐元素 \(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\) 的优化问题等价于约束优化的罚函数法，其中惩罚是从约束边界开始的半二次函数。可以将其视为边界投射的“阴影”。无论近似多么精确，椭圆锥与其锥形近似的阴影形状是不同的。下图说明了 2D 接触的这种效应，其中金字塔甚至不是近似，而是代表与椭圆锥相同的约束集。我们绘制了锥形（红色）和椭圆（虚线蓝色）锥的惩罚/阴影等高线，摩擦系数从左到右变化。从数学上讲，锥形情况下的惩罚是二次样条，而椭圆情况下的惩罚包含二次函数减去二次函数平方根的部分——允许围绕锥尖形成圆形等高线。

总而言之，椭圆和锥形摩擦锥定义了不同的软接触动力学（尽管它们通常非常接近）。椭圆模型更具原则性，与物理直觉更一致，相应的求解器效率也很高，但根据模型不同，可能不如锥形求解器高效。

碰撞检测

碰撞检测作用于几何体（geom），几何体是刚性连接到底层刚体的几何实体。碰撞检测的输出是活动接触列表，定义为接触距离小于其裕度参数的接触。它们存储在全局数组 mjData.contact 中，然后用于构造约束雅可比矩阵和计算约束力。下面我们将解释如何选择几何体对进行碰撞检测，如何进行碰撞检测，以及如何确定生成接触的参数。

选择

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

生成

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

刚体对是通过基于改进的扫掠剪枝算法的宽相碰撞检测生成的。改进之处在于，排序轴选择为所有几何体中心协方差矩阵的主特征向量——这最大化了分散度。然后，对于每个刚体对，使用轴对齐边界框（AABB）的静态边界体积层次结构（BVH 二叉树）执行中相碰撞检测。每个刚体都配备了一个其几何体的 AABB 树，分别与所有内部或叶子节点的刚体惯性或几何体框架对齐。

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

过滤

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

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

2. 应用边界球测试，考虑接触裕度。如果一对几何体中的一个是一个平面，则这变为平面-球体测试。

3. 两个几何体不能属于同一个刚体。此外，它们不能属于父子刚体，除非父刚体是世界刚体。这样做的目的是避免刚体和关节内的永久接触。注意，如果几个刚体以它们之间没有关节的方式焊接在一起，则在此测试中它们被视为单个刚体。父过滤器测试可以由用户禁用，而同体测试不能禁用。

4. 两个几何体必须在以下意义上“兼容”。每个几何体都有整数参数 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”）是基于 Gilbert-Johnson-Keerthi 和 Expanding Polytope 算法（GJK / EPA）在 MuJoCo 中原生实现的。原生流水线比基于 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 的原生流水线（底部）在所有距离处都能计算出正确的值。

凸分解

为了对除高度场以外的非凸对象进行建模，用户必须将其分解为凸形几何体（可以是基本形状或网格）的并集，并将它们附加到同一个刚体上。高度场本质上是一种自动分解为棱柱体的形状

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

此规则的例外情况是 SDF 插件（参见其中的文档），它们在某些情况下可能很高效，但有其他要求和限制。

模拟流水线

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

正向动力学

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

顶层

• 顶层函数 mj_step 调用下面的整个计算序列。

• mj_forward 仅调用阶段 2-22，计算连续时间正向动力学，最终得到加速度 mjData.qacc。

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

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

阶段

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

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

位置

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

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

3. 在以相应运动学子树质心为中心的全局框架中计算刚体惯量和关节轴：mj_comPos

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

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

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

7. 计算关节空间惯量矩阵的稀疏因式分解：mj_factorM

8. 构造活动接触列表。这包括宽相和近相碰撞检测：mj_collision

9. 构造约束雅可比矩阵并计算约束残差：mj_makeConstraint

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

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

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

速度

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

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

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

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

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

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

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

力/加速度

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

19. 计算执行器力和激活动力学（如果已定义）：mj_fwdActuation

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

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

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

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

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

25. 使用选定的积分器将模拟状态向前推进一个时间步。注意，龙格库塔积分器会重复上述序列三次，但可选计算除外，它们只执行一次：mj_Euler, mj_RungeKutta, mj_implicit 中的一个。

mjData 中的一致性

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

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

• 在 mj_step 之后（它在更新状态后立即终止），mjData 中的量对应于先前的状态（或者更确切地说，是先前状态与当前状态之间的转换）。特别是，所有与位置相关的传感器值和与位置相关的计算，例如运动学雅可比矩阵，都将是相对于先前的位置的。

可重现性

MuJoCo 的模拟流水线是完全确定且可重现的——如果在轨迹中保存并重新加载状态并再次调用 mj_step，产生的下一个状态将完全相同。然而，有一些重要的注意事项

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

• 状态之间的任何数值差异，无论多么小，在积分后都会变得显著，特别是对于有接触的系统。接触事件具有高的李雅普诺夫指数；这是任何刚体模拟器（以及现实世界物理）的特性，并非 MuJoCo 特有。

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

逆向动力学

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

1. 计算正向运动学。

2. 计算刚体惯量和关节轴。

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

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

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

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

7. 构造活动接触列表。

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

9. 计算仅依赖于位置的传感器数据，如果启用，则计算势能。

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

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

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

13. 计算所有被动力。

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

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

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

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

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

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

导数

原则上，MuJoCo 的整个计算流程，包括其约束求解器，都可以进行解析求导。编写这些导数的高效实现是开发团队的长期目标。已经计算出了光滑动力学（不包括约束）对速度的解析导数，这使得两个 隐式积分器 成为可能。

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

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

mjd_transitionFD:

计算离散时间正向动力学 (mj_step) 的状态转移和控制转移雅可比矩阵。参见 文档。

mjd_inverseFD:

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

这些导数通过利用 MuJoCo 可配置的计算流程来提高效率，这样在不需要时就不会重新计算量。例如，在对控制量求差分时，只依赖于位置和速度的量不会被重新计算。此外，求解器热启动、四元数和控制钳位都得到了正确处理。支持前向差分和中心差分。

参考文献

这里提供一份简要的参考文献列表，并将其与正文关联起来。

计算机器人运动学和动力学的递归算法历史悠久。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.

我们讨论过但在此并未实际使用的接触建模中的线性互补 (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 通过指出推广高斯原理的可能性，重新引起了人们对其的兴趣。

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.