扩展#

本节介绍 MuJoCo 用于用户自定义扩展的机制。目前,扩展性通过 引擎插件资源提供者 实现。

引擎插件#

引擎插件于 MuJoCo 2.3.0 引入,允许将用户定义的逻辑插入到 MuJoCo 计算管线的各个部分。例如,自定义传感器和执行器类型可以作为插件实现。插件特性在 MJCF 模型的 XML 内容中被引用,这使得即使仿真需求超出了 MuJoCo 的内置能力,MJCF 仍然可以保持系统的抽象物理描述。

插件机制旨在克服 MuJoCo 物理回调 的缺点。这些全局回调 (使用示例) 仍然可用,对于快速原型设计或用户希望在 Python 中实现功能时很有用,但作为扩展功能的稳定机制已被普遍弃用。插件机制的核心特性是

  • 线程安全:插件实例(见下文)是线程本地的,避免冲突。

  • 状态保持:插件可以是状态保持的,并且它们的状态将正确地被(反)序列化。

  • 互操作性:不同的插件可以共存而不互相干扰。

插件的用户和开发者都应该熟悉两个关键概念

插件

一个插件是实现其功能的函数和静态属性的集合,打包到一个 mjpPlugin 结构体中。插件函数是无状态的:它们只依赖于传递给它们的参数。当一个插件需要内部状态时,它会声明这个状态并允许 MuJoCo 来管理它并将其传入。这使得完整的仿真状态可以被(反)序列化。因此,插件可以被视为功能的“纯逻辑”部分,并且通常被打包为 C 库。插件既不是模型元素,也不与特定的模型元素相关联。

插件实例

一个插件实例代表由插件操作的自包含运行时状态:当插件逻辑执行时,引擎会将实例状态传入。插件实例本身就是类型为 mjOBJ_PLUGIN 的模型元素。存在 id 范围在 [0 nplugin-1]mjModel.nplugin 个实例。与其他元素一样,实例可以有名称,通过 mj_name2idmj_id2name 在 id 和名称之间进行映射。与插件代码一次性加载到全局表中不同,可以定义同一插件的多个实例,并且它们与其他模型元素之间存在一对多的关系。

一对一

在这种最简单的情况下,模型中每个实例只被引用一次。例如,两个传感器可以声明它们的值由同一插件的两个插件实例计算。在这种情况下,每次计算传感器输出时,插件逻辑都会单独执行。

一对多

或者,多个元素的行为可以由单个插件实例支持。这在以下两种主要场景中很有用

  • 不同元素类型的值与同一物理实体和计算相关联。例如,考虑一个带有内置温度计的电机。这将表现为一个执行器和一个传感器,两者都与同一个插件实例关联,该实例计算扭矩输出和温度读数。

  • 将多个相关元素的计算批量处理在一起是有益的,例如计算值是神经网络的输出。这里的典型例子是一个配备了 N 个电机的机器人,其中电机动力学被建模为一个神经网络。在这种情况下,通过一次前向传播产生所有 N 个执行器的扭矩输出,比分别为每个电机单独计算要快得多。

下面,我们首先从用户角度描述插件

  • 插件功能的类型。

  • 如何在 MJCF 模型中声明和配置插件。

  • 如何将插件状态整合到 mjData 中,以及当存在插件实例时,用户需要做什么来安全地复制和序列化 mjData 结构体。

接下来,我们将描述插件注册的相关流程,这与插件的用户和开发者都相关。随后是面向插件开发者的部分。

插件功能#

插件通过其关联的 mjpPlugin 结构体的内容来描述。成员 capabilityflags 是一个整数位字段,描述了插件的功能,其中位语义在枚举 mjtPluginCapabilityBit 中定义。使用位字段允许插件支持多种类型的计算。目前支持的插件功能有

  • 执行器插件

  • 传感器插件

  • 被动力插件

  • 符号距离场插件

未来将根据需要添加更多功能。

在 MJCF 中声明#

首先,必须通过 <extension><plugin> 声明插件依赖。解析模型时,如果声明了任何插件但未注册(见下文),将引发模型编译错误。如果只有一个 MJCF 元素由插件支持,则可以在原地隐式创建实例。如果多个元素由同一插件支持,则必须显式声明实例

<mujoco>
  <extension>
    <plugin plugin="mujoco.test.simple_sensor_plugin"/>
    <plugin plugin="mujoco.test.actuator_sensor_plugin">
      <instance name="explicit_instance"/>
    </plugin>
  </extension>
  ...
  <sensor>
    <plugin name="sensor0" plugin="mujoco.test.simple_sensor_plugin"/>
    <plugin name="sensor1" plugin="mujoco.test.simple_sensor_plugin"/>
    <plugin name="sensor2" instance="explicit_instance"/>
  </sensor>
  ...
  <actuator>
    <plugin name="actuator2" instance="explicit_instance"/>
  </actuator>
</mujoco>

在上面的示例中,sensor0sensor1 都由一个简单的插件支持,该插件不共享元素之间的计算,因此通过直接引用插件标识符为每个传感器隐式创建一个实例。相比之下,sensor2actuator2 由一个共享计算的插件支持,因此它们必须引用一个显式声明的共享实例。

在 MJCF 中配置#

插件可以声明自定义属性,这些属性代表专门的可配置参数。例如,一个直流电机模型可以将电阻、电感和电容作为配置属性暴露。在 MJCF 中,这些属性的值可以通过 <config> 元素指定,每个 <config> 都有一个键和一个值。有效的键和值由插件开发者指定,但在插件注册时向 MuJoCo 声明,以便 MuJoCo 模型编译器可以对无效值引发错误。

<mujoco>
  <extension>
    <plugin plugin="mujoco.test.simple_actuator_plugin">
      <instance name="explicit_instance">
        <config key="resistance" value="1.0"/>
        <config key="inductance" value="2.0"/>
      </instance>
    </plugin>
  </extension>
  ...
  <actuator>
    <plugin name="actuator0" instance="explicit_instance"/>
    <plugin name="actuator1" plugin="mujoco.test.simple_actuator_plugin">
        <config key="resistance" value="3.0"/>
        <config key="inductance" value="4.0"/>
    </plugin>
  </actuator>
</mujoco>

在上面的示例中,actuator0 引用了一个通过 <instance> 元素创建和配置的现有插件实例,而 actuator1 是在原地隐式创建和配置一个新的插件实例。请注意,直接将 <config> 子元素添加到 actuator0 将是一个错误,因为那里没有创建新的插件实例。

插件状态#

虽然插件代码应该是无状态的,但单个插件实例被允许持有随 MuJoCo 物理学一起演化的时间依赖状态,例如热力学耦合执行器模型中的温度变量。另外,插件实例也可能需要记忆其操作中潜在耗时的部分。例如,由预训练神经网络支持的传感器或执行器插件会在模型编译时预加载其权重。区分这两种类型的按实例插件负载对我们来说很重要。术语插件状态指插件实例的时间依赖状态,该状态由浮点值组成,而术语插件数据指由记忆负载组成的任意数据结构,应视为插件计算的实现细节。

关键的是,插件数据必须仅能从插件配置属性、插件状态和 MuJoCo 状态变量 重建。这意味着插件数据预计是不可序列化的,并且在 MuJoCo 复制或存储数据时不会被序列化。另一方面,插件状态被认为是物理学不可或缺的一部分,并且必须与 MuJoCo 的其他状态变量一起序列化,以便能够忠实地恢复物理状态。

插件必须通过其 mjpPlugin 结构体的 nstate 回调函数声明每个实例所需的浮点值数量。注意,这个数量可以取决于实例的具体配置。在 mj_makeData 期间,MuJoCo 为每个插件实例在 mjDataplugin_state 字段中分配所需的槽位数量。mjModel 中的 plugin_stateadr 字段指示了每个插件实例可以在整个 plugin_state 数组中找到其状态值的位置。

然而,从 MuJoCo 的角度来看,插件数据是完全不透明的。在 mj_makeData 期间,MuJoCo 会调用相关 mjpPlugininit 回调。在这个回调中,插件可以分配或以其他方式创建一个其运行所需的任意数据结构,并将其指针存储在正在创建的 mjDataplugin_data 字段中。在 mj_deleteData 期间,MuJoCo 会调用同一 mjpPlugindestroy 回调,插件负责释放与其实例相关的内部资源。

当通过 mj_copyData 复制 mjData 时,MuJoCo 将复制插件状态。但是,插件代码负责为新复制的 mjData 设置插件数据。为了方便这一点,MuJoCo 会为每个存在的插件实例调用 mjpPlugincopy 回调。

执行器状态#

编写有状态执行器插件时,有两种选择来保存执行器状态。一种选择是使用上面描述的 plugin_state,另一种是通过实现 mjpPlugin 上的回调来使用 mjData.act

使用后一种选项时,执行器插件的状态将被添加到 mjData.act 中,并且 MuJoCo 将在时间步之间自动积分 mjData.act_dot 值。这种方法的一个优点是像 mjd_transitionFD 这样的有限差分函数将像对原生执行器一样工作。mjpPlugin.advance 回调将在 act_dot 被积分后调用,如果内置积分器不合适,执行器插件可以在此时覆盖 act 值。

用户可以在执行器插件上指定 dyntype 属性,以在用户输入和执行器状态之间引入滤波器或积分器。这样做时,由 dyntype 引入的状态变量将放置在 act 数组中插件的状态变量之后

注册#

插件必须先在 MuJoCo 中注册,才能在 MJCF 模型中引用它们。

旨在支持特定应用的一次性插件(或为帮助排除模型问题而实现的临时插件)可以静态链接到应用中。这可以像在 main 函数中准备一个 mjpPlugin 结构体,然后将其传递给 mjp_registerPlugin 以在 MuJoCo 中注册一样简单。

通常,可复用插件预计会打包为动态库。包含一个或多个 MuJoCo 插件的动态库应确保在库加载时所有插件都已注册。在使用 GCC 兼容编译器时,这可以通过在声明有 __attribute__((constructor)) 的函数中调用 mjp_registerPlugin 来实现,而在 MSVC 中,这可以在 DLL 入口点(通常称为 DllMain)中完成。MuJoCo 提供了一个便捷宏 mjPLUGIN_LIB_INIT,它根据所使用的编译器展开为这些构造中的一个。

上述以动态库形式提供的插件用户可以使用函数 mj_loadPluginLibrary 加载库。这是加载包含 MuJoCo 插件的动态库的首选方式(而不是直接调用 dlopenLoadLibraryA),因为 MuJoCo 期望动态库自动注册插件的具体方式可能会随时间变化,但 mj_loadPluginLibrary 预计也会随之演进以反映最佳实践。

对于需要加载任意用户提供的 MJCF 模型的应用,最好能够自动扫描并加载指定目录下的所有动态库。用户携带需要插件的 MJCF 文件时,可以被告知将所需的插件库放置在相关目录中。例如,simulate 交互式查看器应用就是这样做的。为此类扫描和加载用例提供了 mj_loadAllPluginLibraries 函数。

编写插件#

本节面向开发者,尚未完成。我们鼓励希望编写自己插件的人联系 MuJoCo 开发团队寻求帮助。对于经验丰富的开发者来说,一个好的起点是 相关的测试第一方插件目录 中的第一方插件。

本节的未来版本将包括

  • mjpPlugin 结构体的内容。

  • 为了定义一个插件需要提供哪些函数和属性。

  • 如何为一个插件声明自定义 MJCF 属性。

  • 开发者需要记住的事项,以确保当 mjData 被复制、步进或重置时,插件能正常工作。

有一些第一方插件目录

actuator#

actuator/ 目录中的插件实现了自定义执行器,目前只有一个 PID 控制器。详情请参阅 README

elasticity#

elasticity/ 目录中的插件是基于连续介质力学的被动力,适用于一维和二维物体。一维模型在旋转下保持不变,并捕捉弹性电缆的大变形,解耦扭转和弯曲应变。二维模型适用于计算薄弹性板(即具有平面无应力构型的壳体)的弯曲刚度。在这种情况下,弹性势能是二次的,因此刚度矩阵是常数。更多信息,请参阅 README

sensor#

sensor/ 目录中的插件实现了自定义传感器。目前唯一的传感器插件是触控网格传感器,详情请参阅 README

sdf#

sdf/ 目录中的插件以无网格方式指定自定义形状,通过定义计算查询点处的符号距离场及其梯度的方法。然后,这种形状在 engine_collision_driver.c 顶部的碰撞表中作为一种新的 geom 类型。有关可用 SDF 以及如何编写自己的隐式几何体的更多信息,请参阅 README。本节的其余部分将提供有关碰撞算法和插件引擎接口的更多详细信息。

碰撞点通过梯度下降法,最小化函数 A + B + abs(max(A, B)) 来找到,其中 A 和 B 是两个碰撞的 SDF。由于 SDF 是非凸的,需要多个起始点才能收敛到多个局部最小值。起始点数量使用 sdf_initpoints 设置,并使用 Halton 序列在轴对齐边界框的交集内初始化。梯度下降迭代次数使用 sdf_iterations 设置。

虽然首选精确 SDF——编码到表面的精确符号距离,但与任何在表面处值为零且向外单调增长,内部带有负号的函数也可以发生碰撞。对于这类函数,仍然可能找到碰撞,尽管起始点数量可能会增加。

编译器调用 sdf_distance 方法,使用 MarchingCubeCpp 实现的行进立方体算法生成用于渲染的可视化网格。

未来对梯度下降算法的改进,例如利用 SDF 特性的线搜索,可能会减少迭代次数和/或起始点数量。

对于 sdf 插件,需要指定以下方法

sdf_distance:

返回在局部坐标下给定的查询点的符号距离。

sdf_staticdistance:

这是前一个函数的静态版本,将配置属性作为附加输入。需要此函数是因为在插件对象实例化之前,网格创建发生在模型编译期间。

sdf_gradient:

计算查询点处 SDF 在局部坐标下的梯度。

sdf_aabb:

计算在局部坐标下的轴对齐边界框。在调用行进立方体算法之前,将此体积均匀体素化。

资源提供者#

资源提供者扩展 MuJoCo,以加载不一定来自操作系统文件系统或虚拟文件系统 (mjVFS) 的资源(XML 文件、网格、纹理等)。例如,从互联网下载资源可以作为资源提供者实现。这些扩展在 MuJoCo 中通过 mjResource 结构体进行抽象处理。

概述#

创建新的资源提供者是通过在全局表中通过 mjp_registerResourceProvider 注册一个 mjpResourceProvider 结构体来实现的。一旦注册了资源提供者,它就可以被所有加载函数使用。mjpResourceProvider 结构体存储三种类型的字段

资源前缀

资源通过其名称中的前缀来标识。所选前缀应具有有效的 统一资源标识符 (URI) 方案语法。资源名称也应具有有效的 URI 语法,但这并未强制执行。名称语法为 {prefix}:{filename} 的资源将匹配使用方案 prefix 的提供者。例如,一个通过互联网访问资源的资源提供者可能会使用 http 作为其方案。在这种情况下,名称为 http://www.example.com/myasset.obj 的资源将与该资源提供者匹配。方案不区分大小写,因此 HTTP://www.example.com/myasset.obj 也将匹配。注意冒号的重要性。URI 语法要求资源名称中的前缀后跟一个冒号,以便与方案匹配。例如,https://www.example.com/myasset.obj 将不匹配,因为方案指定为 https

回调函数

资源提供者必须实现三个回调函数:openreadclose。另外两个回调函数 getdirmodified 是可选的。下面将详细介绍这些回调函数。

数据指针

最后,还有一个不透明的数据指针,供提供者将数据传递给回调函数。该数据指针在给定模型中是常量。

资源提供者通过回调函数工作

  • mjfOpenResource:open 回调接受一个类型为 mjResource 的参数。应使用资源的 name 字段验证资源是否存在,并用资源所需的任何额外信息填充资源 data 字段。失败时,此回调应返回 0 (false),否则返回 1 (true)。

  • mjfReadResource:read 回调接受一个 mjResource 和一个指向 void 指针(称为 buffer)的指针作为参数。read 回调应将 buffer 指针指向可以读取资源字节的位置,并返回 buffer 指向的字节数。失败时,此回调应返回 -1。

  • mjfCloseResource:此回调接受一个类型为 mjResource 的参数,应用于释放提供的资源中 data 字段分配的任何内存。

  • mjfGetResourceDir:此回调是可选的,用于从资源名称中提取目录。例如,资源名称 http://www.example.com/myasset.obj 的目录为 http://www.example.com/

  • mjfResourceModified:此回调是可选的,用于检查现有的已打开资源是否已从其原始来源修改。

使用方法#

注册资源提供者后,可以立即用于打开资源。如果资源文件名具有与已注册提供者前缀匹配的前缀,则将使用该提供者加载资源。

示例#

本节提供了一个读取 data URI 方案 的资源提供者的基本示例。首先我们实现回调函数

int str_open_callback(mjResource* resource) {
  // call some util function to validate
  if (!is_valid_data_uri(resource->name)) {
    return 0; // return failure
  }

  // some upper bound for the data
  resource->data = mju_malloc(get_data_uri_size(resource->name));
  if (resource->data == NULL) {
    return 0; // return failure
  }

  // fill data from string (some util function)
  get_data_uri(resource->name, &data);
}

int str_read_callback(mjResource* resource, const void** buffer) {
  *buffer = resource->data;
  return get_data_uri_size(resource->name);
}

void str_close_callback(mjResource* resource) {
  mju_free(resource->data);
}

接下来我们创建资源提供者并在 MuJoCo 中注册它

mjpResourceProvider resourceProvider = {
  .prefix = "data",
  .open = str_open_callback,
  .read = str_read_callback,
  .close = str_close_callback,
  .getdir = NULL
};

// return positive number on success
if (!mjp_registerResourceProvider(&resourceProvider)) {
  // ...
  // return failure
}

现在我们可以将资源作为字符串写入 MJCF 文件中

<asset>
  <texture name="grid" file="grid.png" type="2d"/>
  <mesh content-type="model/obj" file="data:model/obj;base65,I215IG9iamVjdA0KdiAxIDAgMA0KdiAwIDEgMA0KdiAwIDAgMQ=="/>
  ...
</asset>