MuJoCo 仿真引擎

MuJoCo 是一个通用的物理引擎，全称是 Multi-Joint dynamics with Contact。最初由 Roboti LLC 开发，在2021年10月份被 DeepMind 收购，2022年5月开源。

官网: https://mujoco.org/
GitHub仓库: https://github.com/deepmind/mujoco
官方文档: https://mujoco.readthedocs.io/en/stable/overview.html
原 Roboti LLC: https://www.roboti.us/index.html

物理引擎传统上分为两类。机器人和生物力学引擎在广义或关节坐标中使用高效且准确的递归算法。然而，它们要么忽略了接触动力学，要么依赖于早期的弹簧阻尼器方法。此类引擎需要非常小的仿真步长。游戏引擎使用更现代的方法，通过解决优化问题来找到接触力。然而，它们经常采用笛卡尔坐标系，通过数值的方法描述关节约束，当涉及复杂的运动结构时，会导致不准确和不稳定。 MuJoCo 是第一个将两者结合起来的通用物理引擎，广义坐标模拟 + 基于优化的接触动力学。

本系列文章是对 MuJoCo 的源码解读。GitHub 仓库是各篇文章中的例程。我们尽量做到一篇文章对应一个子目录，各个子目录均可以独立编译。为了保证解读的文本和代码一致，我们 fork 了一个源码仓库。

第一部分：入门例程和基本概念

安装试用	本文中，我们先下载官方发布的预编译包试用一下，再源码编译一遍，最后分析了一下它的 cmake 脚本。
编程套路	MuJoCo 作为一个通用的物理引擎，提供了一系列的 API 接口，用来加载模型并推演仿真过程。本文从安装试用中的极简 demo 出发，结合官方文档研究一下 MuJoCo 编程的套路。
simulate(1)	simulate 是官方提供的一个功能齐全的交互式仿真器，它有使用 GFLW 构建窗口，提供 OpenGL 的渲染上下文。它有两个线程分别用于处理 UI 事件和仿真推演。本文介绍仿真推演的线程。
simulate(2)	本文介绍官方仿真器 simulate 的 UI 处理线程。

第二部分：系统模型

根据存储方式和描述等级的不同，在 MuJoCo 里面有四种模型的概念。 MuJoCo 通过上层描述的 MJCF/URDF 文件与我们人类打交道。它允许我们在一个 xml 文件中描述仿真场景。 MuJoCo通过 mj_loadXML 把上层的描述文件加载到内存时，就得到上层的 C++ 数据结构 mjCModel。它只是一个中间结构，仿真时还会被编译成底层的 C 结构 mjModel，配合 mjData 推进整个仿真过程。

初识仿真系统模型	MuJoCo 可以在一个 xml 文件里面通过 MJCF/URDF 格式描述模型，然后通过 `mj_loadXML` 加载到内存中，最终都会转换成一个 mjModel 的 C 语言结构体对象进行仿真。
模型描述的高级数据结构 mjCModel	模型描述的高级数据结构 mjCModel 的成员与 MJCF 格式中定义的各个标签和属性基本上是一一对应的，它包含生成低级模型所需的一切信息。完成 mjCModel 对象的构建之后，就可以调用 Compile 来生成相应的低级模型对象。
模型描述的低级数据结构 mjModel	模型描述的低级数据结构 mjModel 可以说是 MuJoCo 用来跟机器打交道的工具。基本上所有的仿真算法都是基于该数据结构进行的。我们可以通过高级数据结构的 Compile 接口编译得到 mjModel 的对象。
工作空间 mjData	简介。

第三部分：刚体动力学

正运动学 mj_kinematics	从 mj_step→ mj_forward→mj_forwardSkip→mj_fwdPosition→mj_kinematics 一路调用下去，就到了我们要讲的第一个完整的功能函数 mj_kinematics。如其字面意思那样，这是一个正运动学的函数。
质量质心 mj_comPos	mj_fwdPosition 通过 mj_kinematics 更新了各个刚体和关节在世界坐标系下的位姿之后，就要调用函数 mj_comPos 完成质心、惯性张量、以及相对质心运动在世界坐标系下的表示。
复合刚体 mj_crb	正动力学是给定作用力计算刚体系统的加速度，经典的方法有惯性矩阵法和传播算法。本文要介绍的 mj_crb 函数就是一种惯性矩阵法的实现——复合刚体算法(Composite-Rigid-Body)。
惯性矩阵分解 mj_factorM	函数 mj_factorM 将稀疏的惯性矩阵分解成了\(\boldsymbol{L}^T\boldsymbol{D}\boldsymbol{L}\)的形式，其中\(\boldsymbol{L}\)是对角线全为一的下三角矩阵，\(\boldsymbol{D}\)是对角矩阵。

按照函数 mj_fwdPosition 的运行过程，应该调用 mj_collision 进行碰撞检测了。碰撞检测是构建接触约束，求解接触力的重要环节。由于碰撞检测是一个比较复杂的子系统，我们将专门用一个部分介绍它，然后回来继续研究约束力的计算。

第四部分：碰撞检测

碰撞检测的需求描述很简单，就是其字面意思，找出系统中发生碰撞的元素。如果暴力穷举出所有可能发生碰撞的刚体对，将是一个\(O(n^2)\)计算复杂度的任务。比较常见的处理方式就是，从 Broad-Phase 到 Mid-Phase 到 Narrow-Phase 依次从粗到细的检查。Mujoco 也是如此。

碰撞检查是一个较为复杂的子系统，一方面要顾及计算效率，另一方面要处理各种各样的几何物体。有时用户可能还会指定某些特定的物体不参与检测，此外 mujoco 还要模拟复杂的柔体(flexable objects)。为此 mujoco 的引擎中集成了一个 engine_collision_driver。本部分我们先专注于刚体的碰撞检测，从其入口函数 mj_collision 出发，深入到各个细节。至于柔体的检测将放置到第XXX部分中作更深入的分析。

碰撞检测	本文分析碰撞检测的入口函数 mj_collision。它先采用一种扫描剪枝(Sweep-And-Prune)算法生成候选碰撞对，经过四种过滤器筛选之后，根据几何体类型生成详细的碰撞信息。
扫描剪枝算法 Sweep-And-Prune	扫描剪枝算法采用轴对齐包围盒子(AABB)简化几何形状，将 3D 的几何体投影到一维的数轴上，充分利用数据的排序关系过滤了大多数不可能碰撞的几何体。
层次空间分割 BVH
MPR检测算法 Minkovski Portal Refinement

第XXX部分：柔体(flexable objects)

先挖一铲子

简介

第六部分：肌腱

再挖一铲子

简介

第六部分：可视化相关

又挖一铲子

简介

第X部分：其他

数据栈空间	mjData 的数据栈空间
mocap	mocap? 运动捕捉? 啥玩意?

第X部分：原理

开发动机与总体框架	本文是对Computation中 Motivation 和 General Framework 两个部分的翻译和整理。总体了解一下 MuJoCo 的设计逻辑。
约束模型和求解器	本文是对Computation中 Constraint Model 和 Constraint Solver 两个部分的翻译和整理。

作者在Computation的最后罗列了一些参考文献，并且详细解释了这些参考文献的对 Mujoco 的影响，是一份不错的书单。

目前 Mujoco 是按照E. Todorov. Convex and analytically-invertible dynamics with contacts and constraints: Theory and implementation in MuJoCo. 2014实现的。

机器人运动学和动力学的递归算法有着很长历史。 R. Featherstone. Rigid Body Dynamics Algorithms. Springer, 2008. 是一个经典的教材。 Mujoco 中用到的 RNE, CRB 算法以及稀疏惯性分解(sparse inertia factorization)都是参考的它。

我们采用 Snethen 提出的 MPR 算法进行凸网格碰撞(convex mesh collision)检测。 G. Snethen. Complex collision made simple, Game Programming Gems 7, 165-178, 2008.

Stewart 和 Trinkle 提出的线性互补(LCP)算法，目前是大多数仿真引擎进行接触建模的首选。但是 Mujoco 并没有选择它。这是一个很成熟的方法，发展出了很多变种。 D. Stewart and J. Trinkle. An implicit time-stepping scheme for rigid-body dynamics with inelastic collisions and coulomb friction. 1996.

我们的约束模型(constraint model)源自高斯原理(Gauss principle)。 F. Udwadia and R. Kalaba. A new perspective on constrained motion. 1992.

Redon 等人完成了第一个针对接触建模，对高斯原理进行扩展的。他们引入了加速度的不等式约束，并用于模拟无摩擦的接触，构建一个凸二次规划(QP)的问题。 S. Redon, A. Kheddar and S. Coquillart. Gauss’s least constraint principle and rigid body simulations. 2002.

为了用更容易处理的问题来近似 LCP 问题，Anitescu 提出了一种关于加速度的 QP，本质上是 Mujoco 所用的接触模型的一种硬限制(hard limit)。 Redon 对每个接触尽在法线方向上提供一个不等式，与之不同的是，Antiescu 使用的是多个不等式构成的金字塔(pyramid)。这项工作完成了凸互补模型从无摩擦接触到有摩擦解除的转换。 M. Anitescu. Optimization-based simulation of nonsmooth rigid multibody dynamics. Math. Program. Ser. A, 105:113-143, 2006.

Drumwright and Shell 提出了一种基于接触力的 QP，于 Anitescu 早期的工作互补，而且仅限于硬接触。 E. Drumwright and D. Shell, Modeling contact friction and joint friction in dynamic robotic simulation using the principle of maximum dissipation. International Workshop on the Algorithmic Foundations of Robotics, 2010.

E. Todorov. A convex, smooth and invertible contact model for trajectory optimization. IEEE International Conference on Robotics and Automation, 2011.提出了 Mujoco 的第一版模型。它还是一个凸优化问题，但是它可以建立软接触。

Mujoco 获得软约束模型的方式于 ODE 中的约束力混合(CFM)参数有些相似之处，但是 ODE 是在基于 LCP 形式并解决不同的问题。 R. Smith. Open Dynamics Engine user guide. 2006.

Lacoursiere 引入了“ghost variables”，它似乎与 Mujoco 的变形动力学有关。但是，它们有点难以解释，并且与 Mujoco 的确切关系仍有待澄清。 C. Lacoursiere. Ghosts and machines: Regularized variational methods for interactive simulations of multibodies with dry frictional contacts. PhD Thesis, Umea University, 2007.