编程套路
一般在仿真系统中，有专门的图形引擎负责可视化和传感器数据模拟的工作，物理引擎负责正逆动力学的推演。MuJoCo 作为一个通用的物理引擎，提供了一系列的 API 接口， 用来加载模型并推演仿真过程，可以完全脱离 GUI 界面运行。我们在安装试用的时候就已经见识到了这样的一个极简 demo。 本文中，我们从这个极简的 demo 出发，结合官方文档研究一下 MuJoCo 编程的套路。
#include <mujoco/mujoco.h>
#include <iostream>
char error[1000];
int main()
{
mjModel *m = mj_loadXML("hello.xml", NULL, error, 1000);
mjData *d = mj_makeData(m);
while(d->time < 10)
mj_step(m, d);
mj_deleteData(d);
mj_deleteModel(m);
return 0;
}
1. 入门套路
如右侧代码所示，整个仿真过程分为了初始化、推演、释放三段。 一开始通过接口 mj_loadXML 和 mj_makeData
分别实例化了模型 mjModel 和数据 mjData 对象。
然后在一个 while 循环中调用 mj_step 推演了 10 秒的仿真过程。最后释放掉模型和数据对象。
mjModel 和 mjData 是整个仿真过程中的两个主要数据结构，分别管理了目标系统模型以及仿真过程的中间数据。它们都是非常复杂的数据结构，有专门的 API 接口负责资源分配和初始化。 为了提高仿真运行效率，降低频繁分配和释放资源带来的内存碎片化的影响，优化 GPU 加速时内存搬运效率，MuJoCo 在初始化的时候就会申请一个足够大的堆空间，为所有可能的中间结果预先分配内存， 在仿真结束时一次释放。这对于一个仿真要素很多的复杂目标系统而言是一项必要的优化策略。
在右侧的 demo 中，整个仿真过程是通过不断调用 mj_step 来完成的。这个函数主要做了正动力学计算和数值积分两件事情。
正动力学是在给定系统状态和控制量的情况下计算当前时刻的加速度。
在 Mujoco 中当前时刻有 d->time 记录，推算出的加速度被保存在 d->qacc 中。
这里我们并没有修改任何控制量，所以模拟的是一个被动的动力学。我们可以通过修改 while 循环，在调用 mj_step 之前完成控制量的修改 d->ctrl，
或者注册一个控制的回调函数到 mjcb_control 上，来模拟一些控制效果。
数值积分是根据当前系统的状态和正动力学解算出的加速度推算下一时刻的状态，也就是把仿真时间从 d->time
推进到 d->time + m->opt.timestep。这里的 timestep 就是仿真步长，
可以在MJCF模型文件中通过
compiler → lengthrange → timestep 标签设置。
2. 可视化套路
#include <mujoco/mujoco.h>
#include <GLFW/glfw3.h>
#include <stdio.h>
#include <string.h>
mjModel* m = NULL; // MuJoCo model
mjData* d = NULL; // MuJoCo data
mjvCamera cam; // abstract camera
mjvOption opt; // visualization options
mjvScene scn; // abstract scene
mjrContext con; // custom GPU context
虽然可视化和图形渲染并不是物理引擎的主要职责，但它们是我们开发调试的最有力的工具，所以 MuJoCo 还提供了一系列的 3D 可视化接口。MuJoCo 的可视化过程有填充场景和 OpenGL 渲染两个阶段。 MuJoCo 在概念设计上对两个阶段进行了区分，填充场景的阶段不需要任何图形库。官方将这个过程称为抽象可视化工具。用户选择基于 OpenGL 的图形引擎时仍然可以使用这一工具。
填充场景的阶段，主要根据几何对象、灯光、相机等各种仿真要素来填充数据结构 mjvScene。该阶段还可以通过回调函数来响应用户的键盘、鼠标等交互行为。相关的数据结构和函数名都有前缀 mjv。 OpenGL 渲染的阶段则是根据上一阶段填充的 mjvScene 数据结构，调用 OpenGL 的接口完成帧缓冲区的访问和 2D 图像的绘制。MuJoCo 通过一系列以 mjr 为前缀的数据结构和函数封装了 OpenGL 的操作， 因此大多数情况下我们不需要关心它。
官方提供了一个最小的交互式模拟器 basic。 它使用 GLFW 库构建 OpenGL 窗口和上下文，以 60 fps 的帧率推进仿真并完成渲染。 该例程还提供了简单的鼠标和键盘交互，可以拖拽控制相机视角，也可以通过 Backspace 键重置模型。 我们对该例程做了一些简化，重点突出可视化过程中的 mjvScene。
int main(int argc, const char** argv) {
char error[1000];
m = mj_loadXML("hello.xml", NULL, error, 1000);
d = mj_makeData(m);
GLFWwindow* window = InitWindow();
while (!glfwWindowShouldClose(window)) {
mjtNum simstart = d->time;
while (d->time - simstart < 1.0/60.0)
mj_step(m, d);
DrawOneFrame(window);
}
在上面右侧的代码片段中，我们引入了必要的头文件，同时定义了6个全局变量。其中指针 m 和 d 分别是我们的仿真模型和数据对象。 scn 就是第一阶段要填充的 mjvScene 对象。渲染一幅图像除了场景之外还需要提供一个相机，不同的相机参数和视角可以产生不同的图像，因此还需要一个抽象的相机对象 cam。 opt 是可视化相关的配置。con 则是第二阶段进行 OpenGL 渲染时的上下文。
在左侧的代码片段中，我们仍然按照入门套路那样，分别通过 mj_loadXML 和 mj_makeData 构造仿真模型和数据对象。 然后调用了一个函数 InitWindow 用来构建 GLFW 窗口、完成全局变量 cam, opt, scn, con 的初始化、创建场景和渲染上下文的工作。
完成初始化工作后，在一个 while 循环中进行仿真推演并逐帧渲染图像。因为已经有 GLFW 的窗口在运行，所以第7行通过判定该窗口是否关闭，来决定是否退出循环。 在第9行还有一个内部的 while 循环，通过 mj_step 把仿真时间推进 1/60 s。然后调用函数 DrawOneFrame 完成一帧图像的渲染。 下面分别是 InitWindow() 和 DrawOneFrame() 的实现，过程都比较简单，这里不再展开。
GLFWwindow * InitWindow() {
if (!glfwInit())
mju_error("无法初始化 GLFW");
GLFWwindow* window = glfwCreateWindow(1200, 900, "Demo", NULL, NULL);
glfwMakeContextCurrent(window);
glfwSwapInterval(1);
// initialize visualization data structures
mjv_defaultCamera(&cam);
mjv_defaultOption(&opt);
mjv_defaultScene(&scn);
mjr_defaultContext(&con);
// create scene and context
mjv_makeScene(m, &scn, 2000);
mjr_makeContext(m, &con, mjFONTSCALE_150);
return window;
}
void DrawOneFrame(GLFWwindow* window)
{
// get framebuffer viewport
mjrRect viewport = {0, 0, 0, 0};
glfwGetFramebufferSize(window, &viewport.width, &viewport.height);
// update scene and render
mjv_updateScene(m, d, &opt, NULL, &cam, mjCAT_ALL, &scn);
mjr_render(viewport, &scn, &con);
glfwSwapBuffers(window);
glfwPollEvents();
}
3. 关于系统状态与控制量的套路
在现代控制理论中，人们喜欢用一阶微分方程来描述动力学系统，如果碰巧是个线性的系统就可以写成如下的矩阵形式。其中 \(\boldsymbol{x}\) 是系统的状态向量， \(\boldsymbol{u}\) 是系统的控制量。状态向量是一个与时间相关的量，系统在 \(t\) 时刻的状态完全由系统的初识状态 (\(t = t_0\)) 和各时刻 (\(t ≥ t_0\)) 的控制量唯一确定。
$$ \dot{\boldsymbol{x}} = A \boldsymbol{x} + B \boldsymbol{u} $$线性系统是一种比较理想的情况，如下式所示，MuJoCo 针对更一般的情况，用一个函数 \(f\) 来表示一阶微分关系。
这是一个连续时间的公式，MuJoCo 的正动力学 mj_forward 就是建立在这个基础上的。
在 MuJoCo 中系统的状态向量由 x = (mjData.time, mjData.qpos, mjData.qvel, mjData.act) 构成，
其一阶导为 dx/dt = (1, mjData.qvel, mjData.qacc, mjData.act_dot)。
对于一个二阶的系统，只有系统状态通常只有位置(mjData.qpos)和速度(mjData.qvel)，但是 MuJoCo 还允许我们通过 mjData.act 集成一些自定义的状态来支持气缸、肌肉等系统的建模。 mjData.act_dot 则是这些自定义状态的一阶导。另外需要注意的是，MuJoCo 采用四元数来描述自由关节或者球关节， 所以它的位置矢量要比速度矢量多一个维度，所以 \(f(t, \boldsymbol{x}, \boldsymbol{u})\) 并不是简单的一阶导。
MuJoCo 中的控制量是 u = (mjData.ctrl, mjData.qfrc_applied, mjData.xfrc_applied)。这些变量依次表示：模型中定义的执行器的控制信号、
关节空间的力和力矩、笛卡尔空间的力和力矩。
// copy simulation state
dst->time = src->time;
mju_copy(dst->qpos, src->qpos, m->nq);
mju_copy(dst->qvel, src->qvel, m->nv);
mju_copy(dst->act, src->act, m->na);
// copy mocap body pose and userdata
mju_copy(dst->mocap_pos, src->mocap_pos, 3*m->nmocap);
mju_copy(dst->mocap_quat, src->mocap_quat, 4*m->nmocap);
mju_copy(dst->userdata, src->userdata, m->nuserdata);
// copy warm-start acceleration
mju_copy(dst->qacc_warmstart, src->qacc_warmstart, m->nv);;
如果我们有两个 mjData 的对象 src 和 dst，它们对应同一个 mjModel，那么我们可以通过类似右侧的代码把仿真状态从 src 拷贝到 dst 中。
MuJoCo 中还有函数 mj_copyData 用来搬运整个 mjData，但是速度要慢很多。
关于控制量的设置，MuJoCo 推荐的方法是我们提供一个类似如下代码的控制函数，并注册到 mjcb_control = mycontroller 上。
这样在每次调用 mj_step 进行推演时，MuJoCo 可以根据需要自行调用。
void mycontroller(const mjModel* m, mjData* d) {
// clear controls and applied forces
mju_zero(dst->ctrl, m->nu);
mju_zero(dst->qfrc_applied, m->nv);
mju_zero(dst->xfrc_applied, 6*m->nbody);
};
当然，我们也可以不按推荐的方式来，直接在 mj_step 之前修改控制量。因为我们是在离散的时间中推演整个仿真过程，这就使得控制量和系统的状态变量之间的关系比较微妙。 一些仿真的中间结果可能在调用 mj_step 之前是不可用的，我们仍坚持这样做可能改变数据之间的因果关系。为了缓解这一问题，MuJoCo 还把正动力学和数值积分两部分拆成了两段， 如下面右侧的代码所示，我们可以在两段之间更新控制量。
while( d->time<10 ) {
// set d->ctrl or d->qfrc_applied or d->xfrc_applied
mj_step(m, d);
};
while( d->time<10 ) {
mj_step1(m, d);
// set d->ctrl or d->qfrc_applied or d->xfrc_applied
mj_step2(m, d);
};