Mujoco 学习系列（三）机器人状态IO与仿真操作

这篇博客是继上一篇如何写机器人描述文件 xml 后的更进一步，因为我们不可能从 GUI 中读机器人各个关节或状态信息，所以需要用 mujoco 接口从仿真器中获取位置、速度、力等数据，同时也能够向仿真中写入数据。

Mujoco 学习系列（二）基础功能与xml使用

1. 获取 mjData 状态

首先以最简单的滑块为例，定义一个简单的描述文件 merge.xml，这个描述文件中滑块在地面上，并且提供了一个执行器，运行后你可以通过仿真器右侧的 Control 面板来给滑块施加位置环伺服。

xml 复制代码

<mujoco>
  <worldbody>
    <light diffuse="0.5 0.5 0.5" pos="0 0 5" dir="0 0 -1"/> 

    <geom name="ground" type="plane" size="5 5 0.1" pos="0 0 0" 
          rgba="0.5 0.5 0.5 1" solimp="0.9 0.95 0.001" solref="0.02 1"/>

    <body name="slider" pos="0 0 0.1">
      <joint name="slide_joint" type="slide" axis="1 0 0" damping="0.2" range="-2 2"/>
      <geom type="box" size="0.2 0.1 0.1" rgba="0 0.5 0.8 1" mass="0.5" />
    </body>
  </worldbody>

  <actuator>
    <position name="position" joint="slide_joint" kp="100" kv="10"/>
  </actuator>
</mujoco>

然后编写一个 python 脚本 demo.py，因为关节信息是基础元素，所以可以直接从 data 数据类型中获得：

python 复制代码

import mujoco
import mujoco.viewer

# 载入模型
model_path = "./merge.xml"
model = mujoco.MjModel.from_xml_path(model_path)
data = mujoco.MjData(model)

with mujoco.viewer.launch_passive(model, data) as viewer:
    while viewer.is_running():
        mujoco.mj_step(model, data)
        
        # 获取滑块位置：直接读取关节位置
        slider_pos = data.qpos[0]  # 这里只有一个滑动关节
        print(f"Slider position: {slider_pos:.4f}")
        
        viewer.sync()

运行示例：

bash 复制代码

(mujoco) $ python demo.py

拖拽右侧 Control 面板的位置滑块可以看见 terminal 中的位置数据信息变化：

通过查看官网文档中对 MjData 数据类型的描述可获得更丰富的属性。由于API接口的统一性，你可以直接调用结构体里面的对象，上面的python脚本中就是直接获取了 qpos 成员变量：

MjData 官方文档：API Reference - Types

这种形式一旦关节多起来就非常容易混淆，特别是还需要一个一个去辨别索引号，因此也可以通过下面的方式找到指定 joint 的信息：

python 复制代码

import mujoco
import mujoco.viewer

model_path = "./merge.xml"
model = mujoco.MjModel.from_xml_path(model_path)
data = mujoco.MjData(model)

with mujoco.viewer.launch_passive(model, data) as viewer:
    while viewer.is_running():
        mujoco.mj_step(model, data)
        
        # 用name的方式找到明确的关节
        joint_id = model.joint(name="slide_joint").id
        slider_pos = data.qpos[joint_id]
        print(f"Slider position: {slider_pos:.4f}")
        
        viewer.sync()

这就是我在前两篇博客中一直强调的 给关节显示声明name属性 的意义，如果你在 xml 中没有显示定义关节名，那么在获取关节时就只能一个一个去找对应的 index 。复杂构型的机器人光是这一步就可以消耗掉一天的时间。

2. 获取 sensor 状态

mujoco 还提供了一种方式是通过传感器 sensor 获取机器人状态，以这种方式获取时需要在 xml 文件中定义传感器及其类型，这里以关节位置传感器 jointpos 为例，对上面的 xml 文件进行修改：

xml 复制代码

<mujoco>
  <worldbody>
    <light diffuse="0.5 0.5 0.5" pos="0 0 5" dir="0 0 -1"/> 

    <geom name="ground" type="plane" size="5 5 0.1" pos="0 0 0" 
          rgba="0.5 0.5 0.5 1" solimp="0.9 0.95 0.001" solref="0.02 1"/>

    <body name="slider" pos="0 0 0.1">
      <joint name="slide_joint" type="slide" axis="1 0 0" damping="0.2" range="-2 2"/>
      <geom type="box" size="0.2 0.1 0.1" rgba="0 0.5 0.8 1" mass="0.5" />
    </body>
  </worldbody>

  <actuator>
    <position name="position" joint="slide_joint" kp="100" kv="10"/>
  </actuator>

  <!-- 添加关节位置传感器 -->
  <sensor>
    <jointpos name="joint_sensor" joint="slide_joint" />
  </sensor>
</mujoco>

上面的文件中通过使用 <sensor> 标签定义了一个针对 slide_joint 的关节位置传感器，那么 python 脚本中也需要进行一些修改：

python 复制代码

import mujoco
import mujoco.viewer

model_path = "./merge.xml"
model = mujoco.MjModel.from_xml_path(model_path)
data = mujoco.MjData(model)

with mujoco.viewer.launch_passive(model, data) as viewer:
    while viewer.is_running():
        mujoco.mj_step(model, data)
        
        # 获取滑块位置：通过joint
        joint_id = model.joint(name="slide_joint").id
        slider_pos = data.qpos[joint_id]
        # 获取滑块位置：通过sensor
        sensor_id = model.sensor(name="joint_sensor").id
        sensor_value = data.sensordata[sensor_id]
        print(f"Slider position: {slider_pos:.4f} | Sensor position: {sensor_value:.4f}")
        
        viewer.sync()

运行后可以发现有持续的值打印：

bash 复制代码

(mujoco) $ python demo.py

此时你可能会问，我都有关节信息了为啥还要额外添加一个传感器来获取相同的信息，而且两者的读数还完全一样。那是因为我们现在的示例还是非常初级的阶段，我个人在使用时通常会将两者同时获取，理由如下：

joint 数据是真值，而 sensor 是外界能够直接获取的状态估计，为此在仿真环境中可以对传感器数据添加噪声以更贴近实际情况；
当前只对关节状态进行了仿真，传感器还有 camera、touch、force 等诸多模式；
可以通过设置环境属性来影响传感器的输出，比如添加电场、磁场等；

更多传感器及其类型可以通过官网链接查看：

sensor 文档：XML Reference - Sensor

3. 设置 joint 值

通过代码可以获取机器人 joint 值自然也可对其进行设置，这是机器人控制的基础功能，编写 xml 文件如下：

xml 复制代码

<mujoco>
  <worldbody>
    <light diffuse="0.5 0.5 0.5" pos="0 0 5" dir="0 0 -1"/> 

    <geom name="ground" type="plane" size="5 5 0.1" pos="0 0 0" 
          rgba="0.5 0.5 0.5 1" solimp="0.9 0.95 0.001" solref="0.02 1"/>

    <body name="slider" pos="0 0 0.1">
      <joint name="slide_joint" type="slide" axis="1 0 0" damping="0.2" range="-2 2"/>
      <geom type="box" size="0.2 0.1 0.1" rgba="0 0.5 0.8 1" mass="0.5" />
    </body>
  </worldbody>

  <actuator>
    <position name="position" joint="slide_joint" kp="100" kv="10"/>
  </actuator>
</mujoco>

再写一个 python 脚本用来给机器人位置以线性方式进行增长：

python 复制代码

import mujoco
import mujoco.viewer

model_path = "./merge.xml"
model = mujoco.MjModel.from_xml_path(model_path)
data = mujoco.MjData(model)

with mujoco.viewer.launch_passive(model, data) as viewer:
    step = 0
    while viewer.is_running():
        # 施加控制信号，使滑块向右滑动
        ctrl_signal = min(1.0, 0.000001 * step)  # 控制信号线性增长，最大为1（由 ctrlrange 限制）
        data.ctrl[0] = ctrl_signal

        mujoco.mj_step(model, data)
        
        # 读取并打印滑块位置
        slider_pos = data.qpos[0]
        print(f"Step {step} | Control: {ctrl_signal:.3f} | Slider Position: {slider_pos:.4f}")

        viewer.sync()
        step += 1

同样可以通过指定关节名的形式修改位置状态，推荐使用这种：

python 复制代码

import mujoco
import mujoco.viewer

model_path = "./merge.xml"
model = mujoco.MjModel.from_xml_path(model_path)
data = mujoco.MjData(model)

# 获取 actuator 的索引
actuator_id = model.actuator(name="position").id

# 启动可视化窗口
with mujoco.viewer.launch_passive(model, data) as viewer:
    step = 0
    while viewer.is_running():
        # 设置控制信号（使用 actuator_id 替代 data.ctrl[0]）
        ctrl_signal = min(1.0, 0.000001 * step)
        data.ctrl[actuator_id] = ctrl_signal

        mujoco.mj_step(model, data)
        print(f"Step {step} | ctrl={ctrl_signal:.3f} | pos={data.qpos[0]:.4f}")
        viewer.sync()
        step += 1

运行后可以看到滑块在缓慢移动，即关节位置以线性的方式被修改：

bash 复制代码

(mujoco) $ python demo.py

4. 不开启GUI界面运行

后面的绝大多数实验会在 GPU 服务器上运行，GPU 服务器通常不会有 GUI 相关的可执行文件，mujoco 实现无 GUI 运行也非常简单，下面这个例子就写了一个循环，当超时后自动结束仿真：

python 复制代码

import mujoco
import mujoco.viewer
import time

model_path = "./merge.xml"
model = mujoco.MjModel.from_xml_path(model_path)
data = mujoco.MjData(model)

# 获取 actuator 的索引
actuator_id = model.actuator(name="position").id

# 仿真主循环
step = 1
start_time = time.time()
while True:
    ctrl = min(1.0, 0.000001 * step)
    data.ctrl[actuator_id] = ctrl

    mujoco.mj_step(model, data)

    pos = data.qpos[0]
    print(f"Step {step} | ctrl={ctrl:.3f} | pos={pos:.4f}")
    step += 1
    if time.time() - start_time > 5.0:
        break
    
print("Simulate done.")

运行结果如下：

bash 复制代码

(mujoco) $ python demo.py

Step 537599 | ctrl=0.538 | pos=0.5375
Step 537600 | ctrl=0.538 | pos=0.5376
Step 537601 | ctrl=0.538 | pos=0.5376
Step 537602 | ctrl=0.538 | pos=0.5376
Step 537603 | ctrl=0.538 | pos=0.5376
Simulate done.
...

5. 重置仿真

重置方针条件也是进行大规模试验的基本需求，如果每次仿真结束都重启程序的话会导致大量资源消耗在仿真器初始化上，大模型训练和测试都以 K 为单位起步，因此 mujoco 也提供了重置仿真环境的功能：

python 复制代码

import mujoco
import mujoco.viewer
import time

model_path = "./merge.xml"
model = mujoco.MjModel.from_xml_path(model_path)
data = mujoco.MjData(model)

actuator_id = model.actuator(name="position").id

start_time = time.time()
step = 0

with mujoco.viewer.launch_passive(model, data) as viewer:
    while viewer.is_running():
        ctrl_signal = min(1.0, 0.000001 * step)
        data.ctrl[actuator_id] = ctrl_signal

        mujoco.mj_step(model, data)
        print(f"Step {step} | ctrl={ctrl_signal:.3f} | pos={data.qpos[0]:.4f} | during time={time.time()-start_time}")
        viewer.sync()
        step += 1
        
        if time.time() - start_time > 5:
            start_time = time.time()
            print("Ready to reset simulation")
            mujoco.mj_resetData(model, data)    # 重置仿真环境
            print("Simulation reset done")
            step = 0

上面的代码核心就是这句 mujoco.mj_resetData(model, data) ，运行后每隔 5s 就会重置一次环境，滑块会瞬间跳回到初始位置。

除了一口气将仿真环境完全重制外，还可以结合第 3 节的内容通过给关节赋值来让关节到指定姿态上，这种情况一般会用在 VLA 模型某个case执行失败后，让其回到如第 5 秒的姿态后以该姿态为初始条件重新生成动作序列。

6. 固定视角录制仿真视频

固定视角录制仿真环境中的视频也是基础需求之一，这可以用来训练模型和 bad case review，使用下面的代码可以实现 mp4 格式的视频录制：

python 复制代码

import mujoco
import imageio
import numpy as np
import time
import mujoco
import mujoco.viewer

model_path = "./merge.xml"
model = mujoco.MjModel.from_xml_path("./merge.xml")
data = mujoco.MjData(model)
renderer = mujoco.Renderer(model, width=640, height=480)

actuator_id = model.actuator(name="position").id

# 视频录制
frames = []
total_steps = 10000
record_fps = 30                         # 录制的帧率
record_time_iter = 1.0 / record_fps     # 通过fps计算两帧的时间间隔
last_frame_time = time.time()           # 上一帧的时间
g_start_time = time.time()

# 启动可视化窗口
with mujoco.viewer.launch_passive(model, data) as viewer:
    step = 0
    while viewer.is_running():
        ctrl_signal = min(1.0, 0.000001 * step)
        data.ctrl[actuator_id] = ctrl_signal

        mujoco.mj_step(model, data)
        print(f"Step {step} | ctrl={ctrl_signal:.3f} | pos={data.qpos[0]:.4f}")
        viewer.sync()
        step += 1
        
        # 录制视频
        if time.time() - last_frame_time > record_time_iter:
            renderer.update_scene(data)
            frame = renderer.render()
            frames.append(np.copy(frame))
            last_frame_time = time.time()
            
        # 超时跳出
        if time.time() - g_start_time > 10:
            break
            
imageio.mimsave("./simulation.mp4", frames, fps=30)
print(f"total simulate time: {time.time() - g_start_time}")

运行后可以在当前路径下生成一个 simulation.mp4 文件：

bash 复制代码

(mujoco) $ python demo.py
	...
total simulate time: 10.277767
(mujoco) $ ll | grpe mp4
-rw-rw-r--  1 gaohao gaohao  35K May 21 14:22 simulation.mp4

通常来说固定视角的视频录制是用来批量生成仿真数据的，由于 mujoco 的框架设计，你可以在 GPU 服务器上一口气创建数万个仿真环境（假设资源充足的话），然后同时生成视频数据用于后续模型训练。

至此，有关 mujoco 最基础的原生功能已经介绍完了，后面的博客将会结合 ROS、Isaac Sim、VLA 等进行联合使用。总体而言 mujoco 在使用上比较简洁，虽然GUI显示效果和多样性落后于 Rviz、Isaac，但能够以非常小计算资源跑起来就是很优秀的作品，因为在 VLA 领域中模型本身就非常吃算力，如果仿真又吃掉一大块资源容易导致模型 out of memory。