0. 前言

MuJoCo 官方的手册里大部分内容是面向开发者和程序员编写的，对于初次阅读的用户来说，很容易迷失在一大堆计算机专业术语中。

本入门操作手册，旨在为刚接触机器人仿真的用户提供一个较为清晰的脉络，只保留了一些必要的基础知识，尽量简化了学习路径，去除了大量与实操无关的编程细节。

1. MuJoCo 安装与运行

MuJoCo (Multi-Joint dynamics with Contact) 它是一个通用的物理引擎，旨在促进机器人、生物力学、图形和动画、机器学习和其他需要快速准确地模拟与环境相互作用的铰接结构的领域。
代码层面，MuJoCo是一个带有C API的C/C++库。MuJoCo的模型常常由MJCF/URDF所定义，这通常表现为一个.xml文件。MJCF是Mujoco原生的场景文件，但是MuJoCo也可以加载URDF模型文件。
Mujoco 是一个开源软件，官网如下
mujoco官网
用户可以直接在github下载编译好的文件并解压即可mujoco github下载链接
此外，通过以下命令可以直接安装mujoco python接口

pip install mujoco

本教程会用到python接口

另外，为了直接运行本教程的例程，此处还需安装mediapy，dm_control与ffmpeg

pip install mediapy
pip install dm_control
sudo apt install mesa-utils
sudo apt install ffmpeg #以Debian为例

MuJoCo的python notebook例程可在如下网站找到mujoco python notebook

python教程则可以可以在mujoco python 教程找到

此处以Linux系统为例，其目录结构为

.
├── bin
│   ├── basic
│   ├── compile
│   ├── mujoco_plugin
│   ├── record
│   ├── simulate
│   └── testspeed
├── include
│   └── mujoco
├── lib
│   ├── libmujoco.so -> libmujoco.so.3.1.5
│   └── libmujoco.so.3.1.5
├── model
│   ├── adhesion
│   ├── balloons
│   ├── car
│   ├── cards
│   ├── composite
│   ├── cube
│   ├── hammock
│   ├── humanoid
│   ├── humanoid100
│   ├── mug
│   ├── plugin
│   ├── replicate
│   ├── slider_crank
│   └── tendon_arm
├── sample
│   ├── array_safety.h
│   ├── basic.cc
│   ├── cmake
│   ├── compile.cc
│   ├── Makefile
│   ├── record.cc
│   └── testspeed.cc
├── simulate
│   ├── array_safety.h
│   ├── cmake
│   ├── glfw_adapter.cc
│   ├── glfw_adapter.h
│   ├── glfw_corevideo.h
│   ├── glfw_corevideo.mm
│   ├── glfw_dispatch.cc
│   ├── glfw_dispatch.h
│   ├── main.cc
│   ├── Makefile
│   ├── platform_ui_adapter.cc

运行以下命令，可以快速运行仿真示例

./bin/simulate ./model/humanoid/humanoid.xml

运行结果如下所示

但是，在实际使用中，我们往往是使用Python脚本来控制MuJoCo的进程，调用底层的C代码。在MuJoCo中，最重要的两个数据就是mjModel和mjDate，为了方便后续理解，我们首先再次给出如下的参考表格。

如果需要详细了解MuJoCo仿真的计算架构，可以参考MuJoCo官方Computation介绍

2. MuJoCo Python 基础

MuJoCo Python 官方教程MuJoCo Colab Python 教程

2.1 Interactive viewer

2.1.1 独立app

当前版本的MuJoCo拥有原生的Python API接口，通过如下代码可以打开一个可视化窗口

python -m mujoco.viewer --mjcf=/path/to/some/mjcf.xml

2.1.2 Managed Viewer 模式

通过在Python脚本中使用函数viewer.launch即可调出窗口，这个函数会中断用户代码以实现物理回路的精准计时。如果用户代码被实现为引擎插件或物理回调，并且在mj_step期间由Mujoco调用，则应该使用此模式。

viewer.launch() #打开一个空的可视化窗口，可以通过拖拽加载模型
viewer.launch(model) #打开一个包含给定mjModel的可视化窗口，并且在内部创建mjData实例
viewer.launch(model, data) #打开一个包含给定mjModel的可视化窗口，可视化窗口直接在给定的mjData实例上操作。一旦退出，data对象会被修改

2.1.3 Passsive viewer 模式

viewer.launch_passive(model, data) #该函数允许用户代码继续执行，这种情况下，用户的脚本负责计时并推进物理状态，此时用户的鼠标拖动也不再扰动

launch_passive这个函数会返回一个用以和viewer交互的handle。该handle有以下属性：

1. cam, opt, pert 分别对应mjvCamera, mjvOption, mjvPerturb 这三个结构体

2. lock(): 互斥锁，确保用户代码在修改物理和可视化状态之前锁定viewer

3. sync(): 同步mjModel，mjData以及GUI输入。为了使用户能够在不lock viewer的情况下任意修改mjModel和mjData，passive viewer在不调用sync时，不会访问和修改这些structs。而用户脚本必须调用sync才能让viewer显示物理状态的改变

4. update_hfield(hfieldid), update_mesh(meshid), update_texture(texid): 在后续渲染中更新指定id的height field，mesh，texture数据

5. close(): 关闭窗口，无需lock即可调用

6. is_running(): 返回viewer窗口情况，True or False，无需lock即可调用

7. user_scn: mjvScene对象，允许用户添加更改渲染flags并将自定义可视化geoms添加到渲染场景中。

以下是一个调用launch_passive的最小python 脚本案例

import time
import mujoco
import mujoco.viewer

m = mujoco.MjModel.from_xml_path('./ur5.xml')
d = mujoco.MjData(m)

with mujoco.viewer.launch_passive(m, d) as viewer:
  # Close the viewer automatically after 30 wall-seconds.
  start = time.time()
  while viewer.is_running() and time.time() - start < 30:
    step_start = time.time()

    # mj_step can be replaced with code that also evaluates
    # a policy and applies a control signal before stepping the physics.
    mujoco.mj_step(m, d)

    # Example modification of a viewer option: toggle contact points every two seconds.
    with viewer.lock():
      viewer.opt.flags[mujoco.mjtVisFlag.mjVIS_CONTACTPOINT] = int(d.time % 2)

    # Pick up changes to the physics state, apply perturbations, update options from GUI.
    viewer.sync()

    # Rudimentary time keeping, will drift relative to wall clock.
    time_until_next_step = m.opt.timestep - (time.time() - step_start)
    if time_until_next_step > 0:
      time.sleep(time_until_next_step)

此外，viewer.launch_passive可以接受以下关键字参数

1. key_callback: 一个callable对象，当viewer窗口中有keyboard事件时会调用。例如下面这段代码可实现按下spacebar来启停仿真

paused = False

def key_callback(keycode):
  if chr(keycode) == ' ':
    nonlocal paused
    paused = not paused

...

with mujoco.viewer.launch_passive(m, d, key_callback=key_callback) as viewer:
  while viewer.is_running():
    ...
    if not paused:
      mujoco.mj_step(m, d)
      viewer.sync()
    ...

2. show_left_ui and show_right_ui: Boolean变量，指明UI可见或隐藏。不过用户依然可以按下Tab或Shift+Tab来切换UI状态

2.2 Basic usage

2.2.1 Structs

mjModel以外的所有结构体在Python中都有constructor。对于一个拥有mj_defaultFoo形式的初始化函数的结构体，Python constructor会自动调用默认初始化。例如，mujoco.MjOption()创造了一个mjOption实例，该实例由mj_defaultOption初始化。

拥有mj_makeFoo形式初始化函数的结构体，在Python中由相应的constructor。例如Python中的mujoco.MjvScene(model, maxgeom=10)创造了一个mjvScene实例，该实例由C中的mjv_makeScene(model, [the new mjvScene instance], 10)初始化。

mujoco.MjModel类在Python中没有constructor但是可以通过函数来创造mjModel实例：mujoco.MjModel.from_xml_string, mujoco.MjModel.from_xml_path, mujoco.MjModel.from_binary_path

2.2.2 Functions

Mujoco函数和Python函数的函数名一样。在大多数情况下，函数参数的显示方式与C中的完全相同，并且支持使用与mujoco.h中声明的名称相同的关键字参数。调用这些函数时，按mujoco.h中出现的顺序传递数组大小以外的所有参数，或者使用关键字参数。

例如，在Python中，mj_jac的调用方法是mujoco.mj_jac(m, d, jacp, jacr, point, body)。

Python bindings也提供了一个附加功能。由于mj_step经常在循环中调用，我们添加了一个额外的nstep参数，指示底层mj_step应该被调用多少次。如果未指定，nstep将采用默认值1。以下两个代码段执行相同的计算，但第一个代码段在后续物理步骤之间不获取GIL。

(注: Global Interpreter Lock，简称GIL，是一个互斥锁，它防止多个线程同时执行Python字节码。GIL是CPython实现的一部分，CPython是Python最广泛使用的实现)

2.2.3 Enums and constants

Mujoco的枚举变量可以通过符合mujoco.mjtEnumType.ENUM_VALUE的变量实现，例如mujoco.mjtObj.mjOBJ_SITE。

Mujoco 常量和mujoco Python库里的名称一致，比如mujoco.mjVISSTRING。

2.2.4 最小示例

import mujoco
ASSETS=dict()
with open('/path/to/gizmo.stl', 'rb') as f:
  ASSETS['gizmo.stl'] = f.read()

model = mujoco.MjModel.from_xml_path('./mini.xml', ASSETS)
data = mujoco.MjData(model)
while data.time < 1:
  mujoco.mj_step(model, data)
  print(data.geom_xpos)

这里我们详细解释一下model和data两个变量

MuJoCo中的 mjModel 包含了一个模型的所有不随时间改变的描述。

mjData则包含了状态以及与之相关的变量，比如时间、广义坐标、广义速度，分别可以由data.time, data.qpos 和 data.qvel 调用。

2.2.5 Named access

为了方便和代码可读性，Python bindings 在MjModel和MjData上提供了“named access” API。mjModel结构体中的每个name_fooadr字段都定义了一个名称类别foo。

以下是mujoco中所有类，以及其相应的Python API别名

body

jnt or joint

geom

site

cam or camera

light

mesh

skin

hfield

tex or texture

mat or material

pair

exclude

eq or equality

tendon or ten

actuator

sensor

numeric

text

tuple

key or keyframe

2.2.6 Rendering

OpenGL上下文（OpenGL context）是用于OpenGL渲染的一个概念，它包含了OpenGL状态机的状态，以及用于执行OpenGL命令的所有必要信息。而MuJoCo 本身希望用户再调用任何mjr_渲染之前，设置一个OpenGL context。Python bindings 提供了mujoco.GLContext这样的基础类，来帮助用户设置一个context用以离屏渲染。在多线程或多屏幕的应用程序中，可能会有多个context，但一次只能有一个context是current。

用户可以通过调用以下代码来创建一个context

ctx = mujoco.GLContext(max_width, max_height)

在图形编程中，特别是在使用OpenGL或OpenGL ES这样的图形API时，“make current”是一个过程，它将特定的上下文（context）设置为当前上下文，即当前进行渲染和调用的上下文。在调用MuJoCo渲染函数前，我们必须通过ctx.make_current()。

通常情况下，ctx对象被删除时context会被自动释放，但是部分多线程情景必须要手动调用ctx.free()来释放底层的OpenGL context，且此后用户需要保证不在该context上调用渲染。

官方可视化教程

2.2.7 Error handling

MuJoCo通过mju_error机制报告不可恢复的错误，该机制会立即终止整个过程。Python bindings 使用了longjmp，这样可以将不可恢复的MuJoCo错误转换为mujoco.FatalError类型的Python异常。

2.2.8 Callbacks

MuJoCo 允许用户安装自定义的回调函数来修改计算pipeline中的某些部分。比如，mjcb_sensor可以用来自定义sensor，而mjcb_control可以被用来自定义actuators。callback是通过mujoco.h中前缀为mjcb_的函数指针公开的。

对于每个回调函数mjcb_foo，用户可以通过mujoco.set_mjcb_foo(some_callable)将其设置为Python callable。调用mujoco.set_mjcb_foo(None)则可以将其重置。调用mujoco.get_mjcb_foo()可以恢复当前安装的callback。

bindings每次进入callback时会自动获取GIL，并在再次进入MuJoCo时释放。因此在计算pipeline过程中如果多次调用callbacks可能会严重影响性能。所以callback可能不太适合生产环境，更适合制作复杂模型的原型。

2.3 Modules

mujoco 包含了两个sub-mudules: mujoco.rollout 和 mujoco.minimize

2.3.1 rollout

mujoco.rollout 可以展开一个轨迹，即在一个循环中调用mj_step，给定initial state, sequence of controls就可以返回subsequent states以及sensor values。其基础用法为如下形式：

state, sensordata = rollout.rollout(model, data, initial_state, control)

其中 initial_state 是一个 nroll x nstate 的 array，具有大小为 nstate 的 nroll 初始状态。其中 nstate = mj_stateSize(model, mjtState.mjSTATE_FULLPHYSICS) 是 Full physics state (注：Full physics state 包含所有物理状态以及仿真时间还有插件状态)。

而 control 是一个 nroll x nstep x ncontrol 的 array 变量。控制默认为 mjModel.nu 标准 actuator，但可以通过传递一个可选的 control_spec 位标志来指定任何用户输入array的组合。

如果一次rollout发散，当前状态和传感器值将被用来填充轨迹的剩余部分。因此，可以使用非递增的时间值来检测发散的rollout。

rollout 函数被设计为完全无状态的，因此stepping管道的所有输入都被设置，给定MjData实例中已经存在的任何值都不会影响输出。

2.3.2 minimize

该模组包含了一些优化相关的工具。函数 minimize.least_squares() 实现了一个非线性最小二乘优化器 (Least Squares Optimizer) ，通过 mju_boxQP 来解决一个序列化的Quadratic Programs的问题。

2.4 mujoco-py migration

在mujoco-py中，主要的入口点是MjSim类。用户可以从一个MJCF模型构造一个有状态MjSim实例（类似于dm_control.Physics），这个实例持有对mjModel实例及其关联的mjData的引用。相比之下，MuJoCo的Python bindings 采取了更底层的方法，遵循C库的设计原则，mujoco 模块本身是无状态的，只是封装了底层的原生结构和函数。

以下给出部分常见函数的迁移方法：

1. mujoco_py.load_model_from_xml(bstring): 该函数被 mujoco.MjModel.from_xml_string 和 mujoco.MjModel.from_xml_path 替代了。具体用法上，用户应当按如下形式编写：

model = mujoco.MjModel.from_xml_string(xml)
data = mujoco.MjData(model)

2. sim.reset(), sim.forward(), sim.step(): 此处用户需要调用底层库函数来传递MjModel和MjData的实例：

mujoco.mj_resetData(model, data)
mujoco.mj_forward(model, data)
mujoco.mj_step(model, data)

3. sim.get_state(), sim.set_state(state), sim.get_flattened_state(), sim.set_state_from_flattened(state): MuJoCo库的计算在给定特定输入时是确定性的，如编程部分所述。mujoco-py 实现了获取和设置一些相关字段的方法（同样，dm_control.Physics 提供了与扁平情况对应的方法）。mujoco不提供这种抽象，用户需要明确地获取/设置相关字段的值。

4. sim.model.get_joint_qvel_addr(joint_name): 这是mujoco-py中的一个便捷方法，它返回与这个关节对应的连续索引列表。该列表从model.jnt_qposadr[joint_index]开始，其长度取决于关节类型。mujoco不提供这个功能，但可以使用model.jnt_qposadr[joint_index]和xrange轻松构建这个列表。

5. sim.model.*_name2id(name): 在 Python bindings 中由 mujoco.mj_name2id(model, type_enum, name) 代替

6. sim.save(fstream, format_name): 在 Python bindings 中由mujoco.mj_saveLastXML(fname) 替代。mujoco-py的实现有一个方便的额外特性，即模拟数据的姿态（由sim.data的状态确定）被转换为一个关键帧，在保存之前添加到模型中。这个额外特性目前在mujoco中不可用。

3. MuJoCo 计算框架

包括被动力、数值积分以及状态相关内容，请详见官方文档相关内容

此外，详细的仿真pipeline中前向动力学与反向动力学的运算流程，也请参阅Simulation Pipeline

3.1 MuJoCo Actuation Model

MuJoCo提供了一中灵活的$actuator$模型，所有的$actuator$都是单输入单输出(SISO)，用户指定一个标量控制量$u_i$输入到$actuatori$。输出量是一个标量力$p_i$，其通过由transmission确定的力臂向量映射到关节坐标。一个$actuator$也可以有它自己的状态变量$w_i$。

所有$actuator$的控制输入都被存储在mjData.ctrl，力的输出被存储在mjData.actuator_force，$actuator$的状态变量存储在mjData.act。

一个$actuator$如何工作需要由三个组件确定，分别是transmission, activation dynamics, force generation。用户可以独立地设定每个部分，或者通过$actuatorshortcuts$来实例化一些常见的执行器类型。

具体的不同执行器函数定义可以参阅MuJoCo官方actuator文档

此处我们需要简单介绍一些

1. actuator/⁠general (*): 创建了一个通用的执行器，用户可以自定义执行器的每一个组件。

2. actuator/⁠motor (*): 属于$actuatorshortcuts$，当使用该方式时，指针会创建一个general执行器，属性设置为如下

Attribute	Setting	Attribute	Setting
dyntype	none	dynprm	1 0 0
gaintype	fixed	gainprm	1 0 0
biastype	none	biasprm	0 0 0

3. actuator/⁠position (), actuator/⁠velocity (), actuator/⁠intvelocity (), actuator/⁠damper (), actuator/⁠cylinder (), actuator/⁠muscle (), actuator/⁠adhesion (): 请参考官方文档

3.1.1 Transmission

Transmission 官方文档

每个$actuator$都有一个由其transimission种类与参数定义的标量长度$l_i(q)$。其梯度$\mathrm{\nabla }{l}_i$是一个维度为$n_v$的力臂向量。这决定了从标量的执行器力映射到关节力。transmission的属性取决于$actuator$附加在何种MuJoCo对象上。可能的对象包括joint, tendon, jointinparent, slider-crank, site, body。这里比较常用的就是joint。

3.1.2 Stateful actuators

某些如液压管、肌肉等类型的执行器，拥有一个叫"activation"的内部属性。这是一个与关节位置$q$以及速度$v$无关的一个动态属性。当一个模型中拥有这种自带activation属性的执行其实，就会形成一个三阶动态。我们用向量$w$表示激活状态，它包含一些由激活类型和相应模型参数确定的一阶动态

$${\dot{w}}_i(u_i,w_i,l_i,{\dot{l}}_i)$$

注意到每个执行器拥有和其他执行器独立的动态，目前实现的激活类型有如下这些：

$$\begin{gathered} integrator:{\dot{w}}_i=u_i \\ filter:{\dot{w}}_i=(u_i-w_i)/t \\ filterexact:{\dot{w}}_i=(u_i-w_i)/t \\ muscle:{\dot{w}}_i=muscle(u_i,w_i,l_i.{\dot{l}}_i) \end{gathered}$$

此处$t$是一个储存在mjModel.actuator_dynprm中的执行器相关的常数。此外，激活类型还可以由用户调用mjcb_act_dyn自定义，或者设定为"none"来表示无激活状态。

对于生物力学研究者，如果需要参考更多与肌肉激活相关内容，可以参阅官方肌肉手册

更多关于filterexact细节也请参与官方Stateful Actuators

3.1.3 Force generation

每个执行器都会根据一些函数生成一个标量力$p_i$

$$p_i(u_i,w_i,l_i,{\dot{l}}_i)$$

与激活动态类似，力的生成也是只和每个$actuator$自己相关。目前，力和这些变量是仿射关系

$$p_i=(a{w}_{i}ora{u}_i)+b_0+b_1{l}_i+b_2{\dot{l}}_i$$

此处$a$是增益参数，储存在mjModel.actuator_gainprm，$b_0,b_1,b_2$是偏置参数，储存在mjModel.actuator_biasprm。

不同的增益和偏置参数，可以被用来同时建模直接力控以及位置、速度伺服。还可以通过安装回调mjcb_act_gain和mjcb_act_bias并将增益和偏置类型设置为“user”来计算自定义增益和偏置项。

注意，仿射 force generation 让我们可以使用力臂矩阵的伪逆从逆动力学中计算出的作用力推断出control/activation。然而，现实世界中使用的一些执行器并不是仿射的（尤其是那些具有嵌入式低级控制器的执行器），因此我们正在考虑对上述模型的扩展。

考虑所有$actuators$，在广义坐标下的合力如下：

$$\sum _i\mathrm{\nabla }{l}_i(q)p_i(u_i,w_i,l_i(q),\dot{l}(q,v))$$

这个量被存储在mjData.qfrc_actuator，并被添加到力向量$\tau$中。一起被添加的还有用户定义的关节坐标系或Cartesian坐标系下的力，两坐标系的力分别存储于mjData.qfrc_applied, mjData.xfrc_applied。

4. 机器人建模

Mujoco中常用的机器人描述文件是URDF文件。
URDF (Unifed Robot Description Format)，是一种描述机器人的通用格式。
为了理解URDF，我们需要引入一些机器人学运动学的基础知识。

4.1 机器人运动学基础(Denavit-Hartenberg方法)

一个机器人是由多个连杆和关节构成的。为了方便描述，我们需要在机器人的每个连杆上分别设置一个连杆坐标系，再描述这些连杆之间的关系。
机器人的每个连杆都可以用$4$个运动学参数来描述，分别是连杆长度，连杆扭转角，连杆偏距，关节角。
其中，连杆长度，连杆扭转角用来描述一个连杆自身，而连杆偏距，关节角用来描述相邻连杆之间的连接关系。

而关节是定义以上参数的关键。关节可以用关节轴的直线来表示，比如图中的关节$i$就可以用直线$Z_i$来表示，关节$i-1$就可以用直线$Z_{i-1}$来表示。

4.1.1 连杆描述参数

1. 连杆$i-1$的连杆长度$a_{i-1}$定义为，相邻两个关节轴，即直线$Z_{i+1}$和$Z_i$，的公垂线的长度$a_{i-1}$。公垂线$a_{i-1}$的方向规定为从关节$i-1$指向关节$i$。

2. 而连杆$i-1$的连杆扭转角${\alpha }_{i-1}$定义为，从关节轴$Z_{i-1}$按右手系绕公垂线$a_{i-1}$转至关节轴$Z_i$的平行线的角度。当公垂线$a_{i-1}$长度为$0$时，连杆扭转角${\alpha }_{i-1}$的值可以任意规定。

3. 连杆偏距$d_i$定义为，相邻两个连杆$a_i$和$a_{i-1}$在公共轴线$Z_i$上的截距。

4. 而关节角${\theta }_i$的定义则为，连杆长度向量$a_{i-1}$和$a_i$的夹角。方向依然是从$a_{i-1}$到$a_i$的右手系。

对于转动关节而言，关节角${\theta }_i$是变量，其他三个参数固定不变；对于移动关节而言，连杆偏距$d_i$是变量，其他三个参数固定不变。

这种用连杆参数描述机构运动关系的规则称为 Denavit-Hartenberg (D-H) 方法。例如，对于一个拥有$6$个旋转关节的机器人，则只需要$3\ast 6=18$个参数就能完全描述它的固定的运动学参数。即

$$\begin{array}{}\text{(4.1)}& (a_i,{\alpha }_i,d_i),i=1,2,...,6\end{array}$$

4.1.2 连杆坐标变换

在此我们定义，${}^{A}P$表示向量$P$在坐标系$\{A\}$中的描述。${}_B^{A}T$坐标系$\{B\}$相对于$\{A\}$的表达。并且我们有

$$\begin{array}{}\text{(4.2)}& {}^{A}P{=}_B^{A}T{\cdot }^{B}P\end{array}$$

而对于机械臂而言，连杆坐标系$\{i\}$在$\{i-1\}$中的描述可以由此前定义的四个连杆描述参数连杆扭转角$a_{i-1}$，连杆扭转角${\alpha }_{i-1}$，连杆偏距$d_i$，关节角${\theta }_i$来表示：

$$\begin{array}{}\text{(4.3)}& {}_i^{i-1}T=\left[\begin{array}{cccc}\cos ({\theta }_i)& -\sin ({\theta }_i)& 0& a_{i-1}\\ \sin ({\theta }_i)\cos ({\alpha }_{i-1})& \cos ({\theta }_i)\cos ({\alpha }_{i-1})& -\sin ({\alpha }_{i-1})& -\sin ({\alpha }_{i-1})d_i\\ \sin ({\theta }_i)\sin ({\alpha }_{i-1})& \cos ({\theta }_i)\sin ({\alpha }_{i-1})& \cos ({\alpha }_{i-1})& \cos ({\alpha }_{i-1})d_i\\ 0& 0& 0& 1\end{array}\right]\end{array}$$

基坐标系和末端坐标系的关系可以通过连杆变换矩阵的连乘来得到：

$$\begin{array}{}\text{(4.4)}& {}_N^{0}T{=}_1^{0}T{\cdot }_2^{1}T\cdot \dots {\cdot }_N^{N-1}T\end{array}$$

4.2 URDF

URDF模型就是xml格式下的一种机器人描述语言

4.2.1 xml语言基础语法知识

XML (EXtensible Markup Language)，通常作为配置文件、标准数据格式。
XML的基本单位是元素，元素由标记来定义。
标记包括起始标记<>和结束标记</>，属性要写在起始标记内。并且在XML中，所有元素必须有结束标记。

<元素名 属性名="属性值">
    元素内容
</元素名>

在Mujoco中，元素内容和对应的元素名相关
需要指出的是，XML中可以嵌套，但是不能交叉。
此外，XML文件中的元素名称不能以数字或者特殊字符开偷，不能以字符串"XML"开头，不能包含空格、大小写敏感。可以包含字母、数字和下划线。
XML中的注释写为：

<!--  -->

更多细节可以参考MoJoCo官方xml教程

此处给出一个MJCF格式的简单模型示例，该xml定义了一个长方体

<mujoco>
  <worldbody>
    <light diffuse=".5 .5 .5" pos="0 0 3" dir="0 0 -1"/>
    <geom type="plane" size="1 1 0.1" rgba=".9 0 0 1"/>
    <body pos="0 0 1">
      <joint type="free"/>
      <geom type="box" size=".1 .2 .3" rgba="0 .9 0 1"/>
    </body>
  </worldbody>
</mujoco>

4.2.2 URDF基本描述方法

在URDF中，描述连杆之间拓扑结构，应当写成如下形式

<robot name="test_robot">
  <link name="link1" />
  <link name="link2" />
  <link name="link3" />
  <link name="link4" />

  <joint name="joint1" type="continuous">
    <parent link="link1" />
    <child link="link2" />
    <origin xyz="5 3 0" rpy="0 0 0" />
    <axis xyz="-0.9 0.15 0" />
  </joint>

  <joint name="joint2" type="continuous">
    <parent link="link1" />
    <child link="link3" />
    <origin xyz="-2 5 0" rpy="0 0 1.57" />
    <axis xyz="-0.707 0.707 0" />
  </joint>

  <joint name="joint3" type="continuous">
    <parent link="link3" />
    <child link="link4" />
    <origin xyz="5 0 0" rpy="0 0 -1.57" />
    <axis xyz="0.707 -0.707 0" />
  </joint>
</robot>

其所构建的模型连杆拓扑示意图如下

不过，以上仅仅包含了连杆间的拓扑关系，不包含连杆本身的物理描述。实际上我们还需要给每一个link赋予geometry属性，通常使用mesh格式描述，比如stl文件

<link name="base_link">
  <visual>
    <geometry>
      <mesh filename="../ur5/collision/base.stl" />
    </geometry>
    <material name="LightGrey">
      <color rgba="0.7 0.7 0.7 1.0" />
    </material>
  </visual>

  <collision>
    <geometry>
      <mesh filename="../ur5/collision/base.stl" />
    </geometry>
  </collision>

  <inertial>
    <mass value="4.0" />
    <origin rpy="0 0 0" xyz="0.0 0.0 0.0" />
    <inertia ixx="0.00443333156" ixy="0.0" ixz="0.0" iyy="0.00443333156" iyz="0.0" izz="0.0072" />
  </inertial>
</link>

如果你的电脑安装了ROS，那么可以通过如下命令检查URDF文件是否正确

''' urdf_to_graphiz robot1.urdf '''

此外可以通过如下方法将关系图导出为pdf，并使用evince查看 ''' urdf_to_graphiz robot1.urdf evince robot1.pdf '''

当然，实际操作中，较少需要自行手写xml来建立模型，主要还是寻找已有的模型，或者借助sw2urdf，把solidworks模型导出为urdf格式

4.2.3 URDF导出MJCF格式

mujoco自带了编译工具，可以对URDF进行格式转换

./bin/compile model.urdf model.xml
./bin/compile model.urdf model.txt
./bin/compile model.urdf model.mjb

4.2.4 MJCF特点

1. MJCF的根元素是mujoco，属性model为模型名
2. 元素asset定义所有mesh为子元素
3. 机器人全部建立在worldbody元素中
4. 一个body包含子元素inertial、joint等

此处给出URDF与MJCF的对比

URDF格式的ur5.urdf描述如下

ur5.urdf

MJCF格式的ur5.xml描述如下

ur5.xml

5. 机器人控制理论

5.1 动力学状态方程

Mujoco 作为一个动力学仿真器，动力学的具体解算可以交给计算机。

在实际任务中，机器人通过传感器获得外界信息，并通过轨迹生成器生成期望轨迹，而驱动器通常按照扭矩发送指令。

下图表现了轨迹生成器和机器人的关系，机器人从控制系统接收到一个关节扭矩矢量$\tau$，机器人的传感器允许控制器读取关节位置矢量$\mathrm{\Theta }$和关节速度矢量$\dot{\mathrm{\Theta }}$。轨迹生成器会给定期望的关节运动参数${\mathrm{\Theta }}_d$，${\dot{\mathrm{\Theta }}}_d$，${\ddot{\mathrm{\Theta }}}_d$

基于牛顿-欧拉方程，机器人动力学可以写成如下形式

$$\begin{array}{}\text{(5.1)}& \tau =M\left(\mathrm{\Theta }\right)\ddot{\mathrm{\Theta }}+V(\mathrm{\Theta },\dot{\mathrm{\Theta }})+G\left(\mathrm{\Theta }\right)\end{array}$$

式中$M\left(\mathrm{\Theta }\right)$为机器人的$n\times n$质量矩阵，$V(\mathrm{\Theta },\dot{\mathrm{\Theta }})$是$n\times 1$的离心力和哥氏力矢量，$G(\mathrm{\Theta },\dot{\mathrm{\Theta }})$是$n\times 1$的重力矢量。上式也称为机器人的状态空间方程

对于一个接受轨迹生成器信号的控制器而言，其输出的关节扭矩矢量$\tau$则是由状态变量的期望轨迹决定

$$\begin{array}{}\text{(5.2)}& \tau =M\left({\mathrm{\Theta }}_d\right){\ddot{\mathrm{\Theta }}}_d+V({\mathrm{\Theta }}_d,{\dot{\mathrm{\Theta }}}_d)+G\left({\mathrm{\Theta }}_d\right)\end{array}$$

如果动力学模型是完整且精确的，并且系统不存在噪音，那系统确实可以通过$eq(3.2)$获得的控制输入$\tau$沿着期望轨线运行。

但这显然是一个开环系统，真实的机器人控制中，我们需要实时获得__伺服误差__

$$\begin{array}{}\text{(5.3)}& E={\mathrm{\Theta }}_d-\mathrm{\Theta }\end{array}$$

并最终要通过闭环控制来减小伺服误差

5.2 操作臂的控制问题

对于一个机械臂而言，通常不能通过在工作点附近局部线性化的方法来处理，我们选择直接研究非线性运动方程，而不借助线性化方法设计控制器。

5.2.1 控制律的分解

本教程的控制部分参考了《机器人学导论》中的方法，将控制分为基于模型的控制和伺服控制。在非线性模型中，基于模型的控制相当于一个线性化函数。在《Applied Nonlinear Control》中该方法也成为反馈线性化(Feedback Linearization)

例如，对于一个二阶非线性受控系统

$$\begin{array}{}\text{(5.4)}& m\ddot{x}+b\dot{x}+k{x}^3=f\end{array}$$

其控制输入的设计可以分为两步，首先是基于模型的控制，该步骤希望将系统简化为一个单位质量，为此我们将输入$f$分解为

$$\begin{array}{}\text{(5.5)}& f=\alpha f^{\prime }+\beta \end{array}$$

此处通过选择$\alpha ,\beta$为某些函数或常数，将原系统转变为以$f^{\prime }$为输入的单位质量。例如，在如式$(4.4)$的系统中，可以设

$$\begin{gathered} \begin{array}{}\text{(5.6)}& \alpha =m \\ \beta =b\dot{x}+k{x}^3\end{array} \end{gathered}$$

将$(4.6),(4.5)$代入$(4.4)$则可得

$$\begin{array}{}\text{(5.7)}& \ddot{x}=f^{\prime }\end{array}$$

此处伺服控制$f^{\prime }$则可以设计为

$$\begin{array}{}\text{(5.8)}& f^{\prime }=-k_v\dot{x}-k_{p}x\end{array}$$

至此，通过分解控制律，可以得到系统最终的动力学方程为如下线性形式

$$\begin{array}{}\text{(5.9)}& \ddot{x}+k_v\dot{x}+k_{p}x=0\end{array}$$

6 MuJoCo Python 实战

本节内容主要依然参考MuJoCo Colab

尽管前述章节也有零散介绍许多MuJoCo的代码相关知识，但是本章节依然将从头带大家梳理一些常见的项目场景及其代码。

6.1 代码示例

# xml reference: https://mujoco.readthedocs.io/en/stable/XMLreference.html

import time
import mujoco
import mujoco.viewer

model = mujoco.MjModel.from_xml_path('./tippe_top.xml')
data = mujoco.MjData(model)

#renderer = mujoco.Renderer(model)

print("initial position: ", data.qpos)
print("initial velocity: ", data.qvel)
data.qpos=[0, 0, 0.02, 1, 0, 0, 0]
data.qvel=[0, 0, 0, 0, 1, 200]

print("type qpos: ", type(data.qpos))
print("type qvel: ", type(data.qvel))

#print(dir(data))


with mujoco.viewer.launch_passive(model, data) as viewer:
  # Close the viewer automatically after 30 wall-seconds.
  start = time.time()
  while viewer.is_running() and time.time() - start < 30:
    step_start = time.time()

    # mj_step can be replaced with code that also evaluates
    # a policy and applies a control signal before stepping the physics.
    mujoco.mj_step(model, data)

    # Example modification of a viewer option: toggle contact points every two seconds.
    # 这一段主要是展示了一个在viewer中添加接触点显示的示例
    with viewer.lock():
      viewer.opt.flags[mujoco.mjtVisFlag.mjVIS_CONTACTPOINT] = int(data.time % 2)

    # Pick up changes to the physics state, apply perturbations, update options from GUI.
    viewer.sync()

    # Rudimentary time keeping, will drift relative to wall clock.
    # 这一段是确保了仿真步进的统一
    time_until_next_step = model.opt.timestep - (time.time() - step_start)
    if time_until_next_step > 0:
      time.sleep(time_until_next_step)

在Mujoco中，要实现控制，需要在机器人的关节处增加$actuator$，比如，我们需要在此前的MJCF格式的UR5模型中添加$actuatort$使其变为如下形式

ur5_a.xml

在添加了$actuator$后，我们还需要......（待更新）