跳转至

第 8 章 传感器系统

机器人的控制与学习都依赖观测。本章完整讨论 <sensor>:49 类传感器各占多少数据宽度、sensordata 的地址如何对应、公共属性 noise/cutoff/delay/interval 的真实语义、把读数取出来的通路与成本,以及扩展传感器的三条通道。相机、射线与 IMU 这三件器件的建模与出图实操在第 7 章,本章只在类型清单里给出它们的宽度与依赖。

1. sensordata 的布局与地址

传感器定义集中放在顶层 <sensor> 组合元素里。<sensor> 是组合元素、本身没有属性;所有传感器的输出首尾相接存进 mjData.sensordata,长度是 mjModel.nsensordata,而且这份数据不参与任何内部计算(XML reference)。

两句话定了两件事:

  1. 所有传感器的读数按首尾相接的方式存放在一维数组 sensordata 中;第 i 只传感器从 sensor_adr[i] 开始、占 sensor_dim[i] 个数。
  2. 传感器是只读旁路——它的值不参与任何物理计算。加多少只传感器都不改动力学,只改 nsensordata 与每步的传感器计算耗时(「共享属性 noise、cutoff、delay、interval」给数字)。

地址表的拼接方式如下——读数错位九成来自把其中一段的宽度算错:

flowchart LR
    s0["sensor 0<br/>adr=0,dim=1"] --> s1["sensor 1<br/>adr=1,dim=3"]
    s1 --> s2["sensor 2<br/>adr=4,dim=2"]
    s2 --> sd[("mjData.sensordata<br/>长度 = Σ sensor_dim")]
    s1 -. "按名取值:slice(adr, adr+dim)" .-> view["只读视图"]

地址不连续只是表象:每段的起点由前段宽度累加,改一只传感器的类型(dim 变)就会挪动它之后所有传感器的偏移。

<sensor> 下能放的元素共 49 类。mjtSensor 枚举 0–48 连续无空洞,与 XML reference 里 sensor/* 的 49 个小节一一对应——不存在"文档有枚举无"或反之的差异。

平铺布局按各传感器宽度依次分块,块宽就是 sensor_dim:

block-beta
    columns 7
    q["q_shoulder<br/>adr 0,dim 1"]:1
    acc["acc<br/>adr 1,dim 3"]:3
    gyro["gyro<br/>adr 4,dim 3"]:3

2. 传感器类型与输出维度

下表是 49 类传感器的最小引用对象与输出宽度(sensor_dim)速查:引用错对象编译就过不去,宽度算错读数就错位。

类型码 元素 引用什么 dim 备注
0 touch site 1 区域内接触法向力求和,非负标量
1 accelerometer site 3 site 局部线加速度,含重力
2 velocimeter site 3 site 局部线速度
3 gyro site 3 site 局部角速度(IMU 三件套的拼法见第 7 章「IMU 三件套与安装朝向」)
4 force site 3 子↔父相互作用力,site 系(六维 F/T 拼装见第 7 章「六维力/力矩传感器」)
5 torque site 3 同上的力矩(与 force 共用同一 site 即为六维 F/T)
6 magnetometer site 3 option.magnetic 投影到 site 系
7 rangefinder site 或 camera site→1;camera→宽×高 未命中返回 -1(通路见第 7 章「射线传感器与 mj_ray」)
8 camprojection site+camera 2 3D→像素,不裁剪不判遮挡(光栅化通路见第 7 章「相机的三条出图通路」)
9–10 jointpos/jointvel joint 1 只认标量关节
11–12 tendonpos/tendonvel tendon 1
13–15 actuatorpos/vel/frc actuator 1
16 jointactuatorfrc joint 1 关节空间执行器力
17 tendonactuatorfrc tendon 1
18 ballquat joint(ball) 4
19 ballangvel joint(ball) 3
20–22 jointlimitpos/vel/frc joint(limited) 1 关节必须 limited
23–25 tendonlimitpos/vel/frc tendon(limited) 1 tendon 必须 limited
26 framepos objtype/objname (+ref) 3 可换参考系
27 framequat objtype/objname (+ref) 4
28–30 framex/y/zaxis objtype/objname 3
31–32 framelinvel/frameangvel objtype/objname 3 全局
33–34 framelinacc/frameangacc objtype/objname 3 全局
35 subtreecom body 3 子树质心(全局)
36 subtreelinvel body 3 子树质心速度
37 subtreeangmom body 3 子树角动量
38 insidesite objtype/objname/site 1 在体内=1 否则=0
39 distance geom1/geom2 或 body1/body2 1 有符号最近距离
40 normal 同上 3 geom1→geom2 单位法线
41 fromto 同上 6 最近点对 (x1,y1,z1,x2,y2,z2)
42 contact geom/body/subtree/site num×Σ字段 见「三类特殊传感器」
43–44 e_potential/e_kinetic 无 1 势能/动能
45 clock 无 1 仿真时间
46 tactile geom+mesh 3×顶点数 需 ≥4 顶点的 mesh
47 plugin plugin/instance 插件定 需插件 .so
48 user dim 必填 dim 回调填

三条硬约束各自的边界:

写法 结果
<plugin name="s" plugin="nope"/>,没有同名插件已加载 插件名找不到,编译失败
<tendonlimitpos tendon="ten"/>,ten 没写 limited 腱未限位,编译失败
同一条腱补上 limited="true" 编译通过,sensor_dim=[1]
<tactile geom="link_g" mesh="tri"/>,mesh 只有 3 个顶点 顶点数不足以构成一段弹性梁,编译失败
同一只 tactile 换成四面体 mesh(4 个顶点) 编译通过,sensor_dim=[12]

即:plugin 要先加载同名插件,tendonlimit* 要求被引用的腱处于 limited,tactile 关联的 mesh 至少 4 个顶点。最后一条与表里的维度公式一致——4 顶点的四面体给出 sensor_dim=[12],正是 3 × 顶点数。

sensordata 是一块被各传感器首尾相接铺满的一维数组:每只占 sensor_dim[i] 个宽度、起点是 sensor_adr[i],中间不留空洞。四只不同宽度的传感器就能看清这个性质(两只 1 宽、两只 3 宽):

<mujoco>
  <worldbody>
    <body pos="0 0 0">
      <joint name="j0" type="slide" axis="1 0 0"/>
      <geom size="0.1"/><site name="s0"/>
      <body pos="0.5 0 0">
        <joint name="j1" type="slide" axis="0 1 0"/>
        <geom size="0.1"/><site name="s1"/>
      </body>
    </body>
  </worldbody>
  <sensor>
    <jointpos joint="j0"/>                  <!-- dim 1 -->
    <jointvel joint="j1"/>                  <!-- dim 1 -->
    <accelerometer site="s0"/>              <!-- dim 3 -->
    <framepos objtype="site" objname="s1"/> <!-- dim 3 -->
  </sensor>
</mujoco>
m = mujoco.MjModel.from_xml_string(xml)
m.nsensor, m.nsensordata                                     # (4, 8)
list(m.sensor_dim)                                           # [1, 1, 3, 3]
list(m.sensor_adr)                                           # [0, 1, 2, 5] —— 无缝拼接
int(m.sensor_adr[-1]) + int(m.sensor_dim[-1]) == m.nsensordata  # True,末地址+末维度正好铺满

把除 plugin 外的 48 类同时装进一个模型也成立:nsensor=48、nsensordata=114,且 sensor_adr 单调递增、首尾相接。正因没有空洞,就没有"按传感器编号当索引"的余地(这是最常见的读数错位根因,见「共享属性 noise、cutoff、delay、interval」)。

3. 三类特殊传感器

3.1 contact 与变长接触

contact 的定位很实在:主动力学管线里的接触数组本质上是变长的,而接触传感器的任务就是把这些信息报告进固定大小的数组(XML reference)。

它按 匹配 → 化简 → 提取 三段工作:geom1/2、body1/2、subtree1/2、site 做交集匹配;reduce 把匹配数化到恰好 num 个槽位;data 指定每槽位提取哪些字段。输出宽度 = num × Σ(字段维),data 字段顺序被强约束:

data 可以一次列多个字段,但要保持规定的相对顺序(XML reference)。

顺序违规直接被拒:

<!-- 匹配 geom1×geom2,槽位数固定 5;data 列三个字段 ⇒ 输出宽度 5×(1+1+3)=25 -->
<contact name="c" geom1="box" geom2="floor" num="5" data="found dist force"/>

字段必须按 found, force, torque, dist, pos, normal, tangent 这个顺序写。改成 data="found force dist" 就编译通过,sensor_dim 变成 25 ——正是 num(5 个槽位)乘每槽字段数(1 + 3 + 1)。

真正压在地上时,contact 的 found 报告的是匹配到的接触总数(箱子四角压地 ⇒ found=5,因为多了一个 plane-capsule 计数),而 force 字段在 reduce=none(默认,按 contact 出现顺序)下会把没有约束行的槽位读成 0。换成 maxforce/netforce 才拿到有意义合力:

写法 sensor_dim 槽位 0 的读数 含义
默认 data="found" 1 [5.] 匹配到 5 个接触
reduce="maxforce" 5 [5. 0.981 0. -0. -0.0004] 力最大的接触被排到槽位 0
reduce="netforce" 4 [5. 0. 0. -0.981] 槽位 0 的力 = 全部匹配接触的合力

netforce 的 z 分量 −0.981 N 正等于箱子质量 0.1 kg × 9.81,是把 5 个接触合成一个"力螺旋"后的净竖直力。contact 传感器忽略 cutoff(sensor 文档)——cutoff=1.0 既不截 dist 也不把 found 归零(该行读数为 [5. 0.])。

3.2 碰撞类传感器的 cutoff

这三类采用窄相 geom-geom 距离检测器,独立于标准碰撞流水线。其 cutoff 语义与常规相反——不是饱和上限,而是可检出的最大距离 dismax:

区别正在此处:普通传感器的 cutoff 仅对读出的值做截断,碰撞传感器的 cutoff 定义"最远能检测到多大距离",也就是 dismax(XML reference)。

例如:两个 geom 实际相距 0.353 m,而 cutoff=0.05 的那只 distance 读数为 0.05——既不是哨兵值也不是 0,而是 cutoff 本身;normal 未命中返回 (0,0,0),fromto 未命中返回 6 个 0。箱子正好贴地(距离≈0)时 distance=0,而 cutoff=0 意味着"只报穿透",所以未穿透的 normal 读成 (0,0,0)——要拿非穿透面上的法线,必须给正的 cutoff。这是碰撞类传感器最容易踩的坑:cutoff=0 不等于"不截断",而是"只检穿透"。

3.3 tactile 的输出宽度与匹配粒度

tactile 关联一个 geom 和一个带法线的 mesh,mesh 顶点放在该 geom 的坐标系里作为采样点, 每个顶点输出 3 个数:最大穿透深度 + 切向框架里的两个滑移速度,所以输出宽度 = 顶点数 × 3。 它只对 SDF geom 的接触生效(XML reference)。三点补充( 用安装库里的 model/tactile/tactile.xml):

  • 输出宽度:两只触觉传感器的 sensor_dim 都是 4107 = 3×1369(采样网格是 37×37 的内置 wedge/plate)。 顶点数不足 4 无法编译;4 顶点的网格给出 sensor_dim=12。
  • 接触按 body(weld)匹配,而不是按那只被关联的 geom。那份样例里触觉 mesh geom 写成 mass=0 contype=0 conaffinity=0 的装饰件,真正与齿轮 SDF 接触的是同体的另一只 primitive geom (接触对读出来是 ('geom#2','gear')、('geom#4','gear')),触觉读数照常出现: 稳定接触时球面有 178/1369 个顶点非零、指面 897/1369。源码依据是 src/engine/engine_sensor.c 里先取传感器关联 geom 的 geom_bodyid → body_weldid, 再遍历 d->contact 收集同一 weld 上的接触。⇒ 别指望靠 contype=0 屏蔽掉一只体上的触觉; 反过来,把采样 mesh 设成装饰件是正解(让它参与碰撞反而会失败:平面网格建凸包会直接被拒)。
  • 读数不是 mj_forward 一跑就有。needstage 是 position 阶段,但样例里球初始悬空, 只做一次 mj_forward 时全部为零;接触建立之后每拍才有值。禁用位 <flag sensor="disable"/> 会让整条读数冻结在上一拍(逐元素相等)。

一个仍未澄清的点先记在这:文档把第一个分量叫"最大穿透深度",并说穿透深度为正表示未接触、 该顶点全为 0;稳定接触时该分量取的是 0 到 +0.0126 的非负值,且与两个切向分量同量级 (最深那行为 [0.012553, 0.012145, 0.011197])。符号约定与量纲归属文档没有说明, 判定该顶点是否参与接触,只看这一向量是否全为零;各分量的物理单位不参与判定。

4. 共享属性 noise、cutoff、delay、interval

<sensor> 大部分元素共享一组属性:name, noise, cutoff, nsample, interval, delay, user。它们的默认行为需要将状态固定后反复读取读数才能测清。

4.1 sensor_noise 的当前语义

changelog 的 3.1.4 段落写得很明确:移除了向传感器原生添加噪声的能力,但 mjModel.sensor_noise 字段与对应属性都还保留着,现在的定位是"给用户存标准差、供自己使用"的方便位置。

这一点可直接看出:一个 slide 关节写 noise="0.1",把 qpos 每步写回 0.0(重力清零),连跑 2000 步——sensordata 的 2000 个读数全为 0,标准差严格 0,而且两个独立进程(不同全局随机种子)逐位相同。

标准差严格 0 ⇒ noise 只是存进 model.sensor_noise 的元数据,sensordata 里没有噪声。要噪声得自己加(第 9 章给出 wrapper 写法)。

4.2 cutoff 的饱和限幅

cutoff="0.05" 的 jointpos,将 qpos 从 0 单步跳变到 0.3,输出为 [0.05],第一拍即达上限;qpos 改为 −0.4 则输出 [-0.05],限幅双侧对称。普通传感器的 cutoff 就是对 |值| 的对称饱和,且当步生效、不做低通。势能动能一对同样说明截断发生在读出之后:e_potential 读 [0.387],被 cutoff=0.2 夹到 [0.2]。这与「三类特殊传感器」碰撞类里 cutoff=dismax 是两套语义,别混。

4.3 delay、interval 与 nsample

首先受一条硬约束限制——仅给出 delay 而不配置历史缓冲:

<!-- 仅写 delay 不足:历史缓冲由 nsample 决定,缺失则延迟功能无法启用 -->
<sensor><jointpos name="s" joint="j" delay="0.05" nsample="10"/></sensor>

delay 必须有历史缓冲才能用,而历史缓冲由 nsample(或 interval)开启:只给 delay 不给这两个之一,模型就编译不过。delay="0.05" nsample="2" 与只给 interval="0.02" 两种写法都能编译通过。把三者配齐后扫 delay,看阶跃输入第几拍才非零(timestep=0.002):

delay 首次非零出现在 对应时刻
0.001 与 0.002 第 1 拍 0.002 s
0.004 第 2 拍 0.004 s
0.05 第 25 拍 0.050 s
0.1 第 50 拍 0.100 s

delay 以秒为单位,向上取整到时间步边界(0.001 和 0.002 都在第 1 拍)。nsample>1 则对窗口取平均:给斜坡输入 qpos=0.002·k,interval="0.02" nsample="5" 读出恒定的 0.100(窗内均值),而 nsample="1" 读最新值——想清楚要"平均"还是"抽样保持"再选 nsample。

执行器同享这套机制(<general> 家族的 nsample/delay):延迟的是 ctrl 的读取——forward dynamics 从缓冲里取 time − delay 时刻的命令。position 执行器(nsample=50)的阶跃首动拍对照:delay=0 在第 1 拍、0.01 在第 6 拍、0.05 在第 26 拍,即延迟拍数 = delay/timestep + 1(缓冲先读后写);

delay=0.0001 这类亚拍值上取整为 1 拍,所以最小正延迟就是一个 timestep。缓冲本体是 mjData.history,随物理状态一起推进——只在 mj_step 时写入,mj_forward 不写。nsample>0 且 delay=0 是 "history-only" 模式:仿真不受影响,但 mj_readSensor/mj_readCtrl 可读回任意时刻的缓冲值(time−0.006 读回的值与该时刻的 sensordata 逐位相等),排查延迟链路时用它做离线回放。

reftype/refname:给 frame 换参考系*

framepos 支持可选的 reftype/refname,把读数从全局坐标改到某个参考坐标系。设参考系后,同一只传感器的读数从全局的 [0.2, 0, 0.5] 变成相对锚点的 [0.15, -0.02, 0.2],与手算的"全局位置减锚点位置"逐分量相等。没设参考系的传感器在 model.sensor_refid 里记为 -1(此处三个传感器是 [-1, -1, 1],只有最后一只挂在锚点上)。相对值是 R_refᵀ(全局位置 − 参考位置),这里锚点无旋转,退化成做差。

关掉传感器:mjDSBL_SENSOR

禁用位生效时读数完全冻结——fp 停在上一拍的 [0.2, 0.2, 0] 不动;重新启用后一拍就跟上当前状态(读到 [0.5, 0, 0.5])。另注意 clock 的读数(0.098)比 data.time(0.100)小一个时间步:它归属的是本步开始时刻,因为传感器在积分之前算(呼应第 5 章「mj_step 的阶段划分」)。

坑:加传感器不改动力学,改宽度就全错位

传感器是只读旁路,加多少只都不影响物理;但每段的起点由前段宽度累加——改一只传感器的类型就会挪动其后所有传感器的 adr,按旧偏移切片读到的是别人的数据。

5. 读数通路与成本

读取 sensordata 的三种写法,成本与适用场景各不相同:

## (a) 老 C 风格:adr + dim 切片。名字→地址要先缓存。
sid  = mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_SENSOR, "tcp_pos")
adr, dim = int(m.sensor_adr[sid]), int(m.sensor_dim[sid])
val  = d.sensordata[adr:adr + dim]

## (b) Python 视图:按名字/编号取,返回的是绑定到 sensordata 的只读视图。
val  = np.array(d.sensor("tcp_pos").data)

## (c) 遍历全表建索引,一次性缓存所有 (adr, dim, type)
index = {
    mujoco.mj_id2name(m, mujoco.mjtObj.mjOBJ_SENSOR, i):
        (int(m.sensor_adr[i]), int(m.sensor_dim[i]), int(m.sensor_type[i]))
    for i in range(m.nsensor)
}

"用传感器编号 i 当 sensordata 下标"是经典错位——编号是第几只,地址是 sensor_adr[i],两者只在每只都恰好 1 维时才重合。三种读法的单次成本差得不少(同一模型上测,mj_step 基准 2.8–3.3 µs/步):

取法 单次耗时 注
d.sensor("tcp_pos").data 视图 0.171 µs 最便宜
sensordata[adr:adr+dim] 切片 0.173 µs adr 得自己缓存
按编号取 m.sensor_adr[i] / sensor_dim[i] 0.672 µs 比字段视图贵 4 倍
mj_name2id(SENSOR, name) 0.860 µs 放进每拍循环里就错
按名字取整个传感器包装 1.552 µs 最贵的一条

三条结论:

  • 传感器计算本身几乎免费:从 4 只到 50 只 framepos,每步只从 2.82 µs 涨到 3.26 µs(+0.44 µs)。
  • Python 侧读取才是主要开销,且按名字读(1.552 µs)比按地址切片(0.672 µs)贵 2 倍多,比直接读 data.qpos 字段贵近 9 倍。
  • 别在每拍循环里调 mj_name2id(0.86 µs)——把名字→(adr,dim) 在初始化时缓存成字典。这条与第 5 章「健康检查项与成本」的"健康检查读 MjData 要缓存、要与物理同线程"一脉相承。

49 类按数据来源分三种,排查时先分清是哪一种:

  • 引擎为本征计算的:touch/force/torque/accelerometer/gyro/velocimeter/magnetometer/rangefinder/camprojection/insidesite/distance/normal/fromto/contact/tactile。这些需要额外算一遍量,其中 force/torque/accelerometer/framelinacc/frameangacc 会触发 mj_rnePostConstraint,subtree 类触发 mj_subtreeVel(出处为 sensor 文档各小节)。
  • 只是把已算好的量复制进 sensordata 的:jointpos/jointvel/tendon*/actuator*/ball*/frame*、e_potential/e_kinetic/clock。文档在 jointpos 小节明说:

"此元素及剩余的传感器元素不涉及特定于传感器的计算。相反,它们将已计算的量复制到数组 mjData.sensordata 中。"

  • 需要用户或插件供给的:user(回调 mjcb_sensor 填)、plugin(引擎插件填)。

第一类"触发额外反向动力学"的含义要留意:即使模型里没有这些传感器,mjData.cacc / cfrc_int / cfrc_ext 这些量在 mj_step 里也照样算(文档 §sensor 开头),传感器只是把其中一部分搬到 sensordata。想在 C 层直接取而不建传感器,是允许的。

6. 扩展传感器的三条通道

MuJoCo 内置 49 类之外的传感器,有三条扩展通道,可验证程度不同:

  1. sensor/user + 回调 mjcb_sensor(可运行验证)。XML 只声明"占几维、挂在哪个对象、需要哪个阶段",值由回调填。文档:

"MuJoCo 不知道如何计算此传感器的输出。相反,用户应该安装回调函数 mjcb_sensor,该函数预计会填充 mjData.sensordata 中的传感器数据。"

Python 侧用 mujoco.set_mjcb_sensor(fn),回调签名是 fn(model, data, phase)(第三个参数是触发阶段 mjSTAGE)。行为是:一步触发一次,写入即读出,phase=1(pos 阶段,因为 needstage="pos")。两个要点:引擎每步先把 user 传感器槽位置 0,回调不写就是 0(不残留上一拍);datatype="quaternion" 不再自动归一化(写进去范数 1.000025 的四元数,读出来范数还是 1.000025),因为归一化本是服务于已移除的加噪功能。

def sensor_cb(model, data, phase):
    adr = int(model.sensor_adr[0])
    data.sensordata[adr]   = data.qpos[0] * 2.0      # 自己定义"虚拟量"
    data.sensordata[adr+1] = data.qvel[0] + 1.0
mujoco.set_mjcb_sensor(sensor_cb)
  1. sensor/plugin(API 已在,需编译插件 .so)。mujoco.mj_loadPluginLibrary / mj_loadAllPluginLibraries 都已绑定,mjModel 暴露了 nplugin / plugin / plugin_attr / sensor_plugin 等整组字段;本模型没加载插件时 sensor_plugin 全为 −1。传入不存在的库路径会当场失败(共享对象打不开就立刻终止),不会静默跳过。

插件能注册自定义传感器/执行器/SDF 能力位(mjtPluginCapabilityBit.mjPLUGIN_SENSOR),但需要一个编译好的动态库;端到端接法标外部证据。

  1. 第三方库层。不改 MJCF,而是在 Python 侧把传感器读数加工成观测/接口:gymnasium 用 ObservationWrapper 读 sensordata(第 9 章「传感器接成观测」给出可运行实现),mujoco_ros2_control 之类桥接把传感器映射成 ROS 2 topic(第 9 章)。这些生态包一律标"待核实"。

7. 要点

  • 49 类内置传感器的读数依次存放在同一条 sensordata 中,用 sensor_adr/sensor_dim 定位;按编号当下标必错位。
  • noise 自 3.1.4 起不再由引擎施加,sensor_noise 只是留给用户的元数据;要噪声自己加,且 quaternion 不再自动归一化。
  • cutoff 有两套语义:普通传感器 = 对称饱和;碰撞类(distance/normal/fromto)= 检出的最大距离 dismax,未命中分别返回 cutoff / 0向量 / 6个0。
  • delay 以秒计、向上取整到时间步;delay>0 必须有历史缓冲(靠 nsample/interval 开启);nsample>1 取窗口均值。
  • 传感器计算近乎免费,Python 侧按名字读最贵——名字→地址缓存、与物理同线程读。
  • 扩展走三条路:user+mjcb_sensor(每步先清零槽位)、plugin(API 在、待 .so)、第三方库层(gymnasium/ROS 2,见第 9 章)。

参考文献与延伸阅读

  • Todorov, E., Erez, T., & Tassa, Y. MuJoCo: A Physics Engine for Model-Based Control. IROS, 2012. DOI
  • MuJoCo. 文档入口(传感器与插件). 文档
  • MuJoCo. 源码仓库. 仓库