MuJoCo 入门教程（二）

本文主要是介绍MuJoCo 入门教程（二），希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！

系列文章目录

前言

一、简介

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

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

二、柔性接触模型的动机

机器人和人类主要通过物理接触与环境互动。鉴于物理建模在机器人学、机器学习、动画、虚拟现实、生物力学和其他领域的重要性与日俱增，因此需要既能保证物理准确性又能提高计算效率的接触动力学仿真模型。仿真模型的一个应用是在物理系统上部署之前评估候选的估计和控制策略。另一种应用是自动设计这些策略 —— 通常是通过在内环中使用仿真的数值优化。后一种应用提出了一个额外的约束条件：针对接触动力学定义的目标函数应适合数值优化。作为 MuJoCo 基础的接触模型在这些方面和其他相关方面都有优势。在下面的章节中，我们将讨论它的优点，同时阐明它与线性互补（linear complementarity，LCP）接触模型系列的区别，后者是事实上的标准。

2.1 物理真实性和柔性接触

我们的接触模型的许多优点可以追溯到这样一个事实，即我们放弃了线性互补模型（LCP formulation）核心的严格互补性约束（strict complementarity constraint）。我们将把这一系列模型称为凸模型（models convex）；相关工作见参考文献。对于无摩擦接触，放弃明确的互补性约束没有任何区别，因为所得到的凸二次规划的卡鲁什-库恩-塔克（KKT）最优条件等同于 LCP。但对于摩擦接触来说，情况就不同了。

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

当然，并不是每一种柔性模型都是理想的；例如，弹簧阻尼模型是软的，但存在不稳定性。同时，不同的材料有不同的轮廓，因此与硬接触模型不同，柔性模型必须有足够丰富的参数化，才能适用于多个相关系统。这反过来又有助于系统识别接触模型参数。

2.2 计算效率

具有摩擦接触的 LCP 模型对应于 NP-hard 优化问题。这催生了近似求解器行业，但不幸的是，许多流行的物理引擎使用的捷径记录不全，由此产生的运动方程难以表征。公平地说，NP-hardness 是关于最坏情况下性能的声明，并不意味着在实践中不可能快速求解 LCP。尽管如此，凸优化仍具有公认的优势。在 MuJoCo 中，我们观察到，对于典型的机器人模型，使用投影高斯-赛德尔法（PGS）进行 10 次扫频，就能得到实际意义上与全局最小值无异的解决方案。当然，有些问题即使是凸问题，数值求解也要困难得多，对于这类问题，我们有更高阶收敛的共轭梯度求解器。

不同的使用情况对计算效率的要求也不同。如果我们需要的只是实时仿真，那么即使使用低效求解器，现代计算机的速度也足以处理大多数相关的机器人系统。然而，在优化方面，并不存在足够快的仿真。如果目标函数及其导数的计算速度可以更快，那么搜索范围或训练集或样本量就会更大，这反过来又会增加搜索范围或训练集或样本量，这反过来又会增加搜索范围或训练集或样本量，这反过来又会增加搜索范围或训练集或样本量。

2.3 连续时间

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

过去，摩擦接触的凸模型也依赖于离散时间近似，但这并非必要。本模型采用连续时间定义，以力和加速度为单位。考虑到现实世界中的时间是连续的，这种方法更为自然。这也是控制文献中的首选表述方式，事实上，我们希望 MuJoCo 能够吸引该领域的用户。连续时间公式的另一个优势是，它们可以进行复杂的数值积分，而无需支付离散时间变分积分器的计算开销（当惯性与配置相关时，离散时间变分积分器必然是隐式的）。连续时间动力学还具有良好的时间后向定义，而这正是某些优化算法所需要的。

2.4 逆动力学和优化

逆动力学的目标是根据多关节系统的位置、速度和加速度恢复外加力和接触力。对于硬接触，这种计算是不可能的。考虑一下推墙不动的情况。接触力无法从运动学中恢复，当然，除非我们考虑材料变形 —— 在这种情况下，我们需要一个柔性接触模型。使用弹簧阻尼接触模型计算逆动力学是非常简单的，因为在这种情况下，接触力只是位置和速度的函数，并不取决于外力。但这也是弹簧阻尼模型不可取的原因：忽略外加力意味着每个时间步都会引入误差，因此仿真器永远处于纠错模式，进而导致不稳定性。相比之下，现代接触求解器在计算接触力/冲量时会考虑外力（以及所有内力）。但这使得反演变得复杂。本接触模型具有唯一定义的反演。事实上，反向动力学比正向动力学更容易计算，因为优化问题变成了对角线问题，并分解成单个接触的独立优化问题 —— 这些问题可以通过分析求解。

反向动力学在系统识别、估计和控制中出现的优化算法中起着关键作用。反动力学算法可以将位置序列（或其参数表示）视为优化对象。然后，通过对位置进行微分计算出速度和加速度；使用逆动力学计算外加力和接触力；最后构建一个目标函数，该函数可取决于上述所有因素。这就是所谓的时空优化法、频谱法和直接定位法。MuJoCo 独一无二地适用于在存在接触和其他约束条件的情况下促进此类计算。

三、总体框架

下表概括了我们的符号。稍后将介绍与约束条件相关的其他符号。如果有的话，我们还会显示与数学符号相对应的主要数据结构 mjModel 和 mjData 的字段。

Symbol	Size	Description	MuJoCo field
$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 *n_V$	关节空间惯性矩阵	`mjData.qM`
$J(q)$	$n(C)*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 个，4 个铰链关节各 1 个，自由浮动物体 6 个。它们以相同的顺序出现在维数为 $n_V$ 的所有系统级向量和矩阵中。与给定模型元素相对应的数据可通过索引操作恢复，详见概述一章中的 "说明 "部分。维数为 $n_C$ 的向量和矩阵有些不同，因为运行时活动约束会发生变化。在这种情况下，仍然有一个固定的枚举顺序（与模型元素在 mjModel 中出现的顺序相对应），但任何非活动约束都会被省略。

只要使用四元数来表示三维方向，位置坐标 $n_Q$ 的数量就会大于自由度 $n_V$ 的数量。当模型包含球关节或自由关节时，就会出现这种情况（即在大多数模型中）。在这种情况下， $\dot{q}$ 不等于 $v$ ，至少不是通常意义上的。相反，我们必须考虑刚体方向 $SO(3)$ 组--它在四维空间中具有单位球体的几何形状。速度存在于这个球体的三维切线空间中。所有内部计算都会考虑到这一点。对于自定义计算，MuJoCo 提供了函数 mj_differentiatePos，它可以 "减去 "两个维数为 $n_Q$ 的位置向量，并返回一个维数为 $n_V$ 的速度向量。此外，还提供了许多与四元数相关的实用功能。

MuJoCo 可以连续计算正向和反向动力学。然后在指定的 mjModel.opt.timestep 上用所选的数值积分器对正向动力学进行积分。连续时间的一般运动方程为

$M\dot{v}+c=\tau+J^Tf$

雅各比建立了关节坐标和约束坐标中的量之间的关系。它将运动向量（速度和加速度）从关节坐标系映射到约束坐标系：关节速度 $v$ 映射到约束坐标系中的速度 $J_v$ 。雅各布的转置将力向量从约束坐标系映射到关节坐标系：约束力 $f$ 映射到关节坐标中的力 $J^{T}f$ 。

关节空间惯性矩阵 $M$ 总是可逆的。因此，一旦已知约束力 $f$ ，我们就可以将正向和反向动力学计算结果确定为 :

$\begin{aligned}\text{forward:}\quad\dot{v}&=M^{-1}(\tau+J^Tf-c)\\\text{inverse:}\quad\tau&=M\dot{v}+c-J^Tf\end{aligned}$

约束力的计算是最难的部分，将在后面介绍。但首先，我们将通过总结如何计算上述数值直至约束雅各布，来完成对总体框架的描述。

外加力 $\tau$ 包括来自弹簧减震器和流体动力学的被动力，驱动力以及用户指定的附加力。
偏向力 $c$ 包括科里奥利力、离心力和重力。它们的总和使用牛顿-欧拉递归算法（RNE）计算，加速度设为 0。
关节空间惯性矩阵 $M$ 采用复合刚体（CRB）算法计算。该矩阵通常比较稀疏，因此我们采用专门为运动学树定制的格式来表示它。
由于我们经常需要将向量与 $M$ 的逆矩阵相乘，因此我们在计算其 $L^{T}DL$ 因式分解时会保留稀疏性。当以后需要 $M^{-1}x$ 形式的量时，我们会通过稀疏反代换来计算它。

在进行这些计算之前，我们会应用前向运动学，计算所有空间物体的全局位置和方向以及关节轴。虽然通常建议在局部坐标中应用 RNE 和 CRB，但在这里我们要为碰撞检测做好准备，而碰撞检测是在全局坐标中进行的，因此 RNE 和 CRB 也是在全局坐标中实现的。尽管如此，为了提高浮点精度，我们还是以子树质量中心为中心（mjData 中以 c 开头的字），在全局坐标系中表示每个运动学子树的数据。本章末尾给出了仿真流水线的详细总结。

3.1 执行器模型

MuJoCo 提供灵活的执行器模型。所有执行器都是单输入-单输出（SISO）。执行器 $i$ 的输入是用户指定的标量控制 $u_i$ 。输出为标量力 $p_i$ ，该力通过由传动装置确定的力矩臂矢量映射到关节坐标。执行器还可以有自己的动态激活状态 $w_i$ 。所有执行器的控制输入存储在 mjData.ctrl，力输出存储在 mjData.actuator_force，激活状态（如有）存储在 mjData.act。

执行器的这三个组成部分 —— 传动、激活动态和力的产生 —— 决定了执行器的工作方式。用户可以对它们进行独立设置以获得最大的灵活性，也可以使用执行器快捷方式将常用的致动器类型实例化。

3.1.1 传动

每个执行器都有一个标量长度 $l_i(q)$ ，由传动类型及其参数定义。梯度 $\nabla l_{i}$ 是力矩臂的 $n_V$ 维向量。它决定了从标量推杆力到关节力的映射。传动属性由执行器所连接的 MuJoCo 对象决定；可能的连接对象类型包括关节、肌腱、关节母体、滑块-曲柄、部位和主体（ joint, tendon, jointinparent, slider-crank, site, and body）。

3.1.1.1 关节和肌腱 joint and tendon

关节和腱的传动类型与预期的一样，对应于执行器对目标对象施加力或力矩。球形关节比较特殊，详情请查看致动器/常规/关节文档。

3.1.1.2 父节点 jointinparent

jointinparent 传输是球关节和自由关节所独有的，表示旋转应在父系而非子系坐标系中测量。

3.1.1.3 曲柄滑块 slider-crank

滑块曲柄传动将线性力转换为扭矩，如活塞驱动的内燃机。该模型包含教学实例。滑块曲柄也可以通过创建 MuJoCo 体并将其与相等约束耦合来显式建模，但这样做的效率和稳定性都较差。

3.1.1.4 体 body

体传递对应于在属于一个体的接触点上施加力，以模拟真空抓取器和生物力学粘附器。有关粘附的更多信息，请参阅粘附致动器文档。这些传递目标的固定零长度 (q)=0。

3.1.1.5 站点

站点传动对应于在站点的坐标系中施加卡氏力/力矩。当没有定义参考点时（见下文），这些目标具有固定的零长度 (q)=0，适用于喷射器和推进器建模：力和力矩固定在站点坐标系上。

如果使用可选的 refsite 属性定义了站点传动，那么力和力矩将作用在参照站点的坐标系中，而不是站点自身的坐标系中。如果定义了参照站点，那么执行器的长度就不会为零，而是对应于两个站点的姿态差，并投影到参照系中选定的方向上。该长度可以通过位置执行器进行控制，从而实现笛卡尔末端执行器控制。更多详情，请参阅 refsite 文档。

3.1.2 有状态的执行器

一些执行器，如气动和液压缸以及生物肌肉，都有一种称为 "激活 "的内部状态。这是一种超越关节位置 $q$ 和速度 $v$ 的真正动态状态。将这类致动器纳入模型会产生三阶动力学。我们将致动器激活矢量 $w$ 表示。

$\dot{w_i}\left(u_i,w_i,l_i,\dot{l_i}\right)$

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

$\begin{aligned}\text{integrator :}\quad&\dot{w}_i=u_i\\\text{filter :}\quad&\dot{w}_i=(u_i-w_i)/\mathbf{t}\\\text{filterexact :}\quad&\dot{w}_i=(u_i-w_i)/\mathbf{t}\\\text{muscle :}\quad&\dot{w}_i=\text{muscle}(u_i,w_i,l_i,\dot{l}_i)\end{aligned}$

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

有关肌肉激活动态的更多信息，请参见肌肉。

对于 filterexact 激活动力学， $\dot{w}$ 的欧拉积分由解析积分取代：

$\begin{aligned}\mathrm{filter}:\quad&w_{i+1}=w_i+h(u_i-w_i)/\mathbf{t}\\\mathrm{filterexact}:\quad&w_{i+1}=w_i+(u_i-w_i)(1-e^{-h/\mathbf{t}})\end{aligned}$

这两个表达式在 $h\to0$ 的极限收敛到相同的值。请注意，欧拉积分滤波器在 $\mathtt{t}<h$ 时会发散，而精确积分滤波器在任何正 $\text{t}$ 时都是稳定的：

actearly
如果 actearly 属性设置为 "true"，mjData.actuator_force 将根据 $w_{i+1}$ （下一次激活）进行计算，从而将 $u$ 的变化与其对加速度的影响之间的延迟减少一个时间步长（因此总的动力学是二阶而不是三阶）。

3.1.3 力的产生

每个执行器产生一个标量力 $p_{i}$ ，它是某个函数

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

与激活动力学类似，力的产生机制也是特定于执行器的，不能与模型中的其他执行器相互作用。目前，在激活状态时，力是仿射的，反之则是控制的：

$p_i=(aw_i\mathrm{~or~}au_i)+b_0+b_1l_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)\left.p_i\left(u_i,w_i,l_i(q),\dot{l_i}(q,v)\right)\right.$

这个量存储在 mjData.qfrc_actuator 中。它与用户自定义的关节坐标或笛卡尔坐标力（分别存储在 mjData.qfrc_applied 和 mjData.xfrc_applied）一起被添加到作用力矢量 $\tau$ 中。

3.2 被动力

被动力是指只取决于位置和速度的力，而不取决于正向动力学中的控制力或逆向动力学中的加速度。因此，这些力既是正向动力学计算的输入，也是反向动力学计算的输入，并且在两种情况下都是相同的。它们存储在 mjData.qfrc_passive 中。然而，用户可以安装回调 mjcb_passive，并在 mjData.qfrc_passive 中添加可能增加能量的力。只要这些用户力仅取决于位置和速度，就不会干扰 MuJoCo 的运行。

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

关节和肌腱中的弹簧阻尼器。详见以下属性。关节：刚度、弹簧反力、阻尼、弹簧阻尼器。肌腱：刚度、弹簧长度、阻尼。
重力补偿力。详情请查看身体重力补偿属性。
周围介质施加的流体力。详见流体力章节。

3.3 数值积分

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

有四种数字积分器可供选择：三种单步积分器和多步四阶 Runge-Kutta 积分器。在介绍积分器之前，我们先对单步欧拉积分器进行一般描述：显式、半隐式和速度隐式。MuJoCo 不支持显式欧拉方法，但该方法具有教学价值。它可以写成

$\begin{aligned}\text{activation:}\quad&w_{t+h}=w_t+h\dot{w}_t\\\text{velocity:}\quad&v_{t+h}=v_t+ha_t\\\text{position:}\quad&q_{t+h}=q_t+hv_t\end{aligned}$

需要注意的是，在存在四元数的情况下，由于 $q$ 和 $v$ 的维数不同， $q_t+hv_t$ 的运算比简单求和要复杂得多。之所以不采用显式欧拉公式，是因为下面的半隐式欧拉公式严格来说更好，是物理仿真的标准公式：

$\begin{aligned}&v_{t+h}=v_t+ha_t\\&q_{t+h}=q_t+hv_{\color{red}{t+h}}\end{aligned}$

比较 (2) 和 (3)，我们可以发现，在半隐式欧拉方程中，位置是用新的速度更新的。隐式欧拉是指

$\begin{aligned}v_{t+h}&=v_t+ha_{\color{red}{t+h}}\\q_{t+h}&=q_t+hv_{t+h}\end{aligned}$

比较 (3) 和 (4)，我们可以看到，速度更新右侧的加速度 $a_{t+h}=\dot{v}_{t+h}$ 是在下一个时间步进行评估的。虽然不通过步进计算下一个加速度是不可能的，但我们可以使用一阶泰勒展开来逼近这个量，并采用牛顿法的单步计算。当扩展只涉及速度（而非位置）时，积分器被称为隐含速度欧拉积分器。这种方法对于速度函数 $a_t=a(v_t)$ 的系统特别有效，我们要近似的速度更新为

$v_{t+h}=v_t+ha(v_{t+h})$

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

$a(v)=M^{-1}(\tau(v)-c(v)+J^Tf(v))$

因此，我们定义导数

$\begin{aligned} \frac{\partial a(v)}{\partial v}& =M^{-1}D \\ \text{D}& \equiv\frac\partial{\partial v}(\tau(v)-c(v)+J^Tf(v)) \end{aligned}$

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

$\begin{aligned} v_{t+h}& =v_t+ha(v_{t+h}) \\ &\approx v_t+h(a(v_t)+\frac{\partial a(v)}{\partial v}\cdot(v_{t+h}-v_t)) \\ &=v_t+ha(v_t)+hM^{-1}D\cdot(v_{t+h}-v_t) \end{aligned}$

先乘以 M 再重新排列得到

$(M-hD)v_{t+h}=(M-hD)v_t+hMa(v_t)$

求解 $v_{t+h}$ ，我们就得到了隐式速度更新

$v_{t+h}=v_t+h(M-hD)^{-1}Ma(v_t)$

MuJoCo 中的所有三个单步积分器都使用更新 (5)，但对 $D$ 矩阵的定义不同， $D$ 矩阵总是通过分析计算得出。

半隐式隐式关节阻尼（欧拉 Euler）
对于这种方法， $D$ 只包括联合阻尼的导数。需要注意的是，在这种情况下 $D$ 是对角线， $M-hD$ 是对称的，因此可以使用 Cholesky 分解法。如果模型没有关节阻尼或设置了 eulerdamp 禁用标志，则会禁用隐式阻尼，并使用半隐式更新方法 (3)，而不是 (5)。

速度内隐（隐式 implicit）
对于这种方法， $D$ 包括除约束力 $J^Tf(v)$ 以外的所有力的导数。这些导数目前被忽略，因为即使计算它们是可能的，但也很复杂，而且数值测试表明，包含这些导数并没有什么好处。尽管如此，我们计划在未来版本中加入约束力的分析导数。此外，为了提高计算效率，我们限制 $D$ 具有与 $M$ 相同的稀疏性模式。这一限制将排除连接运动学树不同分支上的机构的筋的阻尼。由于 $D$ 不是对称的，所以我们不能使用 Cholesky 因式分解，但是由于 $D$ 和 $M$ 具有与运动树拓扑结构相对应的相同稀疏性模式，所以可以保证对 $M-hD$ 进行反阶 LU 因式分解时没有填充。这个因式分解结果存储在 mjData.qLU 中。

快速速度内隐法（内隐快速法）
对于这种方法， $D$ 包括隐式方法中使用的所有力的导数，但 RNE 算法计算的向心力和科里奥利力 $c(v)$ 除外。此外，它是对称的 $\begin{aligned}D\leftarrow(D+D^T)/2\end{aligned}$ 。放弃 RNE 导数的原因之一是它们的计算成本最高。其次，这些力只有在复杂垂体和旋转体的高旋转速度时才会迅速变化，这种情况并不常见，而且 Runge-Kutta 积分器已经可以很好地处理（见下文）。由于 RNE 导数也是造成 $D$ 不对称的主要原因，因此通过舍弃 RNE 导数并将其对称化，我们可以使用更快的 Cholesky 而不是 LU 分解。

四阶 Runge-Kutta (RK4)
连续时间公式的一个优势是，我们可以使用高阶积分器，如 Runge-Kutta 或多步法。尽管用户可以通过调用 mj_forward，自行对加速度进行积分，轻松实现其他积分器，但目前唯一实现的此类积分器是固定步长的四阶 Runge-Kutta 方法。我们观察到，对于能量守恒系统（示例），RK4 在稳定性和准确性方面都优于单步方法，即使时间步数减少 4 倍（因此计算量相同）也是如此。在存在与速度相关的巨大力的情况下，如果所选的单步法对这些力进行了隐式积分，那么单步法的稳定性会明显高于 RK4。

选择时间步长和积分器

时间步长

通过减小时间步长 h，可以提高所有积分器的精度和稳定性。当然，较小的时间步长也会减慢仿真速度。时间步长可能是用户可以调整的最重要参数。如果时间步长过大，仿真将变得不稳定。如果时间步长过小，CPU 时间将被浪费，而精度却不会得到明显改善。时间步长总有一个 "恰到好处 "的舒适范围，但这个范围取决于模型。

积分器
小结：推荐使用 implicitfast 积分器，它通常是稳定性和性能的最佳平衡点。

欧拉：使用 Euler 与旧型号和 MJX 兼容。特别是对于 MJX，设置 eulerdamp 禁用标志可以提高性能。
隐式快速积分器：implicitfast 积分器的计算成本与欧拉相似，但稳定性更高，因此是一种严格的改进。它是大多数模型的推荐积分器。
隐式：与 implicitfast 相比，其优势在于隐式积分科里奥利力和向心力，包括陀螺力。gyroscopic.xml 显示了一个在斜面上滚动的椭球体，使用 implicitfast 会迅速发散，而使用 implicit 则很稳定。
RK4：pendulum.xml显示的是一个复杂的钟摆机构，使用欧拉或隐式快速运算会迅速发散，但使用 RK4 运算却能很好地保存能量。请注意，在隐式条件下，该模型不会发散，而是会损失能量。

3.4 状态

为了完成对总体框架的描述，我们现在将讨论状态的概念。MuJoCo 有一个紧凑、定义明确的内部状态，它与确定性计算管道一起，意味着重置状态和计算动态导数等操作也是定义明确的。

状态完全封装在 mjData 结构中，由多个组件组成。这些组件在 mjtState 中以比特标志的形式枚举，同时还有几种常见的组合，与下面的分组相对应。可以分别使用 mj_getState 和 mj_setState 方便地从 mjData 中读取和向 mjData 中写入串联状态向量。

3.4.1 物理状态

物理状态（mjSTATE_PHYSICS）包含步进过程中时间积分的主要量。它们是 mjData.{qpos、qvel、act}：

位置：qpos
        广义坐标系中的配置，用 q 表示。

速度：qvel
        广义速度，用 v 表示。

执行器激活：act
        mjData.act 包含有状态执行器的内部状态，用 w 表示。

3.4.2 完整物理状态

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

时间： time
仿真时间由标量 mjData.time 给出。由于物理状态是时间不变的，因此不包括在物理状态中；例外情况包括依赖时间的用户回调和插件（例如开环控制器），在这种情况下应包括时间。

插件状态：plugin_state
mjData.plugin_state 是引擎插件声明的状态。详情请参阅插件状态部分。

3.4.3 用户输入

这些输入字段（mjSTATE_USER）由用户设置并影响物理仿真，但仿真器不会触及。除 MoCap poses 外，所有输入字段的默认值均为 0。

控制：ctrl
mjData.ctrl 的值要么直接产生广义力（无状态执行器），要么影响 mjData.act 中的执行器激活，然后产生力。

辅助控制：qfrc_applied 和 xfrc_applied
        mjData.qfrc_applied 是直接施加的广义力。
        mjData.xfrc_applied 是施加到单个体的 CoM 上的笛卡尔扳手。例如，本机查看器会使用这个字段来应用鼠标扰动。
        请注意，qfrc_applied 和 xfrc_applied 的效果通常可以通过适当的执行器定义来重现。

MoCap 姿态：mocap_pos 和 mocap_quat
mjData.mocap_pos 和 mjData.mocap_quat 是这里描述的特殊可选运动学状态，允许用户实时设置静态物体的位置和方向，例如在从动作捕捉设备流式传输 6D poses 时。mj_resetData 设置的默认值为默认配置下的体姿。

等价约束切换：eq_active
mjData.eq_active 是一个字节值数组，允许用户在运行时切换平等约束的状态。该数组的初始值为 mjModel.eq_active0，可使用 XML 中的等价约束 active 属性进行设置。

用户数据：userdata
mjData.userdata 是用户定义的内存空间，引擎不会触及。例如，它可以被回调使用。编程一章对此有更详细的介绍。

3.4.4 热启动加速

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

不同的暖启动对动力学没有明显影响，但如果加载非初始状态时需要完美的数值重现性，则应保存暖启动。需要注意的是，尽管暖启动对物理的影响可以忽略不计，但许多物理系统在时间步进时会以指数方式累积微小的差异，从而迅速导致不同暖启动的轨迹出现分歧。

3.4.5 积分状态

积分状态（mjSTATE_INTEGRATION）是上述所有 mjData 字段的集合，构成正向动力学的整个输入集。在逆向动力学中，mjData.qacc 也被视为输入变量。所有其他 mjData 字段都是积分状态的函数。

需要注意的是，mjSTATE_INTEGRATION 所给出的完整积分状态是最大化的，包含了一些通常未使用的字段。如果希望状态大小较小，最好避免保存未使用的字段。特别是 xfrc_applied（应用）字段可能相当大（6 x nbody），但经常未被使用。

3.4.6 仿真状态：mjData

仿真状态是 mjData 结构和相关内存缓冲区的整体。该状态包括动力学计算过程中计算出的所有派生量。由于 mjData 缓冲区是为最坏情况预先分配的，因此从积分状态重新计算派生量往往比使用 mj_copyData 要快得多。

四、约束模型

MuJoCo 有一个非常灵活的约束模型，但稍后描述的求解器会以统一的方式处理该模型。在此，我们将从概念上解释各个约束条件，以及它们在系统级向量和矩阵 $n_C$ 中的布局。每个概念约束条件都可以为总计数 $n_C$ 贡献一个或多个标量约束条件，每个标量约束条件在约束条件雅各布 $J$ 中都有相应的行。主动约束条件按以下描述的类型顺序排列，然后在每个类型中按模型元素排列。类型包括：相等、摩擦损失、极限、接触（equality, friction loss, limit, contact）。限值在求解器中作为无摩擦接触处理，内部不作为单独类型处理。我们在 mjData 中使用前缀 efc 来表示包含约束相关数据的系统级向量和矩阵。

4.1 等式

MuJoCo 可以以 $r(q)=0$ 的一般形式模拟等式约束，其中 $r$ 可以是位置矢量 $q$ 的任何可变标量或矢量函数。求解器实际上也可以处理非符合人体工程学的约束，但我们尚未定义此类约束类型。每个相等约束都会为总约束数 $n_C$ 带来 $dim(r)$ 元素。 $J$ 中对应的块就是残差的雅各布因子，即 $\partial r/\partial q$ 。请注意，由于四元数的特性，与 $q$ 有关的微分会产生大小为 $n_V$ 而非 $n_Q$ 的向量。

在其他应用中，等式约束可用于创建 "循环关节"，即无法通过运动树建模的关节。游戏引擎就是以这种方式表示所有关节的。在 MuJoCo 中也可以这样做，但不建议这样做 —— 因为这会导致仿真速度更慢、精度更低，实际上把 MuJoCo 变成了一个游戏引擎。用相等约束来表示关节的唯一原因是对柔性关节进行建模，这可以通过约束求解器来实现，但不能通过运动学树来实现。

接下来将介绍五种类型的相等约束。标题中的数字与每种情况下约束残差的维度相对应。

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

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

关节（joint）:1
该约束仅适用于标量关节：铰链和滑块。它可用于将一个关节锁定在恒定位置，或通过四次多项式将两个关节耦合。锁定一个关节最好是通过移除该关节来实现，但它在特殊情况下也很有用，例如反向间隙建模（通过柔性相等约束）。两个关节的耦合可用于模拟螺旋关节或其他形式的机械耦合。四次多项式模型的定义如下。假设 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$ 。

肌腱（tendon）：1
该约束与上述关节约束非常相似，但适用于肌腱的长度而非关节的位置。肌腱是取决于位置矢量的长度量。这种依赖性可以是标量关节位置的线性组合，也可以是缠绕空间障碍物的最小长度字符串。在模型配置 mjModel.qpos0 中，关节的位置可以直接从位置矢量中读取，但肌腱长度的计算却不那么简单。这就是为什么所有肌腱的 "静止长度 "都是由编译器计算并存储在 mjModel 中的原因。一般来说，mjModel 中所有名称以 0 结尾的字段都是编译器在初始模型配置 mjModel.qpos0 中计算的量。

距离（distance）：1
注意

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

4.2 摩擦损失

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

摩擦损失与所有其他约束类型不同，因为没有位置残差可与之关联；因此我们正式将 r(q) 的相应分量设为零。事实上，我们将在后面看到，我们的约束求解公式需要以一种不同寻常的方式进行扩展，以纳入这种约束。尽管如此，受影响关节或肌腱的速度仍可作为速度 "残差" —— 因为约束的作用是降低该速度，并在理想情况下将其保持为零。因此，约束雅各布中的相应块就是关节位置（或肌腱长度）相对于 $q$ 的雅各布。对于肌腱来说，这就是力矩臂向量。

关节（joint）:1、3 或 6
不仅可以为标量关节（滑动和铰链）定义摩擦损失，也可以为具有 3 个自由度的球形关节和具有 6 个自由度的自由关节定义摩擦损失。定义后，它将独立应用于受影响关节的所有自由度。摩擦损失参数具有与关节（线性或角度）相适应的隐式单位。自由关节是个例外，因为它们既有线性分量也有角度分量，而 MJCF 模型格式允许每个关节使用一个摩擦损耗参数。在这种情况下，线性分量和角度分量使用相同的参数。可以说，自由连接中的摩擦损失是不应该被允许的。不过，我们允许使用摩擦损失，因为它可以模拟有用的非物理效应，例如在物体受到足够的推力之前保持不动。

肌腱（tendon）：1
肌腱是标量，因此为肌腱定义摩擦损耗总会增加一个标量约束。对于空间肌腱，这可用于模拟肌腱与其包裹表面之间的摩擦。不过，这种摩擦与载荷无关。要构建更详细的模型，可以创建多个小浮球，并将它们与腱串联起来。这样，球体与周围表面之间的接触就会产生与载荷有关的摩擦力（即库仑摩擦力），但这种方法的仿真效率较低。

4.3 限制 Limit

限制和接触都有明确的空间残差，但与相等约束不同的是，它们是单侧的，也就是说，它们引入的是不相等约束而不是相等约束。可以为关节和肌腱定义限制。方法是将相应的模型元素标记为 "限制 "并定义其 "范围 "参数。残差 $r(q)$ 是当前位置/长度与范围中指定的两个极限值中较近值之间的距离。该距离的符号会自动调整，如果尚未达到极限，则为正；如果达到极限，则为零；如果违反了极限，则为负。当该距离低于 "余量 "参数时，约束将被激活。然而，这并不等同于通过余量抵消极限值并将余量设为 0，而是通过稍后描述的求解器参数使约束力取决于距离。

给定关节或肌腱的下限和上限都有可能处于激活状态。在这种情况下，它们都会包含在标量约束列表中，但应通过增加范围或减少余量来避免这种情况。尤其要避免使用窄范围来近似相等约束。取而代之的是使用明确的相等约束，如果需要一些松弛，可以通过调整求解器参数使约束变得柔性。这种方法计算效率更高，不仅因为它只涉及一个标量约束而不是两个，还因为求解相等约束力的速度通常更快。

关节（joint）：1 或 2
可以为标量接头（铰链和滑块）以及球形接头定义限值。标量关节的处理方法如上所述。球形关节的限值应用于关节四元数的指数图或角轴表示，即矢量 (θx,θy,θz)，其中 θ 是旋转角度，(x,y,z) 是对应于旋转轴的单位矢量。限值适用于旋转角度 θ 的绝对值。运行时，限值由两个范围参数中较大的一个决定。但是为了语义清晰，应该使用第二个范围参数指定限值，并将第一个范围参数设置为 0。

肌腱（tendon）：1 或 2
tendon 是标量，其限制如上所述。请注意，固定筋（标量关节位置的线性组合）的 "长度 "既可以是正值，也可以是负值，因为关节位置是相对于关节参考点定义的，既可以是正值，也可以是负值。而空间筋的真实长度不能为负。在为肌腱限制设置范围和边距时，请牢记这一点。

4.4 接触

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

MuJoCo 使用点接触，其几何定义是两个几何体之间的一个点和以该点为中心的空间坐标系，两者都用全局坐标表示。该坐标系的第一（x）轴是接触的法线方向，其余（y 和 z）轴定义了切平面。按照 MuJoCo 的可视化惯例，人们可能希望法线与 Z 轴相对应，但我们支持无摩擦接触，只使用法线轴，这也是我们希望法线位于第一位置的原因。与极限类似，当两个几何体分开时，接触距离为正；当它们接触时，接触距离为零；当它们穿透时，接触距离为负。接触点位于两个表面沿法线轴线的中间位置（对于网格碰撞，这可能是近似值）。碰撞检测是一个单独的话题，将在下文中详细讨论。目前我们只需要碰撞检测给出接触点、空间坐标系和法向距离。

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

Parameter	Description
`condim`	坐标系中接触力/力矩的维数。可以是 1、3、4 或 6。
`friction`	摩擦系数向量，维数为 condim-1。
`margin`	距离边距，用于确定是否将接触纳入全局接触数组 mjData.contact。
`gap`	对于自定义计算，有时可以方便地在 mjData.contact 中包含接触点，但不生成接触力。这就是间隙的作用：只有当法线距离低于（margin - gap）时，才会产生接触力。
`solref` and `solimp`	求解器参数，稍后解释。

接触摩擦锥可以是椭圆锥（elliptic），也可以是金字塔锥（pyramidal）。这是一个全局设置，由选择的约束求解器决定：椭圆求解器使用椭圆锥，而金字塔求解器使用金字塔锥（如后面定义）。condim 参数决定接触类型，其含义如下：

condim = 1：1 表示椭圆形，1 表示金字塔形
这相当于无摩擦接触，只增加了一个标量约束。回顾一下，接触坐标系的第一轴是接触法线。无摩擦接触只能沿着法线产生力。这与关节或肌腱限制非常相似，但适用于两个几何体之间的距离。

condim = 3：3 表示椭圆形，4 表示金字塔形
这是一种规则的摩擦接触，可以产生法向力以及切向摩擦力来对抗滑移。

condim = 4：4 表示椭圆形，6 表示金字塔形
除了法向力和切向力之外，这种接触还能产生扭转摩擦力矩，反对绕接触法线旋转。这对于仿真柔性手指非常有用，可以大大提高仿真抓取的稳定性。请注意，扭转（以及滚动）摩擦系数的单位与切向摩擦系数不同。

condim = 6：6 表示椭圆形，10 表示金字塔形
这种接触可以抵消两个几何体之间所有相对自由度的运动。尤其是它增加了滚动摩擦力，例如可以用来阻止小球在平面上无限滚动。它还可以用来模拟轮胎和路面之间的滚动摩擦力，并在一般情况下稳定接触。

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

现在，我们将更正式地描述摩擦锥和相应的雅各比。仅在本节中，让 $f$ 表示单个接触点的约束力向量（而不是系统级的约束力向量），μ 表示摩擦系数向量，n 表示接触点维度。当 n>1 时，椭圆形和金字塔形摩擦锥的定义为

$\begin{aligned}\text{elliptic cone}:&\quad\mathcal{K}=\left\{f\in\mathbb{R}^n:f_1\geq0,f_1^2\geq\sum_{i=2}^nf_i^2/\mu_{i-1}^2\right\}\\\text{pyramidal cone}:&\quad\mathcal{K}=\left\{f\in\mathbb{R}^{2(n-1)}:f\geq0\right\}\end{aligned}$

金字塔锥定义中的向量不等式是指元素不等式。当 n=1 时，两个锥体都定义为非负射线（锥体的特例）。请注意，下文求解器部分讨论的系统级摩擦锥也将表示为 K，它是此处定义的单个触点摩擦锥的乘积。

我们还需要说明约束力如何作用于系统。基向量是空间向量：三维力和三维扭矩。将基向量排列成矩阵 E 的各列，约束力在接触坐标系中产生的力/力矩即为 $Ef$ 。基向量矩阵的构造如下。

图中展示了 $n=6$ 情况下的全基集。否则，我们只使用前 $n$ 列或 $2(n-1)$ 列，具体取决于锥体类型。椭圆锥比较容易理解。由于矩阵 $E$ 是单位矩阵，因此 $f$ 的前三个分量是沿接触坐标系轴线作用的力，而后三个分量是绕轴线作用的力矩。对于金字塔锥体，基向量对应于金字塔的边缘。每个矢量都包含一个法向力分量和一个摩擦力分量或一个摩擦扭矩分量。摩擦系数的缩放确保了所有基向量都位于我们所近似的椭圆摩擦锥内。对于这些向量的任何凸组合也是如此。

最后，我们说明如何计算接触雅各布。首先，我们构建一个 6 乘 $n_V$ 矩阵 S，将关节速度 v 映射到以接触坐标系表示的空间速度 $S_v$ 。我们采用的惯例是，接触力从第一个几何点作用到第二个几何点，因此第一个几何点的空间雅各布值为负号。与所有其他约束条件一样，该矩阵被插入系统级雅各布矩阵 $J$ 中。

五、约束求解器

本节介绍如何计算约束力。计算分为两个阶段。首先，约束力被定义为凸优化问题的唯一全局解。对于金字塔锥体是二次规划，对于椭圆锥体是圆锥规划。其次，该优化问题可通过下述算法求解。我们还描述了约束模型的参数，以及这些参数如何影响所产生的动力学结果。

优化问题的定义本身有两个步骤。我们从一个定义在加速度 $\dot{\upsilon}$ 上的原始问题开始，其中的约束力是隐含的。然后，我们将加速度原始问题转化为拉格朗日对偶问题。对偶问题是约束力的凸优化问题，约束力也是原始问题的拉格朗日乘数。在正向动力学中，必须对原始问题或对偶问题进行数值求解。在逆向动力学中，问题变成了对角线问题，可以用解析法求解。

主问题的表述基于最小约束高斯原理的一般化。高斯原理的基本形式是，如果我们有无约束动力学 $M\dot{v}=\tau$ 并施加加速度约束 $J\dot{v}=a_\mathrm{ref}$ ，则得到的加速度将是

$\begin{aligned}\dot{v}&=\arg\min_x\left\|x-M^{-1}\tau\right\|_M^2\\&\text{subject to }Jx=a_{\mathrm{ref}}\end{aligned}$

其中加权 $L_2$ 规范为通常的 $\|x\|_M^2=x^TMx$ 。因此，约束会导致与无约束加速度 $M^{-1}\tau$ 的最小偏差，其中用于测量关节坐标偏差的度量由惯性矩阵给出。众所周知，这一原理等同于受约束运动的拉格朗日-达朗贝尔原理。在这里，我们将利用它获得一个丰富而原则性强的软约束模型。为此，我们将对高斯原理中的成本函数和约束条件进行概括。

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

Symbol	Size	Description
�z	��nC	constraint deformations
�ω	��nC	velocity of constraint deformations
�k	��nC	virtual constraint stiffness
�b	��nC	virtual constraint damping
�d	��nC	constraint impedance