feat(pybot×actpred): R2 多 MPU6050 支持——8×N 姿态向量拼接与归一化平铺
- robot：_find_imus/_read_imus 按脚本块行序收集拼接 ≤4 个 MPU6050，_pad_imu 对齐 predictor.imu_dim - recorder/imu_norm/dataset/config：imu.npy 宽度 8×N，归一化除数按传感器数平铺 - 测试：双传感器闭环拼接/pad/truncate、宽布局录制、16 维归一化加载
This commit is contained in:
qing committed
1 parent
b289e7d17c
commit
cd4d512ecd
10 files changed
+234
-56
No files matched your search
+45
-35
@@ -160,15 +160,18 @@ class RobotStruct:
|
||||
delay_idx = self._find_delay_idx(columns)
|
||||
|
||||
camera_var = None
|
||||
imu_var = None
|
||||
imu_vars = []
|
||||
if recorder is not None:
|
||||
camera_var = self._find_camera()
|
||||
if camera_var is None and not self.dry_run:
|
||||
raise ValueError(
|
||||
"--record 需要 @Camera 块（或使用 --dry-run 生成合成帧）")
|
||||
imu_var = self._find_imu()
|
||||
if imu_var is None:
|
||||
imu_vars = self._find_imus()
|
||||
if not imu_vars:
|
||||
log.info("--record: 无 @MPU6050_BIND 块，不产出 imu.npy")
|
||||
else:
|
||||
# 槽位宽度 8×N（按块顺序拼接），录制前同步给 recorder
|
||||
recorder.imu_dim = len(IMU_KEYS) * len(imu_vars)
|
||||
|
||||
prev_vals = None
|
||||
|
||||
@@ -189,7 +192,7 @@ class RobotStruct:
|
||||
self._grab_frame(camera_var),
|
||||
self._build_action_vec(
|
||||
step_vals, recorder.action_dim),
|
||||
imu_vec=self._read_imu(imu_var))
|
||||
imu_vec=self._read_imus(imu_vars))
|
||||
self._execute_step(step_vals, columns)
|
||||
if self.dry_run:
|
||||
import time
|
||||
@@ -199,7 +202,7 @@ class RobotStruct:
|
||||
recorder.record(
|
||||
self._grab_frame(camera_var),
|
||||
self._build_action_vec(vals, recorder.action_dim),
|
||||
imu_vec=self._read_imu(imu_var))
|
||||
imu_vec=self._read_imus(imu_vars))
|
||||
self._execute_step(vals, columns)
|
||||
|
||||
prev_vals = vals
|
||||
@@ -219,7 +222,8 @@ class RobotStruct:
|
||||
pybot 不 import 具体推理类，保持零依赖承诺。
|
||||
若额外声明 imu_dim 属性（R2 方案一），则以
|
||||
predict(images, action_history, imu=(imu_dim,)) 调用；
|
||||
脚本无 @MPU6050_BIND 块时 imu 为零向量。
|
||||
脚本无 @MPU6050_BIND 块时 imu 为零向量。多个 MPU6050
|
||||
（最多 4 个）按脚本块行顺序拼接为 8×N 原始向量。
|
||||
hz: 控制频率（周期 = 1/hz 秒，须 ≥ 推理 + 执行耗时）
|
||||
max_steps: 步数上限，None 表示无限循环（Ctrl-C 退出）
|
||||
recorder: 可选 EpisodeRecorder，闭环中同步录制 (frame, action)
|
||||
@@ -239,17 +243,19 @@ class RobotStruct:
|
||||
if camera_var is None and not self.dry_run:
|
||||
raise ValueError("闭环需要 @Camera 块（或使用 --dry-run 生成合成帧）")
|
||||
|
||||
imu_var = self._find_imu()
|
||||
imu_vars = self._find_imus()
|
||||
# 鸭子类型约定：predictor 声明 imu_dim 属性即接受 imu= 关键字
|
||||
# （方案一：独立 IMU token）；未声明则保持旧两参调用，向后兼容
|
||||
use_imu = getattr(predictor, "imu_dim", None) is not None
|
||||
if imu_var is None:
|
||||
if not imu_vars:
|
||||
if use_imu:
|
||||
log.warning("predictor 声明 imu_dim 但脚本无 @MPU6050_BIND "
|
||||
"块，imu 将以零向量输入（等价于无姿态先验）")
|
||||
else:
|
||||
if recorder is not None:
|
||||
recorder.imu_dim = len(IMU_KEYS) * len(imu_vars)
|
||||
log.info("closed loop imu: %s → predictor %s",
|
||||
imu_var.name,
|
||||
",".join(v.name for v in imu_vars),
|
||||
"imu= 接入" if use_imu else "不接入（仅录制）")
|
||||
|
||||
action = self.find_mt("Action")
|
||||
@@ -278,10 +284,10 @@ class RobotStruct:
|
||||
while max_steps is None or step < max_steps:
|
||||
t0 = time.monotonic()
|
||||
frames.extend(self._loop_recent_frames(camera_var, F)) # 1 采样
|
||||
imu = self._read_imu(imu_var)
|
||||
imu = self._read_imus(imu_vars)
|
||||
if use_imu: # 2 推理
|
||||
act = predictor.predict(list(frames), np.array(history),
|
||||
imu=self._imu_to_vec(
|
||||
imu=self._pad_imu(
|
||||
imu, predictor.imu_dim))
|
||||
else:
|
||||
act = predictor.predict(list(frames), np.array(history))
|
||||
@@ -419,38 +425,42 @@ class RobotStruct:
|
||||
return var
|
||||
return None
|
||||
|
||||
def _find_imu(self):
|
||||
"""在已创建的硬件变量中查找第一个 MPU6050Var（无则 None）"""
|
||||
for var in self.M.values():
|
||||
if isinstance(var, MPU6050Var):
|
||||
return var
|
||||
return None
|
||||
def _find_imus(self):
|
||||
"""按脚本块顺序收集全部 MPU6050Var（无则空列表，最多 4 个）"""
|
||||
return [v for v in self.M.values() if isinstance(v, MPU6050Var)]
|
||||
|
||||
def _read_imu(self, imu_var):
|
||||
"""读 MPU6050 最新姿态 dict（键序见 IMU_KEYS）
|
||||
def _read_imus(self, imu_vars):
|
||||
"""读全部 MPU6050 最新姿态，按块顺序拼接为 (8×N,) 原始向量
|
||||
|
||||
无硬件 / 读失败返回 None（recorder 记零向量、predictor 侧零输入）。
|
||||
latest() 冷启动返回全零 dict，语义安全无需特判。
|
||||
imu_vars 为空返回 None（无传感器：recorder 不产出 imu.npy）；
|
||||
单个传感器读失败其 8 槽记零并 log。latest() 冷启动返回全零
|
||||
dict，语义安全无需特判。
|
||||
"""
|
||||
if imu_var is None:
|
||||
return None
|
||||
try:
|
||||
return imu_var.play({})
|
||||
except Exception:
|
||||
log.exception("imu read failed")
|
||||
if not imu_vars:
|
||||
return None
|
||||
import numpy as np
|
||||
nk = len(IMU_KEYS)
|
||||
out = np.zeros(nk * len(imu_vars), dtype=np.float32)
|
||||
for k, var in enumerate(imu_vars):
|
||||
try:
|
||||
latest = var.play({})
|
||||
except Exception:
|
||||
log.exception("imu read failed: %s", var.name)
|
||||
continue
|
||||
out[k * nk:(k + 1) * nk] = [latest.get(key, 0.0)
|
||||
for key in IMU_KEYS]
|
||||
return out
|
||||
|
||||
@staticmethod
|
||||
def _imu_to_vec(imu, imu_dim):
|
||||
"""姿态 dict → np 向量（按 IMU_KEYS 顺序 pad/truncate 到 imu_dim）"""
|
||||
def _pad_imu(vec, imu_dim):
|
||||
"""拼接姿态向量 → pad/truncate 到 predictor 的 imu_dim（None → 全零）"""
|
||||
import numpy as np
|
||||
vec = np.zeros(imu_dim, dtype=np.float32)
|
||||
if imu is not None:
|
||||
src = np.asarray([imu.get(k, 0.0) for k in IMU_KEYS],
|
||||
dtype=np.float32)
|
||||
out = np.zeros(imu_dim, dtype=np.float32)
|
||||
if vec is not None:
|
||||
src = np.asarray(vec, dtype=np.float32).ravel()
|
||||
n = min(len(src), imu_dim)
|
||||
vec[:n] = src[:n]
|
||||
return vec
|
||||
out[:n] = src[:n]
|
||||
return out
|
||||
|
||||
def _build_action_vec(self, vals, action_dim):
|
||||
"""从值列表构建动作向量，pad/truncate 到 action_dim"""
|
||||
|
||||
Reference in new issue
Block a user