feat(pybot): 微动作分解——带时延的 @Action 行按 10ms 粒度插值执行
每行带 delay 的动作分解为 N=delay//10 个微步，舵机角度从上一行 目标值线性插值到当前行目标值。首行不分解（设初始位置），每步调 speaker.play(freq, 10) 提供声音+时序，dry_run 下补 sleep(0.01)。 delay 必须是 10 的倍数。录制时每个微步产出一帧+插值动作向量。
This commit is contained in:
qing committed
1 parent
d47ef6efb4
commit
3fc213e7b9
2 files changed
+213
-52
No files matched your search
+86
-28
@@ -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):
|
||||
|
||||
+127
-24
@@ -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)
|
||||
|
||||
Reference in new issue
Block a user