diff --git a/pybot/pybot/robot.py b/pybot/pybot/robot.py index 2ba2b82..6ae6a6d 100644 --- a/pybot/pybot/robot.py +++ b/pybot/pybot/robot.py @@ -136,14 +136,22 @@ class RobotStruct: return None def run(self, recorder=None): - """逐行执行 @Action 矩阵 + """逐行执行 @Action 矩阵，带时延的行分解为 10ms 微步 - recorder: 可选 EpisodeRecorder，启用后每行执行前录制 (frame, action) + 微步规则： + - 首行（无前序状态）：不分解，按原逻辑执行 + - 后续行 + delay > 0：分解为 N=delay//10 个微步，舵机角度线性插值 + - delay 必须是 10 的倍数 + - 每步调 speaker.play(freq, 10) 提供声音 + 10ms 时序 + - dry_run 下补 sleep(0.01) 维持节拍 + + recorder: 可选 EpisodeRecorder，每步录制 (frame, action) """ action = self.find_mt("Action") if action is None: raise ValueError("no @Action matrix in script") columns = self._validate_action(action) # Q05: 执行前整表校验 + delay_idx = self._find_delay_idx(columns) camera_var = None if recorder is not None: @@ -152,30 +160,84 @@ class RobotStruct: raise ValueError( "--record 需要 @Camera 块（或使用 --dry-run 生成合成帧）") - for i in range(1, action.size): - # 录制：在执行动作前抓帧（observation → action 语义） - if recorder is not None: - action_vec = self._build_action_vec( - action, i, columns, recorder.action_dim) - frame = self._grab_frame(camera_var) - recorder.record(frame, action_vec) + prev_vals = None - row_sets = {} # {设备名: {参数名: float}} - for j, (dev, param) in enumerate(columns): - # 可能的参数别名转换（A → angle） - alias = self.M.get(param) - if isinstance(alias, AliasVar): - param = alias.play({}) - val = parse_mt(action, i, j) - row_sets.setdefault(dev, {})[param] = val - # 执行一行：逐设备调用 play - for dev, params in row_sets.items(): - self.M[dev].play(params) + for i in range(1, action.size): + vals = self._resolve_row(action, i, len(columns)) + delay = int(vals[delay_idx]) if delay_idx is not None else 0 + + if delay > 0 and prev_vals is not None: + if delay % 10 != 0: + raise ValueError( + f"@Action row {i}: delay {delay} 不是 10 的倍数") + n = delay // 10 + for k in range(n): + step_vals = self._build_step_vals( + prev_vals, vals, columns, k, n) + if recorder is not None: + recorder.record( + self._grab_frame(camera_var), + self._build_action_vec( + step_vals, recorder.action_dim)) + self._execute_step(step_vals, columns) + if self.dry_run: + import time + time.sleep(0.01) + else: + if recorder is not None: + recorder.record( + self._grab_frame(camera_var), + self._build_action_vec(vals, recorder.action_dim)) + self._execute_step(vals, columns) + + prev_vals = vals if recorder is not None: path = recorder.flush() log.info("episode saved: %s", path) + def _resolve_row(self, action, row, n_cols): + """解析一行的全部值（占位符继承）""" + return [parse_mt(action, row, j) for j in range(n_cols)] + + def _resolve_param(self, param): + """解析别名 → 真实参数名""" + alias = self.M.get(param) + if isinstance(alias, AliasVar): + return alias.play({}) + return param + + def _find_delay_idx(self, columns): + """找到 spk.delay 列索引；无 Speaker 返回 None""" + for j, (dev, param) in enumerate(columns): + if isinstance(self.M.get(dev), SpeakerVar) and \ + self._resolve_param(param) == "delay": + return j + return None + + def _build_step_vals(self, prev_vals, target_vals, columns, k, n): + """为微步 k 构建插值后的值列表""" + vals = [] + for j, (dev, param) in enumerate(columns): + rp = self._resolve_param(param) + if isinstance(self.M.get(dev), ServoVar): + vals.append(prev_vals[j] + + (target_vals[j] - prev_vals[j]) * (k + 1) / n) + elif isinstance(self.M.get(dev), SpeakerVar) and rp == "delay": + vals.append(10.0) + else: + vals.append(target_vals[j]) + return vals + + def _execute_step(self, vals, columns): + """从值列表构建 row_sets 并逐设备 play()""" + row_sets = {} + for j, (dev, param) in enumerate(columns): + rp = self._resolve_param(param) + row_sets.setdefault(dev, {})[rp] = vals[j] + for dev, params in row_sets.items(): + self.M[dev].play(params) + def _find_camera(self): """在已创建的硬件变量中查找第一个 CameraVar""" for var in self.M.values(): @@ -183,16 +245,12 @@ class RobotStruct: return var return None - def _build_action_vec(self, action, row, columns, action_dim): - """按 @Action 列顺序构建动作向量，pad/truncate 到 action_dim - - 与 actpred/pretrain_mt.py 的 MTActionDataset 语义一致： - 列值按顺序填入向量，不足补零，超出截断。 - """ + def _build_action_vec(self, vals, action_dim): + """从值列表构建动作向量，pad/truncate 到 action_dim""" import numpy as np vec = np.zeros(action_dim, dtype=np.float32) - for j in range(min(len(columns), action_dim)): - vec[j] = parse_mt(action, row, j) + n = min(len(vals), action_dim) + vec[:n] = vals[:n] return vec def _grab_frame(self, camera_var): diff --git a/pybot/tests/test_recorder.py b/pybot/tests/test_recorder.py index fdd8296..81e30e5 100644 --- a/pybot/tests/test_recorder.py +++ b/pybot/tests/test_recorder.py @@ -1,4 +1,4 @@ -"""EpisodeRecorder 测试：actpred 格式数据集录制 +"""EpisodeRecorder 测试：actpred 格式数据集录制 + 微动作分解 全部在 dry_run / mock I2C 下运行，不碰真实硬件。 """ @@ -15,7 +15,7 @@ from pybot.robot import RobotStruct # ---- 测试用 .mt 脚本 ---- -# 仅 @Camera + @Action(cam0.frame) —— 不需要 I2C +# 仅 @Camera + @Action(cam0.frame) —— 不需要 I2C，无 Speaker 无微动作 CAMERA_MT = """ @Camera name width height framerate capacity; @@ -28,11 +28,14 @@ cam0.frame; @ """ -# @Camera + @Servo + 多行 @Action（含占位符）—— 需要 mock I2C +# @Camera + @Servo + @Speaker + 多行 @Action（含占位符 + delay）—— 需要 mock I2C +# 10 行 @Action，首行不分解，后续行 delay=20 → N=2 微步 +# 总步数：1 + 9×2 = 19 SERVO_CAMERA_MT = """ @Alias name alias; A angle; + F frequency; @ @I2C @@ -51,23 +54,99 @@ pca9685-0 i2c-1 0x40; s2 180 - - - 1; @ +@Speaker +name channels sample_rate volumn max_f min_f; + spk 2 48000 0.6 5000 -; +@ + @Camera name width height framerate capacity; cam0 320 240 15 2; @ @Action -s1.A s2.A; - 45 0; - - 10; - - 20; - - 30; - - 0; - - 10; - - 20; - - 30; - - 0; - - 10; +s1.A s2.A spk.F spk.delay; + 45 0 10 20; + - 10 - 20; + - 20 - 20; + - 30 - 20; + - 0 - 20; + - 10 - 20; + - 20 - 20; + - 30 - 20; + - 0 - 20; + - 10 - 20; +@ +""" + +# 简单插值测试：2 行 @Action，delay=30 → N=3 +# s1: row1=0（首行不分解），row2 从 0→30 分 3 步 → 10, 20, 30 +MICRO_ACTION_MT = """ +@Alias +name alias; + A angle; + F frequency; +@ + +@I2C + name path; +i2c-1 /dev/i2c-1; +@ + +@PCA9685 + name I2C address; +pca9685-0 i2c-1 0x40; +@ + +@Servo + name max min offset controller channel; + s1 90 - - pca9685-0 0; +@ + +@Speaker +name channels sample_rate volumn max_f min_f; + spk 2 48000 0.6 5000 -; +@ + +@Action +s1.A spk.F spk.delay; + 0 10 30; + 30 - 30; +@ +""" + +# delay 非 10 倍数 → 应报错 +BAD_DELAY_MT = """ +@Alias +name alias; + A angle; + F frequency; +@ + +@I2C + name path; +i2c-1 /dev/i2c-1; +@ + +@PCA9685 + name I2C address; +pca9685-0 i2c-1 0x40; +@ + +@Servo + name max min offset controller channel; + s1 90 - - pca9685-0 0; +@ + +@Speaker +name channels sample_rate volumn max_f min_f; + spk 2 48000 0.6 5000 -; +@ + +@Action +s1.A spk.F spk.delay; + 0 10 15; + 30 - 15; @ """ @@ -110,11 +189,11 @@ def test_episode_format(tmp_path): assert (ep_dir / "images").is_dir() assert (ep_dir / "actions.npy").exists() jpgs = list((ep_dir / "images").glob("*.jpg")) - assert len(jpgs) == 1 # CAMERA_MT 有 1 行 @Action + assert len(jpgs) == 1 # CAMERA_MT 有 1 行 @Action，无 Speaker 不分解 def test_action_vector_padding(tmp_path, mock_i2c): - """2 列 .mt → 18 维向量，末尾 16 维为零""" + """4 列 .mt → 18 维向量，末尾 14 维为零；微步展开后 19 步""" robot = RobotStruct(SERVO_CAMERA_MT, dry_run=True) robot.active() rec = EpisodeRecorder(str(tmp_path), action_dim=18) @@ -122,12 +201,12 @@ def test_action_vector_padding(tmp_path, mock_i2c): robot.deactive() actions = np.load(tmp_path / "episode_0000" / "actions.npy") - assert actions.shape == (10, 18) # 10 行 @Action × 18 维 - assert np.all(actions[:, 2:] == 0) # 列 2-17 全为零 + assert actions.shape == (19, 18) # 1（首行）+ 9×2（微步）= 19 + assert np.all(actions[:, 4:] == 0) # 列 4-17 全为零 def test_placeholder_resolution(tmp_path, mock_i2c): - """`-` 占位符正确递归解析""" + """`-` 占位符递归解析 + 微步插值""" robot = RobotStruct(SERVO_CAMERA_MT, dry_run=True) robot.active() rec = EpisodeRecorder(str(tmp_path), action_dim=18) @@ -135,10 +214,10 @@ def test_placeholder_resolution(tmp_path, mock_i2c): robot.deactive() actions = np.load(tmp_path / "episode_0000" / "actions.npy") - # s1.A: row0=45, row1..9 全是 '-' → 递归取上一行 → 全 45 + # s1.A: 全 45（首行 45，后续行 '-' 继承 45，插值 45→45 仍为 45） assert np.all(actions[:, 0] == 45) - # s2.A: 0,10,20,30,0,10,20,30,0,10 - expected_s2 = [0, 10, 20, 30, 0, 10, 20, 30, 0, 10] + # s2.A: 首行 0，后续行插值 + expected_s2 = [0, 5, 10, 15, 20, 25, 30, 15, 0, 5, 10, 15, 20, 25, 30, 15, 0, 5, 10] assert list(actions[:, 1]) == expected_s2 @@ -182,6 +261,30 @@ def test_episode_auto_increment(tmp_path): assert (tmp_path / "episode_0001").is_dir() +def test_micro_action_interpolation(tmp_path, mock_i2c): + """微动作插值：s1 从 0→30 分 3 步 → 10, 20, 30""" + robot = RobotStruct(MICRO_ACTION_MT, dry_run=True) + robot.active() + rec = EpisodeRecorder(str(tmp_path), action_dim=18) + robot.run(recorder=rec) + robot.deactive() + + actions = np.load(tmp_path / "episode_0000" / "actions.npy") + # 首行 s1=0（不分解），row2 s1 从 0→30 分 3 步 + expected_s1 = [0, 10, 20, 30] + assert list(actions[:, 0]) == expected_s1 + + +def test_delay_not_multiple_of_10(tmp_path, mock_i2c): + """delay 非 10 倍数时报错""" + robot = RobotStruct(BAD_DELAY_MT, dry_run=True) + robot.active() + rec = EpisodeRecorder(str(tmp_path), action_dim=18) + with pytest.raises(ValueError, match="不是 10 的倍数"): + robot.run(recorder=rec) + robot.deactive() + + def test_actpred_compatibility(tmp_path, mock_i2c): """用 actpred 的 ActionDataset 加载录制的 episode""" pytest.importorskip("torch") @@ -199,8 +302,8 @@ def test_actpred_compatibility(tmp_path, mock_i2c): from dataset import ActionDataset ds = ActionDataset(str(tmp_path), history_len=5, action_dim=18, frame_len=3) - # 10 行，start=max(5,2)=5，samples: t=5..9 → 5 个样本 - assert len(ds) == 5 + # 19 帧，start=max(5,2)=5，samples: t=5..18 → 14 个样本 + assert len(ds) == 14 sample = ds[0] # ActionDataset 默认 resize 到 640×480，与相机原始尺寸无关 assert sample["images"].shape == (3, 3, 480, 640) # (F, C, H, W)