仿真#

初始化#

mjModel 和 mjData 绝对不应由用户直接分配。相反，它们由相应的 API 函数分配和初始化。这些是非常复杂的数据结构，包含其他结构的（数组）、用于存储所有中间结果的预分配数据数组，以及一个 内部堆栈。我们的策略是在仿真开始时分配所有必要的堆内存，并在仿真完成后释放它，这样在仿真过程中就不必调用 C 语言的内存分配和释放函数。这样做是为了提高速度、避免内存碎片、保持 GPU 可移植性，并方便在重置期间管理整个仿真器的状态。然而，这也意味着 size MJCF 元素中的 memory 属性所给出的最大变量内存分配（这会影响 mjData 的分配）必须设置为足够大的值。如果仿真过程中超过了这个最大值，它不会动态增加，而是会产生一个错误。另请参阅下文的 诊断。

首先，我们必须调用用于分配和初始化 mjModel 并返回其指针的函数之一。可用的选项有

// option 1: parse and compile XML from file
mjModel* m = mj_loadXML("mymodel.xml", NULL, errstr, errstr_sz);

// option 2: parse and compile XML from virtual file system
mjModel* m = mj_loadXML("mymodel.xml", vfs, errstr, errstr_sz);

// option 3: load precompiled model from MJB file
mjModel* m = mj_loadModel("mymodel.mjb", NULL);

// option 4: load precompiled model from virtual file system
mjModel* m = mj_loadModel("mymodel.mjb", vfs);

// option 5: deep copy from existing mjModel
mjModel* m = mj_copyModel(NULL, mexisting);

// option 6: compile model from mjSpec
mjModel* m = mj_compile(spec, vfs);

如果出现错误或警告，所有这些函数都会返回一个 NULL 指针。在 XML 解析和模型编译的情况下，错误的描述会返回到作为参数提供的字符串中。对于其余函数，将调用底层的 mju_error 或 mju_warning 并附带错误/警告消息；请参阅 错误处理。一旦我们获得了由上述函数之一分配的 mjModel 指针，我们就将其作为参数传递给所有需要访问模型的 API 函数。请注意，大多数函数将此指针视为 const；更多相关信息请参阅下文的 模型更改。

虚拟文件系统 (VFS) 允许将磁盘资源加载到内存中或由用户以编程方式创建，然后 MuJoCo 的加载函数会在访问磁盘之前搜索 VFS 中的文件。请参阅 API 参考章节中的 虚拟文件系统。

除了保存模型描述的 mjModel 外，我们还需要作为所有计算工作区的 mjData。请注意，mjData 特定于给定的 mjModel。API 函数通常假设用户了解自己在做什么，并执行最少的参数检查。如果传递给任何 API 函数的 mjModel 和 mjData 不兼容（或为 NULL），则会导致未定义的行为。mjData 使用以下函数创建

// option 1: create mjData corresponding to given mjModel
mjData* d = mj_makeData(m);

// option 2: deep copy from existing mjData
mjData* d = mj_copyData(NULL, m, dexisting);

一旦 mjModel 和 mjData 都已分配并初始化，我们就可以调用各种仿真函数。当我们完成后，可以使用以下函数删除它们

// deallocate existing mjModel
mj_deleteModel(m);

// deallocate existing mjData
mj_deleteData(d);

代码示例说明了完整的初始化和终止序列。

MuJoCo 仿真是 确定性的。

仿真循环#

在 MuJoCo 中运行仿真循环有多种方法。最简单的方法是在循环中调用顶层仿真函数 mj_step，例如

// simulate until t = 10 seconds
while (d->time < 10)
  mj_step(m, d);

这本身只会模拟被动动力学，因为我们没有提供任何控制信号或施加力。控制系统的默认方式是实现一个控制回调，例如

// simple controller applying damping to each DOF
void mycontroller(const mjModel* m, mjData* d) {
  if (m->nu == m->nv)
    mju_scl(d->ctrl, d->qvel, -0.1, m->nv);
}

这说明了两个概念。首先，我们检查控制信号的数量 mjModel.nu 是否等于自由度数量 mjModel.nv。通常，根据用户代码的结构，同一个回调可以用于多个模型，因此在回调中检查模型维度是个好主意。其次，MuJoCo 拥有一个非常实用的类 BLAS 函数库；事实上，代码库的很大一部分内部调用了这些函数。上面的 mju_scl 函数通过恒定的反馈增益缩放速度向量 mjData.qvel，并将结果复制到控制向量 mjData.ctrl 中。要安装此回调，只需将其分配给全局控制回调指针 mjcb_control

// install control callback
mjcb_control = mycontroller;

现在，如果我们调用 mj_step，每当仿真流水线需要控制信号时，我们的控制回调就会执行，结果我们将模拟受控动力学。

除了依赖控制回调，我们还可以直接设置控制向量 mjData.ctrl。或者，我们可以按照 状态和控制 中的说明设置施加的力。如果我们能在调用 mj_step 之前计算这些与控制相关的量，那么受控动力学的仿真循环（不使用控制回调）将变为

while (d->time < 10) {
  // set d->ctrl or d->qfrc_applied or d->xfrc_applied
  mj_step(m, d);
}

为什么我们不能在调用 mj_step 之前计算控制量？毕竟，这不就是因果关系的含义吗？答案很微妙但很重要，它与我们在离散时间进行模拟的事实有关。顶层仿真函数 mj_step 做了两件事：计算连续时间内的 前向动力学，然后按 mjModel.opt.timestep 指定的时间段进行积分。前向动力学根据时间 mjData.time 的 状态和控制，计算出时间 mjData.time 的加速度 mjData.qacc。然后数值积分器将状态和时间推进到 mjData.time + mjModel.opt.timestep。现在，控制量被要求是时间 mjData.time 的状态的函数。然而，通用的反馈控制器可能是一个非常复杂的函数，取决于状态的各种特征——特别是 MuJoCo 作为仿真中间结果计算的所有特征。这些特征可能包括接触、雅可比矩阵、被动力。在调用 mj_step 之前，这些量均不可用（或者说，它们可用但 *滞后一个时间步长*）。相比之下，当 mj_step 调用我们的控制回调时，它会尽可能晚地执行——即在计算完所有依赖于状态而不依赖于控制的中间结果之后。

不使用控制回调也可以实现相同的效果。这可以通过将 mj_step 分成两部分来完成：控制所需之前和之后。仿真循环现在变为

while (d->time < 10) {
  mj_step1(m, d);
  // set d->ctrl or d->qfrc_applied or d->xfrc_applied
  mj_step2(m, d);
}

然而，有一个复杂之处：这仅适用于单步 积分器（欧拉、隐式、快速隐式）。龙格-库塔积分器需要多次评估包括反馈控制律在内的整个动力学，这只能使用控制回调来完成。对于单步积分器，将 mj_step 分解为 mj_step1 和 mj_step2 足以为控制律提供计算的中间结果。

为了使上述讨论更清晰，我们提供了 mj_step、mj_step1 和 mj_step2 的内部实现，省略了一些计算计时诊断的代码。主要的仿真函数是

void mj_step(const mjModel* m, mjData* d) {
  // common to all integrators
  mj_checkPos(m, d);
  mj_checkVel(m, d);
  mj_forward(m, d);
  mj_checkAcc(m, d);

  // use selected integrator
  switch ((mjtIntegrator) m->opt.integrator) {
  case mjINT_EULER:
    mj_Euler(m, d);
    break;

  case mjINT_RK4:
    mj_RungeKutta(m, d, 4);
    break;

  case mjINT_IMPLICIT:
  case mjINT_IMPLICITFAST:
    mj_implicit(m, d);
    break;

  default:
    mjERROR("invalid integrator");
  }
}

如果任何数值变得无效或过大，检查函数会自动重置仿真。控制回调（如果有）是从前向动力学函数内部调用的。

接下来我们将展示两步法步进方法的实现，尽管具体细节只有在稍后解释 前向动力学 后才有意义。请注意，现在控制回调被直接调用，因为我们实际上已经解开了前向动力学函数。另请注意，我们在 mj_step2 中总是调用单步积分器；如果选择了 RK4，积分器将默认使用欧拉方法。

void mj_step1(const mjModel* m, mjData* d) {
  mj_checkPos(m, d);
  mj_checkVel(m, d);
  mj_fwdPosition(m, d);
  mj_sensorPos(m, d);
  mj_energyPos(m, d);
  mj_fwdVelocity(m, d);
  mj_sensorVel(m, d);
  mj_energyVel(m, d);

  // if we had a callback we would be using mj_step, but call it anyway
  if (mjcb_control)
    mjcb_control(m, d);
}

void mj_step2(const mjModel* m, mjData* d) {
  mj_fwdActuation(m, d);
  mj_fwdAcceleration(m, d);
  mj_fwdConstraint(m, d);
  mj_sensorAcc(m, d);
  mj_checkAcc(m, d);

  // integrate with Euler or implicit; RK4 defaults to Euler
  if (m->opt.integrator == mjINT_IMPLICIT || m->opt.integrator == mjINT_IMPLICITFAST)
    mj_implicit(m, d);
  else
    mj_Euler(m, d);
}

状态和控制#

MuJoCo 具有定义明确的状态，可以轻松地设置、重置和随时间推进。这与动力学系统的状态概念密切相关。动力学系统通常以通用形式描述

dx/dt = f(t, x, u)

其中 t 是时间，x 是状态向量，u 是控制向量，f 是计算状态时间导数的函数。这是一种连续时间表述，实际上 MuJoCo 模拟的物理模型也是在连续时间中定义的。尽管数值积分器在离散时间中运行，但计算的主要部分——即函数 mj_forward——对应于上面的连续时间动力学函数 f(t,x,u)。下面我们解释这种对应关系。

状态组件#

状态由不同的组件组成，在 mjtState 位域枚举中描述，该枚举列出了单个组件和组件的组合。它们是

物理状态#

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

位置：qpos

广义坐标中的配置，在 数值积分 章节中表示为 \(q\)。

速度：qvel

广义速度，在 数值积分 章节中表示为 \(v\)。在存在四元数的情况下（即使用自由关节或球关节时），位置向量 mjData.qpos 的维度高于速度向量 mjData.qvel，因此这在标量意义上不是简单的时间导数，而是考虑了四元数代数。

执行器激活：act

对于二阶机械系统，状态仅包含位置和速度，但 MuJoCo 还模拟了具有自身激活状态（组装在 mjData.act 中，在 数值积分 章节中表示为 \(w\)）的有状态执行器（如生物肌肉）。

历史缓冲区：history

当执行器或传感器具有正的 nsample 属性（执行器，传感器）时，此缓冲区存储先前控制或传感器值的时间戳样本。有关详细信息，请参阅 延迟。

完整物理状态#

上面的 t, x 对应于 完整物理状态 (mjSTATE_FULLPHYSICS) —— 所有随时间变化的东西。它是 物理状态 和两个额外组件

时间：time

虽然力学是时间无关的，但用户定义的控制律可能是时间相关的；特别是从轨迹获得的控制律通常是时间索引的。因此，时间 t (mjData.time) 是一个具有 dt/dt == 1 的状态组件。

插件状态：plugin_state

mjData.plugin_state 是由 引擎插件 声明的状态。请参阅 插件状态 章节了解更多详细信息。

用户输入#

这些输入字段 (mjSTATE_USER) 由用户设置并影响物理模拟，但仿真器不会触碰它们。除了 MoCap 位姿外，所有输入字段默认均为 0。所有 用户输入 数组的一个通用属性是它们不会被库触碰。因此，从写入此内存的值是持久性的意义上讲，它们也可以被视为是有状态的。

控制向量 u 主要对应于数组 mjData.ctrl，包含用户设置的驱动信号。“主要”是因为力矩和扳手也可以分别使用 mjData.qfrc_applied 和 mjData.xfrc_applied 直接施加。作为 用户控制的静态物体 的 MoCap 物体位姿也是一种用户输入。字段 mjData.userdata 是一个固定大小的内存块（通过设置 nuserdata 分配），旨在为用户提供任何用途，并可用于存储各种类似状态和类似控制的量。

控制：ctrl

控制由 XML 的 执行器 部分定义。mjData.ctrl 值要么直接产生广义力（无状态执行器），要么影响 mjData.act 中的执行器激活，进而产生力。请注意，虽然所有执行器都会产生力，但 ctrl 和 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 位姿时。mj_resetData 设置的默认值是物体在默认配置下的位姿。

等式约束切换：eq_active

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

用户数据：userdata

mjData.userdata 充当引擎不触碰的用户定义内存空间。例如，它可被回调函数使用。这在 编程章节 中有更详细的描述。

热启动#

热启动加速度：qacc_warmstart

mjData.qacc_warmstart 是前一步用于热启动约束求解器的加速度。假设当前解与前一个解相差不大，这可以通过减少收敛所需的迭代次数来加速仿真。当使用像 PGS 这样收敛缓慢的 约束求解器 时，它们可以通过减少收敛所需的迭代次数来加速仿真。然而请注意，默认的牛顿求解器收敛速度非常快（通常 2-3 次迭代），因此热启动对速度的影响通常微乎其微，并且可以被 禁用。

因为我们的优化问题是 严格凸的 且具有单个全局最小值，所以不同的求解器初始化对解没有可察觉的影响（假设已实现收敛）。如果未能实现数值收敛（无论是由于收敛缓慢，还是因为 迭代次数 或 容差 被限制，有时在 MJX 中就是这样做的），这种影响就会变得显著。

需要热启动的另一种情况是，当加载非初始状态时需要完美的数值可重复性（因为初始状态总是冷启动的）。请注意，即使它们对物理的影响微乎其微，许多物理系统在进行时间步进时也会 呈指数级 积累微小的差异，从而迅速导致不同热启动的轨迹发生分歧。有关更多详细信息，请参阅 可重复性。

积分状态#

积分状态 (mjSTATE_INTEGRATION) 是上述所有 mjData 字段的并集，构成了 前向动力学 的全部输入集。两个具有相同积分状态的 mjData 实例的流水线输出将是相同的。在 逆动力学 的情况下，mjData.qacc 也被视为输入变量。所有其他 mjData 字段都是积分状态的函数。

请注意，由 mjSTATE_INTEGRATION 给出的完整积分状态是极简主义的，包含通常未使用的字段。如果需要较小的状态大小，避免保存未使用的字段可能是合理的。特别是 xfrc_applied 可能会变得非常大（nbody x 6），但通常不会被使用。

仿真状态#

仿真状态 是 mjData 结构和相关内存缓冲区的整体。此状态包括在动力学计算过程中计算的所有派生量。因为 mjData 缓冲区是为最坏情况预分配的，所以从 积分状态 重新计算派生量通常比使用 mj_copyData 要快得多。有关在启用睡眠时关于仿真状态的注意事项，请参阅 关于睡眠的说明。

状态操作#

通过 mjtState 位域枚举可以促进对状态的操作，该枚举列出了上述记录的状态组件。可以对组件的组合（其中一些在枚举本身中可用）进行 OR 运算以形成位域值，例如

int sig = mjSTATE_TIME | mjSTATE_QPOS | mjSTATE_CTRL;  // custom choice of state components

使用这些位域的函数是 mj_getState、mj_setState、mj_copyState 和 mj_extractState。例如，在将 mjData 实例 src 的 积分状态 复制到另一个实例 dst 后

mj_copyState(model, src, dst, mjSTATE_INTEGRATION);

步进 src 或 dst 将产生相同的结果。状态可以从单个 mjtNum 数组中检索和设置

int sig = mjSTATE_TIME | mjSTATE_QPOS | mjSTATE_CTRL;
int size = mj_stateSize(model, sig);
mjtNum* state = mju_malloc(size * sizeof(mjtNum));
mj_getState(model, src, state, sig);  // copy time, qpos and ctrl from src into state
mj_setState(model, dst, state, sig);  // copy time, qpos and ctrl from state into dst

整个 mjData 也可以使用函数 mj_copyData 进行复制，但这当然比 mj_copyState 慢得多。

在此上下文中同样相关的是函数 mj_resetData。它将 mjData.qpos 设置为等于模型参考配置 mjModel.qpos0，将 mjData.mocap_pos 和 mjData.mocap_quat 设置为等于 mjModel 中相应的固定物体位姿；并将所有其他状态和控制变量设置为 0。当某些树 初始化为睡眠 时，此函数会执行更多工作，请参阅下文的 睡眠。

前向动力学#

前向动力学的目标是计算状态的时间导数，即加速度向量 mjData.qacc 和激活时间导数 mjData.act_dot。在此过程中，它计算模拟动力学所需的所有其他内容，包括主动接触和其他约束、关节空间惯量及其 \(L^TDL\) 分解、约束力、传感器数据等。所有这些中间结果均在 mjData 中提供，并可用于自定义计算。正如上面 仿真循环 章节中所述，主步进函数 mj_step 调用 mj_forward 来完成大部分工作，然后调用数值积分器将仿真状态推进到下一个离散时间点。

前向动力学函数 mj_forward 在内部调用带有跳过参数 (mjSTAGE_NONE, 0) 的 mj_forwardSkip，其中后一个函数实现为

void mj_forwardSkip(const mjModel* m, mjData* d, int skipstage, int skipsensor) {
  // position-dependent
  if (skipstage < mjSTAGE_POS) {
    mj_fwdPosition(m, d);
    if (!skipsensor)
      mj_sensorPos(m, d);
    if (mjENABLED(mjENBL_ENERGY))
      mj_energyPos(m, d);
  }

  // velocity-dependent
  if (skipstage < mjSTAGE_VEL) {
    mj_fwdVelocity(m, d);
    if (!skipsensor)
      mj_sensorVel(m, d);
    if (mjENABLED(mjENBL_ENERGY))
      mj_energyVel(m, d);
  }

  // acceleration-dependent
  if (mjcb_control)
    mjcb_control(m, d);
  mj_fwdActuation(m, d);
  mj_fwdAcceleration(m, d);
  mj_fwdConstraint(m, d);
  if (!skipsensor)
    mj_sensorAcc(m, d);
}

请注意，这与上面的 mj_step1 和 mj_step2 中的调用序列相同，只是省略了检查实数值以及计算传感器和能量等特征。所调用的函数是仿真流水线的组件。它们反过来调用子组件。

整数参数 skipstage 确定计算的哪些部分将被跳过。可能的跳过级别是

mjSTAGE_NONE

不跳过任何内容。运行所有计算。

mjSTAGE_POS

跳过依赖于位置但不依赖于速度、控制或施加力的计算。此类计算的示例包括前向运动学、碰撞检测、惯量矩阵计算和分解。这些计算通常占用最多的 CPU 时间，应在可能时跳过（见下文）。

mjSTAGE_VEL

跳过依赖于位置和速度但不依赖于控制或施加力的计算。示例包括科里奥利力和离心力的计算、被动阻尼力、用于约束稳定的参考加速度。

mjData 的中间结果字段根据计算它们所需的状态部分被组织成多个部分。使用 mjSTAGE_POS 调用 mj_forwardSkip 假设第一部分（依赖于位置）中的字段已经计算出来，并且不会重新计算它们。同样，mjSTAGE_VEL 假设第一部分和第二部分（依赖于位置和速度）中的字段已经计算出来。

我们什么时候可以使用上述机制并跳过一些计算？在常规仿真中，这是不可能的。然而，MuJoCo 不仅专为仿真设计，还专为更高级的应用（如基于模型的优化、机器学习等）设计。在这种设置下，通常需要对附近的云状态进行采样，或者通过有限差分近似导数——这是采样的另一种形式。如果样本排列在网格上，其中只有位置、速度或控制与中心点不同，则上述机制可以将性能提高约 2 倍。

逆动力学#

逆动力学的计算是 MuJoCo 的一项独特功能，在任何其他能够模拟接触的现代引擎中都没有发现。由于我们在概览章节中描述的 软约束模型，逆动力学定义明确且计算效率极高。事实上，一旦执行了与前向动力学共享的依赖于位置和速度的计算，给定加速度的约束力和施加力的恢复就归结为一个分析公式。这非常快，以至于我们实际上使用逆动力学（使用前一时间步计算的加速度）来热启动前向动力学中的迭代约束求解器。

逆动力学的输入与前向动力学中的状态向量相同，如 状态和控制 中所示，但没有 mjData.act 和 mjData.time。假设没有依赖于用户定义状态变量的回调，逆动力学的输入是 mjData 的以下字段

(mjData.qpos, mjData.qvel, mjData.qacc, mjData.mocap_pos, mjData.mocap_quat)

主要输出是 mjData.qfrc_inverse。这是为了达到观察到的加速度 mjData.qacc 而必须施加在系统上的力。如果通过运行迭代求解器达到完全收敛来精确计算前向动力学，我们将有

mjData.qfrc_inverse = mjData.qfrc_applied + Jacobian'*mjData.xfrc_applied + mjData.qfrc_actuator

其中 mjData.qfrc_actuator 是执行器产生的关节空间力，而雅可比矩阵是从关节空间到笛卡尔空间的映射。当设置 mjModel.opt.enableflags 中的 fwdinv 标志时，上述恒等式用于监控前向动力学解的质量。特别是，mjData.solver_fwdinv 的两个分量分别被设置为正向解和逆向解之间在关节力和约束力方面的 L2 范数差。

与前向动力学类似，mj_inverse 在内部调用带有跳过参数 (mjSTAGE_NONE, 0) 的 mj_inverseSkip。跳过机制与前向动力学相同，可用于加快结构化采样的速度。结果 mjData.qfrc_inverse 是通过使用递归牛顿-欧拉算法计算作用在系统上的净力，然后从中减去所有内力而获得的。

当实验数据可用时，逆动力学可用作分析工具。这在机器人学和生物力学中很常见。它也可用于计算驱动系统沿给定参考轨迹所需的关节力矩；这被称为计算力矩控制。在状态估计、系统辨识和最优控制的背景下，它可以在优化循环内使用，以找到在满足其他成本的同时最小化物理违规的状态序列。物理违规可以量化为逆动力学计算出的任何无法解释的外力的范数。

多线程#

MuJoCo 对步内多线程提供实验性支持。当 mjThreadPool 分配给 mjData.threadpool 时，仿真流水线的部分——例如跨 孤岛 的碰撞检测和约束求解——可以分布在工作线程中。请注意，步内线程目前具有显著的内存开销，并且仍在进行中。

更常见且得到充分支持的多线程用法是加速在更高级应用中常见的采样操作。仿真在时间上本质上是串行的（一个 mj_step 的输出是下一个的输入），而在采样中，由于它们之间没有依赖关系（除了可能共同的初始状态外），因此可以并行执行多次对前向或逆动力学的调用。

MuJoCo 从一开始就是为多线程设计的。与大多数现有仿真器（其中动力学系统状态的概念难以映射到软件状态并且通常分布在多个对象中）不同，在 MuJoCo 中，我们拥有统一的数据结构 mjData，它包含随时间变化的一切。回想一下关于 状态和控制 的讨论。关键思想是为每个线程创建一个 mjData，然后将其用于所有线程的计算。下面是通用模板，使用 OpenMP 来简化线程管理。

// prepare OpenMP
int nthread = omp_get_num_procs();      // get number of logical cores
omp_set_dynamic(0);                     // disable dynamic scheduling
omp_set_num_threads(nthread);           // number of threads = number of logical cores

// allocate per-thread mjData
mjData* d[64];
for (int n=0; n < nthread; n++)
  d[n] = mj_makeData(m);

// ... serial code, perhaps using its own mjData* dmain

// parallel section
#pragma omp parallel
{
  int n = omp_get_thread_num();       // thread-private variable with thread id (0 to nthread-1)

  // ... initialize d[n] from results in serial code

  // thread function
  worker(m, d[n]);                    // shared mjModel (read-only), per-thread mjData (read-write)
}

// delete per-thread mjData
for (int n=0; n < nthread; n++)
  mj_deleteData(d[n]);

由于所有顶层 API 函数都将 mjModel 视为 const，因此该多线程方案是安全的。每个线程只写入自己的 mjData。因此，线程之间不需要进一步同步。

上面的模板反映了一种特定的并行处理风格。我们不创建大量线程（每个工作项一个）并让 OpenMP 在处理器之间分配它们，而是依赖手动调度。更准确地说，我们创建与处理器一样多的线程，然后在 worker 函数中显式地在线程之间分配工作。这种方法更有效，因为与处理器缓存相比，线程特定的 mjData 很大。

为了缓存效率，我们也使用共享的 mjModel。在某些情况下，可能无法为所有线程使用同一个 mjModel。一个明显的原因是 mjModel 可能需要在线程函数内进行修改。另一个原因是，包含在 mjModel 中的 mjOption 结构可能需要调整（例如，为了控制求解器迭代次数），尽管这对所有并行线程来说可能是相同的，因此可以在并行部分之前在共享模型中进行调整。

线程特定的 mjData 如何初始化以及线程函数做什么，当然取决于应用。尽管如此，早期章节中的通用效率指南也适用于此处。将状态复制到线程特定的 mjData 并运行 MuJoCo 来填充其余部分可能比使用 mj_copyData 更快。此外，前向和逆动力学中可用的跳过机制在并行采样应用中特别有用，因为样本通常具有允许重用某些计算的结构。最后，请记住，前向求解器是迭代的，好的热启动可以显著减少所需的迭代次数。当样本在状态和控制空间中彼此接近时，一个样本（理想情况下在中心）的解可用于热启动所有其他样本。在这种设置下，确保附近样本之间的不同结果反映的是样本之间的真正差异，而不是迭代求解器的不同热启动或终止，这一点很重要。

mjModel 更改#

使用 mjSpec 进行程序化模型编辑

下面关于 mjModel 修改的讨论是在引入程序化 模型编辑 之前编写的。它仍然有效，但新框架是修改模型的安全且推荐的方法。在运行时修改 mjModel 而不是修改 mjSpec 并再次编译的主要原因是 速度。然而，进行某些更改可能是不安全的，要么是指段错误（segfaults）可能发生，要么是指物理模拟会意外改变。

一般的规则是实值参数可以安全更改，而结构化整数参数则不然，因为这可能导致不正确的大小或索引。此规则并不普遍适用，我们在下面描述例外情况。

整数 类型 不安全更改 的一般规则的例外情况

字段

可修改性

注释

XXX_limited
XXX_group
XXX_matid
XXX_texid

安全

XXX_sameframe

不安全

此标志告诉引擎跳过父/子帧变换。从非零变为零是安全的，反之则不然。

geom_contype
geom_conaffinity

不安全

如果更新父物体的 body_contype 和 body_conaffinity 为所有子几何体的按位 OR，则可以安全地做到这一点。

geom_condim
geom_priority

安全

cam_resolution

安全

light_castshadow
light_active

安全

flex_contype
flex_conaffinity
flex_condim
flex_priority

安全

tex_data

安全

必须调用 mjr_uploadTexture 以更新 GPU 内存中的值。

在考虑实值参数可以安全更改的规则例外时，我们需要注意函数 mj_setConst，它构成了编译过程的最后一步。此函数将更改从某些字段传播到其他字段，从而允许进行原本不安全的更改。

实值 类型 可以安全更改 的一般规则的例外情况

字段

可修改性

注释

qpos0
qpos_spring

配合 mj_setConst 使用时是安全的。

body_mass
body_inertia
body_ipos
body_iquat

配合 mj_setConst 使用时是安全的。

请注意，质量和惯量通常是一起缩放的，因为惯量是 \(\sum m r^2\)。分别缩放它们是合法的，但暗示了空间质量分布的改变。另请注意，对角惯量必须遵循三角不等式。

body_pos
body_quat

配合 mj_setConst 使用时是安全的。

对于静态物体是不安全的，这会使中间阶段碰撞结构 (BVH) 失效。

body_gravcomp

安全。

如果具有重力补偿的物体数量从零更改为非零，则必须调用 mj_setConst。

dof_armature

配合 mj_setConst 使用时是安全的。

geom_pos
geom_quat
geom_size
geom_rbound
geom_aabb

不安全。

{site,cam,light}_
{pos,quat}

大部分安全。

对于具有跟踪或瞄准功能的摄像机和灯光，需要调用 mj_setConst。

tendon_stiffness
tendon_damping

大部分安全。

影响运动树是否允许睡眠。如果从/向零更改，则需要调用 mj_setConst。

actuator_gainprm
actuator_biasprm

大部分安全。

对于使用 dampratio 的位置型执行器，需要调用 mj_setConst。

eq_data

配合 mj_setConst 使用时是安全的。

对于连接和焊接约束，如果未提供，则会计算偏移量。

hfield_size

配合 mj_setConst 使用时是安全的。

hfield_data

安全。

数据范围必须在 [0, 1] 之间。
mjr_uploadHField 是更新 GPU 内存中的值所必需的。

mesh_scale
mesh_pos
mesh_quat

并非不安全，但没有效果。

mesh_pos 和 mesh_quat 会在运行时影响 SDF 传感器。

mesh_vert
mesh_normal
mesh_face
mesh_polynormal

对于碰撞网格不安全。

对于视觉网格是安全的，但需要调用 mjr_uploadMesh 以更新 GPU 内存中的值。

bvh_aabb
oct_aabb
oct_coeff

不安全

最后，如果对 mjModel 进行了运行时更改，则可能希望将它们保存回 XML。函数 mj_saveLastXML 和 mj_copyBack 在有限的意义上做到了这一点：它们将所有实值参数从 mjModel 复制回 mjSpec（在前一种情况下是全局内部规范，在后一种情况下是用户的副本）。这并没有涵盖用户可能做出的所有可能的更改。保证所有更改都被保存的唯一方法是使用函数 mj_saveModel 将模型保存为二进制 MJB 文件，或者更好的是，直接在 XML 或 mjSpec 中进行更改。总之，我们有合理但不完美的模型更改保存机制。这种不完美的原因是我们正在使用编译后的模型，所以这就像更改二进制可执行文件并要求“反编译器”对 C 代码进行相应更改一样——这在一般情况下是不可能的。

数据布局#

MuJoCo 中的所有矩阵均为 行优先 格式。例如，线性内存数组 (a0, a1, … a5) 表示 2x3 矩阵

a0 a1 a2
a3 a4 a5

这种约定传统上与 C 相关联，而相反的列优先约定则与 Fortran 相关联。选择哪一种并没有特别的原因，但无论选择什么，始终牢记这一点至关重要。所有对矩阵进行操作的 MuJoCo 实用函数（如 mju_mulMatMat、mju_mulMatVec 等）都假设此矩阵布局。对于向量，当然没有行优先和列优先格式的区别。

在可能的情况下，MuJoCo 会利用稀疏性。这可以使缩放差异达到 O(N) 和 O(N^3)。惯量矩阵 mjData.qM 及其 LTDL 分解 mjData.qLD 始终表示为稀疏。 qM 使用为对应于树拓扑的矩阵设计的自定义索引格式，而 qLD 使用标准的 CSR 格式。qM 将在即将到来的更改中迁移到 CSR。函数 mj_factorM、mj_solveM、mj_solveM2 和 mj_mulM 用于稀疏分解、代换和矩阵-向量乘法。用户也可以使用函数 mj_fullM 将这些矩阵转换为密集格式，尽管 MuJoCo 在内部从不这样做。

只要启用了稀疏雅可比选项，约束雅可比矩阵 mjData.efc_J 就会表示为稀疏。函数 mj_isSparse 可用于确定当前是否正在使用稀疏格式。在这种情况下，转置雅可比 mjData.efc_JT 也会被计算，并且逆约束惯量 mjData.efc_AR 变为稀疏。稀疏矩阵以压缩稀疏行 (CSR) 格式存储。对于具有 m-by-n 维度的通用矩阵 A，此格式为

变量

大小

含义

A

m * n

实值数据

A_rownnz

m

每行非零元素的数量

A_rowadr

m

A 和 A_colind 中行数据的起始索引

A_colind

m * n

列索引

因此，A[A_rowadr[r]+k] 是底层稠密矩阵中位于第 r 行和第 A_colind[A_rowadr[r]+k] 列的元素，其中 k < A_rownnz[r]。通常不需要 m*n 的存储空间（假设矩阵确实是稀疏的），但我们为最坏情况分配了空间。此外，在可以更改稀疏模式的操作中，分散数据更有效，这样我们在插入新数据时不必执行许多内存移动。我们将这种稀疏布局称为“未压缩”。它仍然是一种有效的布局，但我们不是使用标准的 A_rowadr[r] = A_rowadr[r-1] + A_rownnz[r]，而是将 A_rowadr[r] 设置为 r*n。MuJoCo 在内部使用稀疏矩阵

为了表示 3D 定向和旋转，MuJoCo 使用单位四元数——即排列为 q = (w, x, y, z) 的 4D 单位向量。这里 (x, y, z) 是由 sin(a/2) 缩放的旋转轴单位向量，其中 a 是弧度旋转角度，w = cos(a/2)。因此，对应于零旋转的四元数是 (1, 0, 0, 0)。这是 MJCF 中所有四元数的默认设置。

MuJoCo 还在内部使用 6D 空间向量。这些是 mjData 中前缀为‘c’的量，即 cvel、cacc、cdot 等。它们是结合了 3D 旋转分量后跟 3D 平移分量的空间运动和力向量。我们不提供用于处理它们的实用函数，记录它们超出了我们在此的范围。请参阅 Roy Featherstone 关于 空间代数 的网页。不寻常的顺序（旋转在前，平移在后）是基于此材料的，并且显然是过去的标准惯例。

数据结构 mjModel 和 mjData 包含许多指向预分配缓冲区的指针。这些数据结构的构造函数（mj_makeModel 和 mj_makeData）分配一个大缓冲区，即 mjModel.buffer 和 mjData.buffer，然后对其进行分区并设置其中的所有其他指针。mjData 还包含此主缓冲区之外的堆栈，如下所述。即使两个指针看起来是一个接一个的，例如 mjData.qpos 和 mjData.qvel，也不要假设数据数组是连续的且它们之间没有间隙。构造函数为每个数据数组实现字节对齐，并在必要时跳过字节。因此，如果您想复制 mjData.qpos 和 mjData.qvel，正确的做法是采用困难的方式

// do this
mju_copy(myqpos, d->qpos, m->nq);
mju_copy(myqvel, d->qvel, m->nv);

// DO NOT do this, there may be padding at the end of d->qpos
mju_copy(myqposqvel, d->qpos, m->nq + m->nv);

定义在可选头文件 mjxmacro.h 中的 X 宏 可用于自动化匹配 mjModel 和 mjData 的数据结构分配，例如在为脚本语言编写 MuJoCo 包装器时。

内部堆栈#

MuJoCo 在 mjData.arena 中的“竞技场”空间中分配和管理动态内存。竞技场内存空间包含两种类型的动态分配内存

  • 与约束相关的内存，因为接触数量在步进开始时是未知的。

  • 由内部堆栈机制管理的临时变量内存。

请参阅 内存分配 以了解有关竞技场和内部堆栈布局的详细信息。

大多数顶层 MuJoCo 函数会在 mjData 堆栈上分配空间，将其用于内部计算，然后释放它。它们不能使用常规 C 堆栈来做到这一点，因为分配大小是在运行时动态确定的。调用堆内存管理函数将效率低下并导致碎片——因此需要自定义堆栈。当调用任何 MuJoCo 函数时，返回时 mjData.pstack 的值是相同的。唯一的例外是函数 mj_resetData 及其变体：它们将 mjData.pstack = 0。请注意，当在 mj_step、mj_step1 和 mj_step2 中检测到不稳定性时，此函数会在内部被调用。因此，如果用户函数利用了自定义堆栈，这需要在具有重置仿真潜力的 MuJoCo 调用之间完成。

下面是用户代码中使用自定义堆栈的通用模板。 

// mark an mjData stack frame
mj_markStack(d);

// allocate space
mjtNum* myqpos = mj_stackAllocNum(d, m->nq);
mjtNum* myqvel = mj_stackAllocNum(d, m->nv);

// restore the mjData stack frame
mj_freeStack(d);

函数 mj_stackAllocNum 检查是否有足够的空间，如果有，它会推进堆栈指针，否则会触发错误。它还会跟踪最大堆栈分配；请参阅下文的 诊断。请注意，mj_stackAllocNum 仅用于分配 mjtNum 数组（最常见的数组类型）。mj_stackAllocInt 用于分配整数数组，mj_stackAllocByte 用于分配任意数量的字节和对齐。

错误和警告#

当发生终端错误时，MuJoCo 会在内部调用函数 mju_error。mju_error 执行以下操作

  1. 将错误消息附加到程序目录中的 MUJOCO_LOG.TXT 文件末尾（如果不存在则创建该文件）。同时写入日期和时间以及错误消息。

  2. 如果安装了用户错误回调 mju_user_error，则使用错误消息作为参数调用该函数。否则，将错误消息和“Press Enter to exit…”打印到标准输出。然后等待任何键盘输入，最后以失败终止仿真器。

如果安装了用户错误回调，它 不得 返回，否则仿真器的行为是未定义的。这里的想法是，如果调用了 mju_error，仿真就无法继续，用户需要进行一些更改以避免错误条件。错误消息是不言自明的。

即使用户仿真器加载模型文件失败，也希望继续运行，这也是一种理想的情况。这可能是因为用户提供了错误的文件名，或者模型编译失败。这是通过避免调用 mju_error 的特殊机制来处理的。模型加载函数 mj_loadXML 和 mj_loadModel 如果操作失败则返回 NULL，并且不需要退出程序。在 mj_loadXML 的情况下，有一个输出参数包含导致失败的解析器或编译器错误，而 mj_loadModel 会生成相应的警告（见下文）。

在内部，mj_loadXML 实际上使用了 mju_error 机制，通过临时安装触发 C++ 异常的“用户”处理程序，然后进行拦截。这是可能的，因为解析器、编译器和运行时是一起编译和链接的，并使用相同副本的 C/C++ 内存管理器和标准库。如果用户实现了一个触发 C++ 异常的错误回调，它将在他们的工作区中，这不一定与 MuJoCo 库工作区相同，因此不清楚会发生什么；结果可能取决于编译器和平台。最好避免这种方法，并在调用 mju_error 时简单地退出（这是在没有用户处理程序的情况下的默认行为）。

MuJoCo 也会生成警告。它们指示可能导致数值不准确的条件，但也可能指示加载模型的问题以及其他仿真器仍然能够继续正常操作的问题。警告机制有两个级别。高级别通过函数 mj_warning 实现。它会在 mjData 中注册警告，如下面的 诊断 章节中所述，并调用低级函数 mju_warning。或者，低级函数可以直接调用（例如在 mj_loadModel 中），而不必在 mjData 中注册警告。这是在 mjData 不可用的地方完成的。

mju_warning 执行以下操作：如果安装了用户回调 mju_user_warning，它会调用该回调。否则，它会将警告消息附加到 MUJOCO_LOG.TXT 并执行 printf，类似于 mju_error 但不退出。当为 MATLAB 等环境开发 MuJoCo 包装器时，安装一个在命令窗口中打印警告的用户回调（使用 mexPrintf）是有意义的。

当 MuJoCo 在堆上分配和释放内存时，它总是使用函数 mju_malloc 和 mju_free。这些函数在安装后会调用用户回调 mju_user_malloc 和 mju_user_free，否则它们会调用标准的 C 函数 malloc 和 free。这种间接调用的原因是用户可能希望 MuJoCo 使用他们控制下的堆。例如，在 MATLAB 中，用于内存分配的用户回调将使用 mxmalloc 和 mexMakeArrayPersistent。

诊断#

MuJoCo 具有几种内置的诊断机制，可用于微调模型。它们的输出在 mjData 开头的诊断部分中进行分组。

当仿真器遇到非终端错误但仍然可疑且可能导致不准确数值结果的情况时，它会触发警告。有几种可能的警告类型，由枚举类型 mjtWarning 索引。数组 mjData.warning 包含每个警告类型的一个 mjWarningStat 数据结构，指示自上次重置以来每种警告类型被触发的次数以及有关该警告的信息（通常是有问题的模型元素的索引）。计数器在重置时被清除。当某种类型的警告首次触发时，警告文本也会由 mju_warning 打印，如上文 错误和内存 中所记录。所有这些都是由函数 mj_warning 完成的，仿真器在遇到警告时会在内部调用它。用户也可以直接调用此函数来模拟警告。

当需要优化模型以进行高速仿真时，了解 CPU 时间花费在流水线的哪个位置非常重要。这反过来可以建议简化模型的哪些部分或如何设计用户应用程序。MuJoCo 提供了广泛的分析机制。它涉及多个由枚举类型 mjtTimer 索引的计时器。每个计时器对应一个顶层 API 函数或此类函数的一个组件。与警告类似，计时器信息会累积，仅在重置时清除。数组 mjData.timer 包含每个计时器的一个 mjTimerStat 数据结构。给定计时器的每次调用平均持续时间（对应于下例中的 mj_step）可以计算为

mjtNum avtm = d->timer[mjTIMER_STEP].duration / mjMAX(1, d->timer[mjTIMER_STEP].number);

此机制内置于 MuJoCo 中，但只有在用户安装了计时器回调 mjcb_time 时才有效。否则，所有计时器持续时间均为 0。此设计的原因是因为没有平台无关的方法可以在不引入额外依赖项的情况下在 C 中实现高分辨率计时器。此外，大多数时候用户不需要计时，在这种情况下没有理由调用计时函数。

仿真流水线中需要密切监控的一个部分是迭代约束求解器。这里最简单的诊断是 mjData.solver_niter，它显示了在最后一次调用 mj_step 或 mj_forward 时求解器进行了多少次迭代。请注意，求解器有用于提前终止的容差参数，因此这个数字通常小于允许的最大迭代次数。数组 mjData.solver 包含约束求解器每次迭代的一个 mjsolverstat 数据结构，其中包含有关约束状态和线搜索的信息。

当在 mjModel.opt.enableflags 中启用 fwdinv 选项时，字段 mjData.fwdinv 也会被填充。它包含前向和逆动力学之间的差异，以广义力和约束力表示。请记住，逆动力学使用分析公式，并且始终是精确的，因此任何差异都是由于前向动力学中迭代求解器的收敛性较差所致。在终止附近 mjData.solver 中的数字与 mjData.fwdinv 中的数字具有相同的数量级，但它们是两个不同的诊断。

由于 MuJoCo 的运行时使用编译后的模型工作，因此在模型编译或加载时会预分配内存。回想一下 MJCF 中 size 元素的 memory 属性。它确定动态数组的预分配空间。用户应该如何知道合适的值是多少？如果有一个可靠的配方，我们会在编译器中实现它，但并没有。理论上的最坏情况，即所有几何体与其他所有几何体接触，需要巨大的分配，这在实践中几乎从不需要。我们的方法是在 MJCF 中提供对大多数模型都足够的默认设置，并允许用户使用上述属性手动调整它们。如果仿真器在运行时耗尽了动态内存，它将触发错误。当触发此类错误时，用户应该增加 memory。字段 mjData.maxuse_arena 旨在帮助进行此调整。它跟踪自上次重置以来的最大竞技场使用量。因此，一种策略是进行非常大的分配，然后在典型仿真期间监控 mjData.maxuse_arena 统计信息，并使用它来减少分配。

当设置 mjModel.opt.enableflags 中的相应标志时，动能和势能会被计算并存储在 mjData.energy 中。这可以用作另一种诊断。通常，仿真不稳定性与能量增加有关。在某些特殊情况下（当所有单边约束、执行器和耗散力都被禁用时），底层的物理系统是能量守恒的。在这种情况下，总能量的任何时间波动都表明数值积分不准确。对于此类系统，龙格-库塔积分器的性能比默认的半隐式欧拉积分器好得多。

雅可比矩阵#

任何向量函数相对于其向量参数的导数称为雅可比矩阵。当该术语用于多关节运动学和动力学时，它指的是某些空间量作为系统配置函数的导数。在这种情况下，雅可比矩阵也是一个作用在配置流形的切空间（或余切空间）中的向量的线性映射——例如速度、动量、加速度、力。这里的一个警告是，编码在 mjData.qpos 中的系统配置维度为 mjModel.nq，而切空间维度为 mjModel.nv，当存在四元数关节时，后者较小。因此，雅可比矩阵的大小为 N-by-mjModel.nv，其中 N 是被微分的空间量的维度。

MuJoCo 可以解析地微分许多空间量。这些量包括肌腱长度、执行器传动长度、末端执行器位姿、接触及其他约束违例。在肌腱和执行器传动的情况下，相应的量为 mjData.ten_moment 和 mjData.actuator_moment；我们将它们称为力臂，但从数学上讲，它们是雅可比矩阵（Jacobians）。所有标量约束违例的雅可比矩阵存储在 mjData.efc_J 中。请注意，我们讨论的是约束违例，而不是约束本身。这是因为约束违例具有长度单位，即它们是可以微分的空间量。约束是更抽象的实体，微分它们意味着什么并不明确。

除了这些自动计算的雅可比矩阵外，我们还提供辅助函数，允许用户按需计算额外的雅可比矩阵。执行此操作的主要函数是 mj_jac。它接收一个 3D 点以及被认为附着在该点上的 MuJoCo 物体（body）。mj_jac 随后计算平移和旋转雅可比矩阵，它们告诉我们如果对运动学配置进行微小改变，锚定在给定点的空间坐标系将如何平移和旋转。更确切地说，雅可比矩阵将关节速度映射到末端执行器速度，而雅可比矩阵的转置将末端执行器力映射到关节力。此外还有其他几个 mj_jacXXX 函数；这些是便捷函数，使用不同的兴趣点（如物体质心、几何中心等）调用主 mj_jac 函数。

精确且高效地计算末端执行器雅可比矩阵是在关节坐标系下工作的主要优势。此类雅可比矩阵是许多控制方案的基础，这些方案将末端执行器误差映射为适合抑制这些误差的执行器指令。通过 mj_jac 函数在 MuJoCo 中计算末端执行器雅可比矩阵在 CPU 开销方面几乎是免费的；因此，请放心使用此函数。

接触（Contacts）#

碰撞检测和接触力求解在计算（Computation）一章中有详细解释。在此，我们从编程的角度进一步阐明接触处理。

碰撞检测阶段查找几何体（geoms）之间的接触，并将其记录在 mjContact 数据结构的 mjData.contact 数组中。它们经过排序，使得同一对物体之间的多个接触是连续的（请注意，一个物体可以附着多个几何体），并且物体对本身也经过排序，第一物体作为主索引，第二物体作为次索引。并非所有检测到的接触都包含在接触力计算中。当接触被包含时，其 mjContact.exclude 字段为 0，其 mjContact.efc_address 是活动标量约束列表中的地址。排除原因可能是 geom 的 gap 属性，以及某些使用虚拟接触进行中间计算的内部处理。

列表 mjData.contact 由正向和反向动力学的位置阶段生成。这是自动完成的。但是，用户可以覆盖内部碰撞检测函数，例如实现非凸网格碰撞，或者用 MuJoCo 提供的原始几何体之外的特定几何体基元来替换我们使用的部分凸碰撞函数。全局二维数组 mjCOLLISIONFUNC 包含了每对几何体类型（在左上三角形中）的碰撞函数指针。要替换它们，只需将这些指针设置为您的函数即可。碰撞函数类型为 mjfCollision。当用户碰撞函数检测到接触时，它们应为每个接触构建一个 mjContact 结构，然后调用函数 mj_addContact 将该接触添加到 mjData.contact 中。mj_addContact 的参考文档解释了哪些 mjContact 字段必须由自定义碰撞函数填充。请注意，我们在此讨论的函数对应于近距离碰撞（near-phase collisions），并且仅在内部宽阶段（broad-phase）碰撞机制构建候选几何体对列表后才会被调用。

计算约束力后，接触 i 的力向量起始于

mjtNum* contactforce = d->efc_force + d->contact[i].efc_address;

其他所有 efc_XXX 向量同理。请记住，根据 mjModel.opt 中选择的求解器，接触摩擦锥可以是棱锥形或椭圆形。函数 mj_isPyramidal 可用于确定使用了哪种摩擦锥类型。对于棱锥形摩擦锥，上述计算出地址的接触力的解释并不直观，因为其分量是沿对应于棱锥边缘的冗余非正交轴的力。函数 mj_contactForce 可用于将给定接触产生的力转换为更直观的格式：3D 力加上 3D 转矩。当 condim 为 1 或 3 时，转矩分量为零，否则为非零。此力和转矩在 mjContact.frame 给出的接触坐标系中表示。与 mjData 中的所有其他矩阵不同，此矩阵以转置形式存储。通常，对应于坐标系的 3x3 矩阵会将坐标轴放在列中。在这里，轴沿矩阵的行排列。因此，鉴于 MuJoCo 使用行主序格式，根据我们的惯例，接触法线轴（即接触坐标系的 X 轴）位于 mjContact.frame[0-2] 位置，Y 轴在 [3-5]，Z 轴在 [6-8]。这种安排的原因是我们可能存在仅使用法线轴的无摩擦接触，因此将其坐标放在 mjContact.frame 的前 3 个位置是有意义的。

休眠孤岛（Sleeping islands）#

休眠孤岛在计算章节中已进行简要描述。这里我们专注于实现细节。

树（Trees）的高级休眠状态由 mjData.tree_asleep 描述（尽管请参阅下方的注意事项）。负值表示树处于唤醒状态，非负值表示处于休眠状态。完全唤醒的树被赋予值 -⁠(1⁠+⁠mjMINAWAKE)，在速度低于休眠 容差（tolerance） 的每个时间步，此整数都会递增，直至达到 -1，即表示“准备休眠”。如果孤岛中的所有树都准备好休眠，它们将在状态推进期间被置于休眠状态，且它们在 tree_asleep 中的关联值被设置为一个（非负）索引循环：即“休眠孤岛”。如果孤岛中的任何树被唤醒，则所有树都会被唤醒。

休眠策略（Sleep policy）#

运动学树的休眠能力由模型编译时确定的策略决定。编译器自动确定策略为“允许（allowed）”或“从不（never）”，尽管这些可以通过 body/sleep 属性进行覆盖（请参阅相关文档）。此外还有一个特殊的“初始化（init）”休眠策略，请参见下一节。

休眠（Sleeping）#

休眠可以通过以下两种方式之一发生

自动

上述速度阈值是相对于与孤岛相关的所有速度的无穷范数（最大绝对值）而言的。在取此范数之前，速度会按元素乘以 mjModel.dof_length，因为旋转速度和移动速度具有不同的单位。平移自由度（DOF）的长度为 1；旋转自由度的长度对应于其关联几何体的平均长度。因此 sleep_tolerance 的单位为 [长度/时间]。

当孤岛被置于休眠状态时，其相关速度被设置为 0。因此，在任何将孤岛置于休眠状态的时间步上，所有依赖于速度的量都必须在利用 mj_forwardSkip 传播休眠状态之前进行重算。

如果孤岛中的任何树具有“从不”休眠策略，则整个孤岛无法休眠。

初始化休眠

通过将树根的 body/sleep 属性设置为“init”，它会被标记为“初始化休眠”，并在 mjData 初始化期间被置于休眠状态。这对于大型模型非常有用，因为等待许多树进入休眠状态可能非常耗时。

由于共享接触或以其他方式处于同一孤岛中的树必须一起休眠，如果孤岛中的某些树被初始化为休眠，则所有树都必须被如此标记。此模型包含一个 XML 示例，由于不满足此条件，将产生编译错误。最后，请注意，初始化休眠功能仅适用于默认配置（不适用于关键帧，请参阅下文讨论）。

唤醒（Waking）#

唤醒发生在时间步的开始，要么在 mj_kinematics 期间，要么在此之后的模拟管线的位置阶段期间。休眠孤岛根据以下标准被唤醒：

  • 用户更改了其关联配置 qpos，例如在模拟暂停时交互式地重新定位配置。

  • 用户将其关联速度 qvel、施加力 qfrc_applied 或 xfrc_applied 设置为非零值，例如在模拟期间交互式地扰动模型时。请注意，检查是通过逐字节与 0 比较执行的，因此将关联元素设置为浮点值 -0.0 将唤醒孤岛，但不会有其他副作用。

  • 它与一棵唤醒状态的树发生了接触。由于接触导致的唤醒会导致碰撞检测两次运行，但仅在发生该情况的时间步。这是为了检测孤岛内部以及孤岛与世界之间的接触，这些接触在它被认为是休眠状态时的第一次运行中被跳过了。

  • 它通过活动的等式约束或受限肌腱与一棵唤醒状态的树相连。

  • 它通过等式约束与不同孤岛中的一棵休眠状态的树相连。要发生这种情况，必须在两棵树都进入休眠时禁用该等式约束。

上述自动唤醒标准旨在使休眠孤岛的行为表现得如同它们处于唤醒状态一样，但这并非总是如此。例如，如果地板上的自由物体进入休眠状态，随后重力反转，它们将保持在原位休眠，直到因其他原因被唤醒。非物理行为最极端的例子是初始化休眠的孤岛。它们可以被放置在半空中或深度碰撞中，但除非被唤醒，否则不会移动。

说明#

休眠执行器

如 body/sleep 文档中所述，带有执行器的树默认不允许休眠，但用户可以覆盖此设置。默认不允许休眠的原因是，一旦执行器被标记为休眠，唤醒它所需的计算就不再执行。即使执行了（即如果总是为所有执行器计算驱动力，而不管其休眠状态如何），此计算也会在加速度/力阶段发生，此时唤醒一棵树已经太晚了，因为唤醒必须在位置阶段发生。因此，如果允许带有执行器的树休眠，则必须如上所述通过触动相关速度或力来手动完成唤醒。

休眠传感器

对于大多数传感器，我们可以在其关联对象处于休眠状态时跳过其数值的计算，直接报告这些对象上次唤醒时计算出的值。有些传感器始终处于唤醒状态，但禁用休眠不会影响它们的计算值：

  • rangefinder 传感器始终处于唤醒状态；它们所附着的位点（site）的休眠状态与报告的值无关。

  • clock 传感器始终处于唤醒状态（没有关联对象）。

  • user 和 plugin 传感器始终处于唤醒状态。

有些传感器始终处于唤醒状态，但禁用休眠可能会影响其计算值。这些传感器显式依赖于接触的存在，而上次唤醒时存在的接触不足以确定其当前值：

  • contact 传感器，如果没有指定对象（匹配所有接触）。

  • contact 传感器，如果其唯一的对象限定符是静态的。

  • contact 传感器，使用了 site 属性。

  • force 或 torque 传感器，附着在静态物体上（例如，地板上的重量传感器）。

临时选择

一些实现选择是临时的，可能会发生变化。

一个具体的例子是硬编码 mjMINAWAKE 的值，而不是将其作为运行时选项暴露给用户。这样做有两个原因。首先，在我们的实验中，我们发现改变该值等同于改变 sleep_tolerance，后者是更有用的旋钮。其次，有人可能会提出以时间单位而不是整数时间步数来衡量休眠时间的语义。在有明确证据表明这些原因中的一个或两个无效之前，我们选择了简单的数值常量。

静态物体（Static bodies）

除了允许运动学树休眠这一主要优化外，休眠功能还包括另一项相关优化：跳过与静态物体相关的计算。如果例如世界体或其静态子对象包含大量几何体，且其位姿只需计算一次，那么这项优化将非常有价值。

这导致了一个微妙（尽管不太可能）的“陷阱”。虽然允许在模拟期间启用休眠，但必须在初始化时或至少经过一次 mj_step 后启用休眠。

// this is OK:
mjData* d = mj_makeData(m);            // sleeping is enabled at init time
mj_step(m, d);
...

// this is also OK:
mjData* d = mj_makeData(m);            // sleeping is disabled at init time
mj_step(m, d);
...
m->opt.enableflags |= mjENABLE_SLEEP;  // enable sleeping after at least one step
mj_step(m, d);

// this is an error:
mjData* d = mj_makeData(m);            // sleeping is disabled at init time
m->opt.enableflags |= mjENABLE_SLEEP;  // enable sleeping
mj_step(m, d);                         // undefined behavior, static elements not computed
被违反的假设

休眠打破了 MuJoCo 核心中内置的几个假设（如果禁用休眠，这些假设仍然成立）。

管线阶段：通常保证在位置阶段结束前不会读取任何与速度相关的量，且在速度阶段结束前不会读取任何与力相关的量。这一位于 mj_step1/mj_step2 分割核心的假设，因在 mj_kinematics 中读取 qvel、qfrc_applied 和 xfrc_applied 而被违反。

紧凑状态：虽然休眠状态理论上由 mjData.tree_asleep 给出，但这是一种假象。一旦孤岛进入休眠状态，mjData 中与之相关的所有位置和速度相关量集合就变成了一种预计算的潜在状态，即“等待孤岛唤醒”。因此，完全保存和恢复带有休眠元素的模拟状态的唯一方法是复制整个 mjData 结构。这也是为什么休眠初始化仅适用于默认配置而不适用于关键帧的原因。请注意，使用标准工具保存和加载状态仍然是有效操作，只是休眠的孤岛会被隐式唤醒。

RK4 积分器

目前不支持 RK4 积分器，这是由于在子步骤中唤醒的复杂性。

坐标系和变换（Coordinate frames and transformations）#

MuJoCo 中使用了多个坐标系。最高级别的区别在于关节坐标系和笛卡尔坐标系。从关节坐标向量到所有物体的笛卡尔位置和方向的映射称为正向运动学，是物理管线的第一步。相反的映射称为逆运动学，但它没有唯一定义，也未在 MuJoCo 中实现。回想一下，切空间之间的映射（即关节速度和力到笛卡尔速度和力）由物体雅可比矩阵给出。

在这里，我们进一步解释坐标系的细微差别和细分，并总结可用的变换函数。在关节坐标系中，唯一的复杂之处在于，由于四元数关节，位置向量 mjData.qpos 与速度和加速度向量 mjData.qvel 和 mjData.qacc 的维度不同。函数 mj_differentiatePos “减去”两个关节位置向量并返回一个速度向量。相反，函数 mj_integratePos 接收一个位置向量和一个速度向量，并返回一个已被给定速度置换的新位置向量。

笛卡尔坐标系更为复杂，因为我们使用了三种不同的坐标系：局部坐标系、全局坐标系和基于质心（com-based）的坐标系。局部坐标系在 mjModel 中用于表示父物体和子物体之间的静态偏移，以及物体与附着在其上的任何几何体、位点、相机和光源之间的静态偏移。这些静态偏移是在任何关节变换之外应用的。因此，mjModel.body_pos、mjModel.body_quat 以及 mjModel 中的所有其他空间量均以局部坐标表示。正向运动学的工作是沿运动学树累积关节变换和静态偏移，并计算全局坐标系中的所有位置和方向。mjData 中以“x”开头的量以全局坐标表示。这些包括 mjData.xpos、mjData.geom_xpos 等。帧方向通常存储为 3x3 矩阵（xmat），但方向也以单位四元数 mjData.xquat 存储的物体除外。给定此物体四元数，附着在该物体上的所有其他对象的四元数可以通过四元数乘法重构。函数 mj_local2Global 将局部物体坐标转换为全局笛卡尔坐标。

位姿（Pose）是 3D 位置和单位四元数方向的组合。没有单独的数据结构；这种组合是逻辑上的。它代表空间中的位置和方向，或者换句话说，空间坐标系。请注意，OpenGL 使用 4x4 矩阵来表示相同的信息，此处我们使用四元数表示方向。函数 mju_mulPose 将两个位姿相乘，这意味着它通过第二个位姿变换第一个位姿（顺序很重要）。mju_negPose 构建相反的位姿，而 mju_trnVecPose 通过位姿变换 3D 向量，如果我们认为位姿是一个坐标系，则将其从局部坐标映射到全局坐标。如果我们只想操作方向部分，可以使用类似的四元数实用函数 mju_mulQuat、mju_negQuat 和 mju_rotVecQuat。

最后是基于质心（com-based）的坐标系。它用于表示 6D 空间向量，包含 3D 角速度、角加速度或力矩，后跟 3D 线性速度、加速度或力。注意顺序：先旋转后平移。mjData.cdof 和 mjData.cacc 就是这类向量的例子；名称以“c”开头。这些向量在多关节动力学计算中起着关键作用。在此不做详细解释，请参阅 Featherstone 关于此主题的优秀幻灯片。通常，用户应避免直接处理此类量。而是使用函数 mj_objectVelocity、mj_objectAcceleration 以及底层的 mju_transformSpatial 来获取给定物体的线性/角速度、加速度和力。不过，对于感兴趣的读者，我们总结了“c”量最不寻常的方面。假设我们想要表示一个物体在原地旋转。人们可能期望空间速度具有非零角速度和零线性速度。但事实并非如此。旋转被解释为围绕穿过坐标系中心（位于物体外部，我们使用运动学树的质心）的轴进行。这样的旋转不仅会旋转物体，还会平移它。因此，空间向量必须具有非零的线性速度，以补偿绕轴外旋转产生的副作用。如果您调用 mj_objectVelocity，产生的 6D 量将在一个以物体为中心且与世界对齐的坐标系中表示。因此，线性分量现在将按预期变为零。此函数还会将平移置于旋转之前，这是我们局部和全局坐标系的惯例。