📚 课程目录第4章传感器与数据提取

4. 传感器与数据提取：MuJoCo 传感器系统详解

传感器，说白了就是仿真世界的「眼睛」和「耳朵」。

没有传感器，你的机器人就是个瞎子。跑得再好看，你也拿不到真实数据。我刚开始用 MuJoCo 时，就犯过这个错——模型跑得飞起，结果一问数据，啥也拿不出来。

这一章，咱们就把传感器系统彻底讲透。从定义到提取，再到可视化，一条龙搞定。

4.1 传感器类型与定义方式

MuJoCo 的传感器系统，其实比你想的灵活。它不强制你装传感器，但一旦装了，数据就自动给你算好。

我个人习惯，在模型里至少挂这几类传感器：

传感器类型	传感器名称 (name)	测量内容	典型用途
关节位置	jointpos	关节角度/位移	运动学反馈
关节速度	jointvel	关节角速度/线速度	速度控制
关节力/力矩	jointactuatorfrc	驱动力矩	力控、碰撞检测
接触力	touch	接触点法向力	抓取、足底力
IMU 数据	framelinvel / frameangvel	线速度、角速度	姿态估计

定义方式很简单，在 XML 里加 <sensor> 块就行。举个例子：

<sensor>
    <jointpos name="shoulder_pos" joint="shoulder_joint"/>
    <jointvel name="shoulder_vel" joint="shoulder_joint"/>
    <jointactuatorfrc name="shoulder_torque" joint="shoulder_joint"/>
    <touch name="foot_contact" site="foot_site"/>
</sensor>

嗯，这里要注意：touch 传感器必须绑定到一个 site 上，而不是 body。我刚开始就踩过这个坑，死活读不到接触力。

4.2 数据提取：从仿真到 Python

传感器定义好了，怎么把数据拿出来？

MuJoCo 把所有传感器数据都放在 data.sensordata 这个数组里。你想想看，一个一维数组，装着你所有传感器的实时读数。

我建议用 data.sensor(name) 来索引，比记下标靠谱得多：

import mujoco

# 加载模型
model = mujoco.MjModel.from_xml_path("robot.xml")
data = mujoco.MjData(model)

# 仿真一步
mujoco.mj_step(model, data)

# 提取传感器数据
shoulder_pos = data.sensor("shoulder_pos")
shoulder_vel = data.sensor("shoulder_vel")
shoulder_torque = data.sensor("shoulder_torque")
foot_contact = data.sensor("foot_contact")

print(f"肩关节位置: {shoulder_pos:.3f} rad")
print(f"肩关节速度: {shoulder_vel:.3f} rad/s")
print(f"肩关节力矩: {shoulder_torque:.3f} Nm")
print(f"足底接触力: {foot_contact:.3f} N")

你看，就这么几行，数据就到手了。

我的小技巧： 在仿真循环里，我习惯把传感器数据存到列表里，最后转成 numpy 数组做分析。别在循环里反复打印，那会拖慢仿真速度。

4.3 数据可视化初探

数据拿出来了，光看数字可不行。你得画出来，才能发现问题。

我一般用 Matplotlib 做快速可视化。比如，画个关节位置随时间变化的曲线：

import matplotlib.pyplot as plt
import numpy as np

# 仿真并记录数据
time_steps = []
pos_data = []
vel_data = []

for i in range(1000):
    mujoco.mj_step(model, data)
    time_steps.append(data.time)
    pos_data.append(data.sensor("shoulder_pos"))
    vel_data.append(data.sensor("shoulder_vel"))

# 画图
plt.figure(figsize=(10, 4))

plt.subplot(1, 2, 1)
plt.plot(time_steps, pos_data, label="位置", color="#3498db")
plt.xlabel("时间 (s)")
plt.ylabel("位置 (rad)")
plt.legend()
plt.grid(True)

plt.subplot(1, 2, 2)
plt.plot(time_steps, vel_data, label="速度", color="#e74c3c")
plt.xlabel("时间 (s)")
plt.ylabel("速度 (rad/s)")
plt.legend()
plt.grid(True)

plt.tight_layout()
plt.show()

画出来之后，你一眼就能看出：关节有没有震荡？速度有没有突变？这些在数字里根本看不出来。

我曾经踩过的坑： 有一次我仿真一个四足机器人，足底接触力一直读不到。折腾了半天，才发现是 touch 传感器绑定的 site 没放在足底几何体的表面。记住，site 的位置决定了接触检测的生效区域。

4.4 传感器数据提取的完整流程

为了让你看得更清楚，我画了张流程图，把整个数据提取链路串起来：

4.5 实战：提取并可视化足底接触力

咱们来一个完整的实战例子。假设你有一个四足机器人，想看看四条腿的足底接触力变化。

第一步，在 XML 里给每条腿的足底加 touch 传感器：

<sensor>
    <touch name="FL_foot" site="FL_foot_site"/>
    <touch name="FR_foot" site="FR_foot_site"/>
    <touch name="RL_foot" site="RL_foot_site"/>
    <touch name="RR_foot" site="RR_foot_site"/>
</sensor>

第二步，仿真并提取数据：

import mujoco
import matplotlib.pyplot as plt
import numpy as np

model = mujoco.MjModel.from_xml_path("quadruped.xml")
data = mujoco.MjData(model)

time_steps = []
FL_force = []
FR_force = []
RL_force = []
RR_force = []

for i in range(2000):
    mujoco.mj_step(model, data)
    time_steps.append(data.time)
    FL_force.append(data.sensor("FL_foot"))
    FR_force.append(data.sensor("FR_foot"))
    RL_force.append(data.sensor("RL_foot"))
    RR_force.append(data.sensor("RR_foot"))

# 可视化
plt.figure(figsize=(12, 6))
plt.plot(time_steps, FL_force, label="左前腿", color="#3498db")
plt.plot(time_steps, FR_force, label="右前腿", color="#e74c3c")
plt.plot(time_steps, RL_force, label="左后腿", color="#2ecc71")
plt.plot(time_steps, RR_force, label="右后腿", color="#f39c12")
plt.xlabel("时间 (s)")
plt.ylabel("接触力 (N)")
plt.title("四足机器人足底接触力")
plt.legend()
plt.grid(True)
plt.show()

画出来之后，你能清楚看到每条腿的触地时序。哪个腿先着地？哪个腿离地了？力的大小是否均匀？这些信息对步态分析至关重要。

    核心要点： 传感器数据提取的本质，就是「定义 → 仿真 → 索引 → 可视化」这四步。把每一步做扎实，你就能从仿真中拿到任何你想要的数据。

4.6 常见问题与避坑指南

传感器数据全是零？ 检查传感器是否正确定义，尤其是 touch 传感器是否绑定了正确的 site。
数据索引越界？ 用 data.sensor(name) 代替下标索引，避免记错顺序。
可视化曲线不光滑？ 增加仿真步数，或者用低通滤波处理数据。
仿真速度变慢？ 不要在仿真循环里做复杂计算或绘图，先存数据，后分析。

嗯，传感器这块其实不难，但细节多。你只要把 XML 定义和 Python 提取这两步练熟了，后面做控制、做分析，就顺手多了。