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

4. 传感器与数据读取：sensordata 接口

传感器，是仿真与现实之间的桥梁。你想想看，一个机器人动起来，我们怎么知道它到底跑得怎么样？靠的就是传感器数据。MuJoCo 在这方面做得相当干脆——所有传感器数据都通过 sensordata 这个接口统一暴露出来。今天我们就把它彻底讲透。

4.1 传感器类型与注册

MuJoCo 支持三大类传感器：位置、速度、力/力矩。每种传感器都需要在 XML 模型里先注册，仿真时才能读到数据。

传感器类型	XML 标签	输出维度	典型用途
位置	`<framepos>`	3 (x,y,z)	末端执行器位姿、关节角度
速度	`<framelinvel>`	3 (vx,vy,vz)	关节角速度、线速度
力/力矩	`<force>`	6 (fx,fy,fz,tx,ty,tz)	接触力、关节力矩、足底力

关键点：传感器必须在 XML 中显式声明，MuJoCo 不会自动为所有物体创建传感器。你声明了什么，sensordata 里就有什么。

4.2 sensordata 接口详解

sensordata 是一个一维浮点数组，所有传感器的数据按注册顺序拼接在一起。读取方式很简单：

# 获取传感器数据（numpy 数组）
sensor_data = data.sensordata

# 获取传感器索引映射
sensor_id = model.sensor_name2id('my_sensor')
sensor_adr = model.sensor_adr[sensor_id]  # 在 sensordata 中的起始位置
sensor_dim = model.sensor_dim[sensor_id]  # 数据维度

# 提取该传感器的数据
my_data = data.sensordata[sensor_adr : sensor_adr + sensor_dim]

我个人习惯在仿真开始前先打印一遍所有传感器的名称和地址，确认映射关系正确。曾经有一次我搞混了力传感器和力矩传感器的顺序，结果调试了整整一个下午……

4.3 高效获取仿真数据的策略

仿真跑起来之后，数据读取的效率直接影响整体性能。我总结了三条实战经验：

批量读取，不要逐帧打印——把数据存到数组里，仿真结束后统一处理。
预分配缓冲区——提前算好总步数，用 np.zeros 分配好内存。
用索引访问，别用名字——sensor_name2id 每次调用都有开销，仿真循环里用整数索引。

我的小技巧：在仿真循环外先建一个字典，把传感器名字映射到 (adr, dim) 元组。循环里直接查字典，比反复调用 MuJoCo 的 C API 快一个数量级。

4.4 实战案例：六维力传感器数据采集

假设我们要采集机器人足底的六维力数据，用于后续的步态分析。代码结构大概是这样的：

import mujoco
import numpy as np

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

# 预计算传感器索引
sensor_map = {}
for name in ['foot_left', 'foot_right']:
    sid = model.sensor_name2id(name)
    sensor_map[name] = (model.sensor_adr[sid], model.sensor_dim[sid])

# 预分配缓冲区（假设仿真 1000 步）
n_steps = 1000
force_data = np.zeros((n_steps, 6))  # 只存左脚

# 仿真循环
for i in range(n_steps):
    mujoco.mj_step(model, data)
    adr, dim = sensor_map['foot_left']
    force_data[i] = data.sensordata[adr : adr + dim]

注意：力传感器输出的是施加在传感器所在物体上的力，方向是全局坐标系。如果你需要局部坐标系下的力，记得做旋转变换。我曾经在这个坑里摔过一次——采集到的足底力方向全是反的，后来才发现是坐标系没对齐。

4.5 传感器数据可视化与验证

光有数据还不够，你得能看懂它。我一般会在仿真结束后立刻画图验证：

位置数据——看轨迹是否平滑，有没有突变。
速度数据——检查是否在合理范围内，有没有超出关节限位。
力/力矩数据——重点关注接触瞬间的冲击峰值。

说白了，传感器数据就是仿真世界的「仪表盘」。你读得快、读得准，后面的参数校准和优化才有根基。

嗯，到这里传感器数据读取这块就讲完了。记住：索引预计算、批量存储、事后验证——这三板斧用好了，数据采集就不会出大问题。