simulate(1)
在安装试用的时候，我们曾通过 simulate 加载过一个仿真小人，但是没有给任何控制，它就从站立逐渐趴下了。simulate 是官方提供的一个功能齐全的交互式仿真器，它有使用 GFLW 构建窗口，提供 OpenGL 的渲染上下文。 它有两个线程分别用于处理 UI 事件和仿真推演。目前已经有人把右侧官方文档中的视频搬运到B站上了， 视频中加载的模型是一个简化的四足机器人。看上去交互的功能还挺多的。
这份代码很长，但是充分注释了，所以官方建议我们只阅读它。我们就这个 simulate 和四足机器人开始盘，出于文章逻辑和排版的需要，我们在保证不影响逻辑的基础上，对源码做了一些修改。
1. main()
simulate 的入口 main 函数逻辑十分简单，主要是创建了两个线程分别用于可视化和仿真推演。下面是该函数的代码片段。它先检查了头文件和库文件的版本，并尝试加载一些插件。
int main(int argc, char** argv) {
std::printf("MuJoCo version %s\n", mj_versionString());
if (mjVERSION_HEADER != mj_version())
mju_error("Headers and library have different versions");
scanPluginLibraries();
紧接着按照编程套路创建用于可视化的相机和配置项对象，mjvPerturb 是用于保存有关鼠标扰动信息的数据结构， 可以通过 MJCF 的 visual/map 进行配置。
mjvCamera cam; mjv_defaultCamera(&cam);
mjvOption opt; mjv_defaultOption(&opt);
mjvPerturb pert; mjv_defaultPerturb(&pert);
接下来检查运行时通过命令行输入的第一个参数，它记录了 simulate 需要加载的模型文件。如果没有指定文件，也可以在运行时通过鼠标将文件拖拽到 simulate 的窗口中完成加载。 与此同时，还创建了 Simulate 对象 sim 和一个线程 physicsthreadhandle。sim 负责可视化并响应鼠标键盘的交互事件，线程 physicsthreadhandle 则在它的线程函数 PhysicsThread 中完成仿真推演。
const char* filename = (argc > 1) ? argv[1] : nullptr;
auto sim = std::make_unique<mj::Simulate>(std::make_unique<mj::GlfwAdapter>(), &cam, &opt, &pert, false);
std::thread physicsthreadhandle(&PhysicsThread, sim.get(), filename);
最后在主线程中开启 sim 的渲染循环，并 join 物理引擎的线程，退出。
sim->RenderLoop();
physicsthreadhandle.join();
return 0;
}
void PhysicsThread(mj::Simulate* sim, const char* filename) {
if (filename != nullptr) {
sim->LoadMessage(filename);
m = LoadModel(filename, *sim);
if (m) {
const std::unique_lock<std::recursive_mutex> lock(sim->mtx);
d = mj_makeData(m);
}
if (d) {
sim->Load(m, d, filename);
const std::unique_lock<std::recursive_mutex> lock(sim->mtx);
mj_forward(m, d);
free(ctrlnoise);
ctrlnoise = static_cast<mjtNum*>(malloc(sizeof(mjtNum)*m->nu));
mju_zero(ctrlnoise, m->nu);
} else {
sim->LoadMessageClear();
} }
PhysicsLoop(*sim);
free(ctrlnoise);
mj_deleteData(d);
mj_deleteModel(m);
}
2. PhysicsThread()
PhysicsThread() 是仿真推演线程的线程函数，作为顶层的封装，它的结构也很简单。主要逻辑都在函数 PhysicsLoop 开启线程的超级循环完成。 右侧是该函数的代码片段，他有两个输入参数，分别是在 main() 函数中构建的用于渲染的 Simulate 对象和从命令行传入的模型文件名。
如右侧所示，如果运行 simulate 时指定了模型文件，该函数会先加载模型 mjModel，准备运行数据 mjData，其中 m 和 d 分别是指向模型和数据的全局的指针变量。
如果成功构建了模型和数据对象，就通过 Load 接口加载到渲染对象 sim 中。接着调用的这个 mj_forward 好像没啥用，我猜应该是初始化一些系统状态吧。
第13到15行中涉及的这个 ctrlnoise 也是一个全局的指针，其数据类型 mjtNum 在编译时根据宏定义 mjUSEDOUBLE 定义为 double 或者 float 类型。
这里为它分配了 nu 个数据对应着每个控制器。看变量名的字面意思，我们应该可以在仿真过程中通过该数组给控制器添加噪声。
如果一切准备就绪，就会调用函数 PhysicsLoop 开启线程的超级循环。如果退出循环则释放刚刚申请的模型、数据、控制器噪声的内存资源。
3. PhysicsLoop()
实际的物理仿真的逻辑是在这个 PhysicsLoop() 中完成的。它是一个超级循环，其结构如右图折叠代码所示，核心逻辑在第19行中折叠起来的那89行代码中。
该函数有一个输入参数 sim 对应的就是我们在 main 函数中构建的 mj::Simulate 对象。函数的一开始定义了两个局部变量 syncCPU 和 syncSim 分别用于记录 cpu 时间和仿真时间。 然后开启 while 超级循环，循环的退出条件是接收到了退出请求。
超级循环内的逻辑大致上也分了三段。先在图中第 8,9 两行折叠的两段代码中检查并加载模型文件，套路与 PhysicsThread() 的一样。然后通过 C++ 线程库中的 yield 或者 sleep_for 接口， 短暂的让出CPU保证主线程能够运行。最后对 sim.mtx 加锁之后完成第19行那段折叠了89行的仿真推理逻辑。
yield 和 sleep_for 的功能类似，不同的是 yield 只是控制线程让出 CPU 进入就绪状态，系统会再次进行任务调度。而调用 sleep_for 的线程将进入阻塞状态休眠， 在休眠的这段时间是不会参与系统资源竞争的，休眠结束后线程将再次进入就绪状态。在这里它们的作用都是让出 CPU，只是 sleep_for 会多等待一段时间。 当系统再次调度到该线程的时候，就会运行接下来的仿真推理。
在开启仿真推理之前先对递归互斥量上锁。这个递归互斥量，可以在同一个线程中被多次上锁，相应地上了多少次锁就应该解多少次。当它被一个线程上锁之后， 其它线程再调用 lock 获取该互斥量就会进入阻塞状态。下面，我们来详细看一下推理过程。
if (!sim.run) {
mj_forward(m, d);
sim.speed_changed = true;
continue;
}
如右侧的代码片段所示，在推理的一开始，如果 sim.run 为0，表示暂停仿真，此时会 mj_forward 完成正动力学的解算，
主要是更新渲染和关节参数。mj_forward 与我们在编程套路中见到的 mj_step 功能类似，
只是缺少了数值积分的动作，不会更新系统状态，推进仿真时间。
bool stepped = false;
const auto startCPU = mj::Simulate::Clock::now();
const auto elapsedCPU = startCPU - syncCPU;
double elapsedSim = d->time - syncSim;
左侧的代码片段中，定义了局部变量 stepped 用于标记是否通过 mj_step 推进了仿真时间。startCPU 用于记录当前的系统时间。syncCPU 和 syncSim 是两个全局变量记录了上次更新时的系统时间和仿真时间， 所以 elapsedCPU 和 elapsedSim 分别表示距离上次更新逝去的系统时间和仿真时间。
如下面的代码所示，如果 sim.ctrl_noise_std 非零，就会通过Ornstein-Uhlenbeck随机过程插入控制噪声。
据说这是一种均值回归过程，描述的是物理系统在受到随机外力的作用下回归平衡状态的过程。在物理引擎中常用来模拟诸如粒子扩散的自然现象。
具体的数学物理意义我目前不清楚，准备后续在Ornstein-Uhlenbeck随机过程专门分析。
if (sim.ctrl_noise_std) {
// convert rate and scale to discrete time (Ornstein–Uhlenbeck)
mjtNum rate = mju_exp(-m->opt.timestep / mju_max(sim.ctrl_noise_rate, mjMINVAL));
mjtNum scale = sim.ctrl_noise_std * mju_sqrt(1-rate*rate);
for (int i=0; inu; i++) {
ctrlnoise[i] = rate * ctrlnoise[i] + scale * mju_standardNormal(nullptr);
d->ctrl[i] = ctrlnoise[i];
} }
下面的局部变量 slowdown 用于描述仿真时间相对于系统时间被放慢了多少倍。其中 sim.percentRealTime 是定义在类 Simulate 中的一个静态数组，
以百分比的形式记录了仿真时间相对比系统时间的倍速关系。misaligned 用来评价仿真过程是否同步。先将系统时间缩放到仿真时间上，然后计算两者的偏差量，如果超出了全局变量 syncMisalign 设定的阈值，
就判定为不同步。不同步的可能的原因有很多，表现出来的效果应该是仿真过程卡了。
// requested slow-down factor
double slowdown = 100 / sim.percentRealTime[sim.real_time_index];
// misalignment condition: distance from target sim time is bigger than syncmisalign
bool misaligned = mju_abs(Seconds(elapsedCPU).count()/slowdown - elapsedSim) > syncMisalign;
如果因为各种各样的原因发现仿真卡了，就赶紧重置时间，并执行一次 mj_step 推进一步。保证下次迭代的时候是同步上的。
if (elapsedSim < 0 || elapsedCPU.count() < 0 || syncCPU.time_since_epoch().count() == 0 || misaligned || sim.speed_changed) {
syncCPU = startCPU; syncSim = d-<time;
sim.speed_changed = false;
// run single step, let next iteration deal with timing
mj_step(m, d);
stepped = true;
}
如果还能还能跟上，就在一个 while 循环中不断地执行 mj_step 推进仿真时间，直到仿真时间超过了缩放后的系统时间，并且达到 UI 界面的刷新周期。 局部变量 prevSim 是在该 while 循环之前记录的仿真时间。如果发现当前的仿真时间倒流了，那一定是系统重置了，将中止此次迭代。在下次迭代时大概率不会通过仿真同步的判定条件， 进而执行上面的 if 分支的语句重置时间。
else {
bool measured = false;
mjtNum prevSim = d->time;
double refreshTime = simRefreshFraction/sim.refresh_rate;
// step while sim lags behind cpu and within refreshTime
while (Seconds((d->time - syncSim)*slowdown) < mj::Simulate::Clock::now() - syncCPU &&
mj::Simulate::Clock::now() - startCPU < Seconds(refreshTime)) {
// 省略相关代码 measure slowdown before first step
// call mj_step
mj_step(m, d); stepped = true;
// break if reset
if (d->time < prevSim) break;
} }
最后，如果在迭代的过程中推进了仿真时间，就通过 sim.AddToHistory 记录下来。
if (stepped)
sim.AddToHistory();