diff --git a/console/robot_service/dance_soak.py b/console/robot_service/dance_soak.py new file mode 100644 index 0000000..0deb15f --- /dev/null +++ b/console/robot_service/dance_soak.py @@ -0,0 +1,436 @@ +#!/usr/bin/env python3 +"""dance_soak — 长期稳定性压测驱动器(无人值守的"遥控器管理权"). + +背景: 站立过久/跳舞一段时间后会周期性摔倒。本工具按网页遥控器完全等价的 +ZMQ 指令循环跳 5 支内置舞 + 穿插行走 + 周期性长站探针,同时记录三路数据: + - 遥测 5572 (50Hz: q/dq/z/quat/w) —— 机体侧 + - 目标 5573 (50Hz: q_target) —— 策略侧(经 spine-A 转发,body 所见同一份) + - 状态 5574 (2Hz: mode/seq/infer) —— service 侧 +每次摔倒自动落盘 150s 环形缓冲现场(判别"目标先跳"还是"机体先动")、复位续测。 + +只 connect 不 bind(不占端口);指令带 src=soak-。 + +用法: + python dance_soak.py --target-dances 200 # ≥200 支舞 + python dance_soak.py --target-dances 200 --dry-walk # 只行走不跳舞(快验链路) +输出: logs/soak_/ {driver.log trials.jsonl events.jsonl progress.json falls/} +""" +import argparse +import json +import os +import sys +import time +from collections import deque +from pathlib import Path + +import numpy as np +import zmq + +sys.path.insert(0, str(Path(__file__).resolve().parent)) +import telbin # noqa: E402 + +# ---------------- 协议常量(与 service.py / web_remote.py 对齐) ---------------- +BOARD_IP = os.environ.get("SOAK_BOARD_IP", "10.112.10.107") +CMD_URI = os.environ.get("SOAK_CMD_URI", f"tcp://{BOARD_IP}:5571") # service SUB +TEL_URI = os.environ.get("SOAK_TEL_URI", "tcp://127.0.0.1:5572") # body PUB +TGT_URI = os.environ.get("SOAK_TGT_URI", "tcp://127.0.0.1:5573") # spine-A 转发 +ST_URI = os.environ.get("SOAK_ST_URI", f"tcp://{BOARD_IP}:5574") # service PUB + +# 五支内置舞(与网页一致: segments.json 有 a-b 的自动附加) +DANCES = [ + ("qinghaiyao", 0, 1465), + ("demo1_y26p_轨迹", 5, 2799), + ("demo2_y26p_轨迹", 0, 744), + ("demo3_y26p_轨迹", None, None), # 无编辑段落: 整支 + ("demo4_y26p_轨迹", 0, 1310), +] +BLEND_S = 2.5 + 2.5 # 入场混合 2.5s + 出场混合 2.5s(service 固定 125 帧) +MARGIN_S = 25.0 # 舞曲时长容差 + +Z_FALL = 0.35 # 低于此高度视为倒地 +Z_FALL_FRAMES = 15 # 连续 15 帧(0.3s)才算真摔 +Z_UP = 0.60 +Z_UP_FRAMES = 150 # 连续 3s 站稳才算恢复(复位后 5s 内再倒的先例存在) +RING_SECONDS = 150 # 摔倒现场环形缓冲长度 + + +def now(): + return time.time() + + +class Soak: + def __init__(self, outdir: Path, target_dances: int, dry_walk: bool, + stand_s: float, probe_s: float, probe_every: int, walk_s: float): + self.out = outdir + self.target = target_dances + self.dry_walk = dry_walk + self.stand_s = stand_s + self.probe_s = probe_s + self.probe_every = probe_every + self.walk_s = walk_s + + self.ctx = zmq.Context() + self.cmd = self.ctx.socket(zmq.PUB) + self.cmd.connect(CMD_URI) + self.sub_tel = self.ctx.socket(zmq.SUB) + self.sub_tel.connect(TEL_URI) + self.sub_tel.setsockopt_string(zmq.SUBSCRIBE, "") + self.sub_tgt = self.ctx.socket(zmq.SUB) + self.sub_tgt.connect(TGT_URI) + self.sub_tgt.setsockopt_string(zmq.SUBSCRIBE, "") + self.sub_st = self.ctx.socket(zmq.SUB) + self.sub_st.connect(ST_URI) + self.sub_st.setsockopt_string(zmq.SUBSCRIBE, "") + self.poller = zmq.Poller() + for s in (self.sub_tel, self.sub_tgt, self.sub_st): + self.poller.register(s, zmq.POLLIN) + + self.src = f"soak-{os.getpid()}" + self.ring = deque(maxlen=int(RING_SECONDS * 60)) + self.trials = open(outdir / "trials.jsonl", "a", buffering=1) + self.events = open(outdir / "events.jsonl", "a", buffering=1) + + # ---- 运行时状态 ---- + self.z = 0.0 + self.tel_t = 0.0 # 最近遥测 wall time + self.tgt_t = 0.0 + self.tgt = None # 最近 q_target (29,) + self.tgt_prev = None + self.tgt_seq = None # 目标流 seq (service 每 tick +1; 倒退=乱序/重放) + self.st = {} # 最近 status + self.st_t = 0.0 + self.fall_low = 0 # 连续低高度帧计数 + self.up_high = 0 + self.fallen = False + self.ever_stood = False # 起来过才把低高度计为摔倒(排除接手时已倒) + self.fall_n = 0 + + self.dances_done = 0 + self.trial_idx = 0 + self.phase = "INIT" + self.probed_at = -1 # 已做过长站探针的 dances_done 值(防无限探针) + self.phase_t0 = time.monotonic() + self.cur_clip = None + self.cmd_until = 0.0 # 行走段: 持续重发 cmd 的截止(单调钟) + self.cmd_vec = (0.0, 0.0, 0.0) + self.last_cmd_send = 0.0 + self.walk_pat_i = 0 + self.dance_started = False + self.ev_last = {"tel": 0.0, "tgt": 0.0, "st": 0.0} + + # ---------------- 基础设施 ---------------- + def log(self, *a): + print(time.strftime("%H:%M:%S"), *a, flush=True) + + def ev(self, kind, **kw): + rec = {"t": now(), "kind": kind, **kw} + self.events.write(json.dumps(rec, ensure_ascii=False) + "\n") + + def send_req(self, req, clip=""): + self.cmd.send_string(json.dumps( + {"src": self.src, "req": req, "clip": clip})) + self.log(f"SEND req={req} clip={clip!r}") + + def send_cmd(self, vec): + self.cmd.send_string(json.dumps({"src": self.src, "cmd": list(vec)})) + + def progress(self): + p = { + "t": now(), "dances_done": self.dances_done, "target": self.target, + "falls": self.fall_n, "phase": self.phase, + "clip": self.cur_clip, "z": round(self.z, 3), + "mode": self.st.get("mode"), "tel_age": round(now() - self.tel_t, 2), + "tgt_age": round(now() - self.tgt_t, 2), + } + (self.out / "progress.json").write_text( + json.dumps(p, ensure_ascii=False, indent=1)) + + # ---------------- 数据面 ---------------- + def pump(self): + for _ in range(200): + socks = dict(self.poller.poll(0)) + if not socks: + return + if self.sub_tel in socks: + try: + raw = self.sub_tel.recv(zmq.NOBLOCK) + except zmq.Again: + pass + else: + m = telbin.decode(raw) + if m is None: + try: + m = json.loads(raw) # names 握手, 忽略 + except Exception: + m = None + if m and "q" in m: + t = now() + if self.tel_t and t - self.tel_t > 0.15: + self.ev("tel_gap", dt=round(t - self.tel_t, 3)) + self.tel_t = t + self.z = m["root_pos"][2] + dq = np.abs(m["dq"]) + w = np.abs(m["root_angvel_world"]) + self.ring.append({ + "t": t, "z": self.z, + "dq": float(dq.max()), "w": float(w.max()), + "q": [round(float(x), 5) for x in m["q"]], + "dq_full": [round(float(x), 5) for x in m["dq"]], + "quat": [round(float(x), 6) for x in m["root_quat_wxyz"]], + "w3": [round(float(x), 5) for x in m["root_angvel_world"]], + "tgt_d": (float(np.abs(self.tgt - self.tgt_prev).max()) + if self.tgt is not None and self.tgt_prev is not None + else 0.0), + "tgt": ([round(float(x), 5) for x in self.tgt] + if self.tgt is not None else None), + "tgt_age": (t - self.tgt_t) if self.tgt_t else -1, + "mode": self.st.get("mode", "?"), + }) + if self.sub_tgt in socks: + try: + raw = self.sub_tgt.recv(zmq.NOBLOCK) + except zmq.Again: + pass + else: + try: + m = json.loads(raw) + except Exception: + continue + if "q_target" in m: + t = now() + if self.tgt_t and t - self.tgt_t > 0.5: + self.ev("tgt_gap", dt=round(t - self.tgt_t, 3)) + new_seq = m.get("seq") + if (self.tgt_seq is not None and new_seq is not None + and new_seq < self.tgt_seq): + self.ev("tgt_reorder", + prev=self.tgt_seq, now=new_seq) # 旧目标晚到=目标后跳 + self.tgt_seq = new_seq + self.tgt_prev = self.tgt + self.tgt = np.array(m["q_target"], dtype=np.float32) + self.tgt_t = t + if self.sub_st in socks: + try: + m = json.loads(self.sub_st.recv(zmq.NOBLOCK)) + except Exception: + continue + prev_mode = self.st.get("mode") + self.st = m + self.st_t = now() + if m.get("mode") != prev_mode and prev_mode is not None: + self.ev("mode", **{"from": prev_mode, "to": m.get("mode")}) + + # ---------------- 摔倒检测与现场捕获 ---------------- + def check_fall(self): + if self.z < Z_FALL: + self.fall_low += 1 + else: + self.fall_low = 0 + if self.z > Z_UP: + self.up_high += 1 + self.ever_stood = True + else: + self.up_high = 0 + if (not self.fallen and self.ever_stood + and self.fall_low >= Z_FALL_FRAMES): + self.fallen = True + self.fall_n += 1 + self.dump_fall() + self.phase = "RECOVER" + self.phase_t0 = time.monotonic() + self.reset_tries = 0 + self.send_req("RESET") + + def dump_fall(self): + d = self.out / "falls" + d.mkdir(exist_ok=True) + rec = { + "n": self.fall_n, "t": now(), "phase": self.phase, + "clip": self.cur_clip, "trial_idx": self.trial_idx, + "dances_done": self.dances_done, + "status": {k: self.st.get(k) for k in + ("mode", "seq", "infer_ms", "tick_hz", "frame", "reset_seq")}, + } + # 判别核心: 摔倒前 150s 全量状态 + 目标流(E4 离线回放用) + arr = list(self.ring) + rec["ring"] = { + "t": [r["t"] for r in arr], + "z": [r["z"] for r in arr], + "dq_max": [r["dq"] for r in arr], + "w_max": [r["w"] for r in arr], + "q": [r["q"] for r in arr], + "dq_full": [r["dq_full"] for r in arr], + "quat": [r["quat"] for r in arr], + "w3": [r["w3"] for r in arr], + "tgt_d": [r["tgt_d"] for r in arr], + "tgt": [r["tgt"] for r in arr], + "tgt_age": [r["tgt_age"] for r in arr], + "mode": [r["mode"] for r in arr], + } + f = d / f"fall_{self.fall_n:03d}.json" + f.write_text(json.dumps(rec, ensure_ascii=False)) + self.log(f"FALL #{self.fall_n} phase={self.phase} clip={self.cur_clip} " + f"-> {f.name} (ring {len(arr)} frames)") + + # ---------------- 试验编排 ---------------- + def start_dance(self): + self.trial_idx += 1 + name, a, b = DANCES[(self.trial_idx - 1) % len(DANCES)] + clip = name if a is None else f"{name}#{a}-{b}" + n_frames = (b - a) if a is not None else 672 + self.cur_clip = clip + self.cur_dur = n_frames / 50.0 + BLEND_S + MARGIN_S + self.dance_started = False + self.send_req("TO_DANCE", clip) + self.phase = "DANCE" + self.phase_t0 = time.monotonic() + + def start_walk(self): + pat = [(0.5, 0, 0), (0.5, 0.2, 0), (0.3, 0, 0.4), + (-0.4, 0, 0), (0.5, -0.2, 0), (0.4, 0, -0.4)] + self.cmd_vec = pat[self.walk_pat_i % len(pat)] + self.walk_pat_i += 1 + self.cmd_until = time.monotonic() + self.walk_s + self.phase = "WALK" + self.phase_t0 = time.monotonic() + self.log(f"WALK vec={self.cmd_vec} {self.walk_s}s") + + def start_stand(self, dur, probe): + self.phase = "STAND" + self.phase_t0 = time.monotonic() + self.stand_dur = dur + self.log(f"STAND {dur}s{' (probe)' if probe else ''}") + + def fsm(self): + m = time.monotonic() + el = m - self.phase_t0 + + if self.phase == "INIT": + if el < 1.0: + return + # 接手时 service 可能残留 DANCE 模式(RESET 不切模式),先回 WALK + self.send_req("TO_WALK") + self.send_req("RESET") + self.phase = "WAIT_STAND" + self.phase_t0 = m + elif self.phase == "WAIT_STAND": + if self.up_high >= Z_UP_FRAMES: + self.log("robot standing; soak begins") + self.phase = "NEXT" + elif el > 25 and self.reset_tries < 3: + self.reset_tries += 1 + self.log(f"not up after 25s, RESET x{self.reset_tries}") + self.send_req("RESET") + self.phase_t0 = m + elif el > 25: + self.log("ALARM: robot won't stand; halting") + self.phase = "HALT" + elif self.phase == "NEXT": + if self.dances_done >= self.target: + self.phase = "DONE" + return + if (self.dry_walk): + self.start_walk() + elif (self.probe_every and self.dances_done > 0 + and self.dances_done % self.probe_every == 0 + and self.probed_at != self.dances_done): + self.probed_at = self.dances_done + self.start_stand(self.probe_s, probe=True) + else: + self.start_dance() + elif self.phase == "DANCE": + mode = self.st.get("mode") + if mode == "DANCE": + self.dance_started = True + if self.dance_started: + if mode == "WALK": # service 自动退舞 -> 本支完成 + self.dances_done += 1 + self.write_trial("ok" if not self.fallen else "fall_exit") + self.progress() + if self.dances_done % 5 == 0: + self.start_walk() # 每完成 5 支插一段行走 + else: + self.start_stand(self.stand_s, probe=False) + elif el > 10: + self.log("dance never started; retry once") + self.write_trial("no_start") + self.start_dance() + elif el > self.cur_dur: + self.write_trial("timeout") + self.dances_done += 1 + self.start_walk() + elif self.phase == "WALK": + if m < self.cmd_until: + if m - self.last_cmd_send > 0.2: + self.send_cmd(self.cmd_vec) + self.last_cmd_send = m + else: + self.send_cmd((0, 0, 0)) + self.start_stand(self.stand_s, probe=False) + elif self.phase == "STAND": + if el >= self.stand_dur: + self.phase = "NEXT" + elif self.phase == "RECOVER": + if self.up_high >= Z_UP_FRAMES: + self.fallen = False # 恢复站立后才允许下一次摔倒检测 + self.log("recovered; resume") + self.phase = "NEXT" + elif el > 25 and self.reset_tries < 8: + # 坏纪元可持续数分钟(板端负载慢变量), 8 次 x 25s 上限 + self.reset_tries += 1 + self.log(f"still down after 25s, RESET x{self.reset_tries}") + self.send_req("RESET") + self.phase_t0 = m + elif el > 25: + self.log("ALARM: unrecoverable; halting (robot needs attention)") + self.phase = "HALT" + + def write_trial(self, outcome): + self.trials.write(json.dumps({ + "idx": self.trial_idx, "clip": self.cur_clip, "t": now(), + "outcome": outcome, "dances_done": self.dances_done, + }, ensure_ascii=False) + "\n") + + def run(self): + self.reset_tries = 0 + self.log(f"soak start: target={self.target} dances, dry_walk={self.dry_walk}") + self.log(f"cmd={CMD_URI} tel={TEL_URI} tgt={TGT_URI} st={ST_URI}") + last_prog = 0.0 + while self.phase not in ("DONE", "HALT"): + self.pump() + self.check_fall() + self.fsm() + t = now() + if t - last_prog > 10: + self.progress() + last_prog = t + time.sleep(0.02) + self.progress() + self.log(f"soak end: dances={self.dances_done} falls={self.fall_n} phase={self.phase}") + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("--target-dances", type=int, default=200) + ap.add_argument("--out-dir", default=None) + ap.add_argument("--dry-walk", action="store_true", + help="快验: 不跳舞只行走一轮") + ap.add_argument("--stand-s", type=float, default=20.0) + ap.add_argument("--probe-s", type=float, default=360.0) + ap.add_argument("--probe-every", type=int, default=15) + ap.add_argument("--walk-s", type=float, default=12.0) + args = ap.parse_args() + + if args.out_dir: + out = Path(args.out_dir) + else: + out = Path(__file__).resolve().parent / "logs" / \ + ("soak_" + time.strftime("%m%d_%H%M%S")) + out.mkdir(parents=True, exist_ok=True) + (out / "falls").mkdir(exist_ok=True) + Soak(out, args.target_dances, args.dry_walk, + args.stand_s, args.probe_s, args.probe_every, args.walk_s).run() + + +if __name__ == "__main__": + main() diff --git a/tests/e1_stand_loop.py b/tests/e1_stand_loop.py new file mode 100644 index 0000000..bb645f3 --- /dev/null +++ b/tests/e1_stand_loop.py @@ -0,0 +1,71 @@ +#!/usr/bin/env python3 +"""E1: velocity 策略"完美站立+零指令"纯 last_action 反馈回路稳定性实验. + +不动真实系统。检验假设: 站立不动点的失稳是否由策略自身的 last_action 反馈 +结构决定(与物理无关的一半回路)。obs 布局对齐 atomic_list(VEL_TERMS, 8): + velocity_command(4) projected_gravity(3) rel_ang_vel(3) dof_pos(29) + dof_vel(29) last_action(29); history=8 = 含当前帧共 8 帧(8*97=776)。 +""" +import sys +from pathlib import Path + +import numpy as np +import onnxruntime as ort + +REPO = Path(__file__).resolve().parents[1] +sys.path.insert(0, str(REPO / "console" / "robot_service")) + +so = ort.SessionOptions() +so.intra_op_num_threads = 4 +so.inter_op_num_threads = 1 +sess = ort.InferenceSession( + str(REPO / "checkpoints/HoloMotion_velocity_tracking_model/exported/velocity_model.onnx"), + so, providers=["CPUExecutionProvider"]) +meta = sess.get_modelmeta().custom_metadata_map + +HIST = 7 # 当前帧之外的历史帧数 + + +def make_obs(a: np.ndarray, hist: list) -> np.ndarray: + cmd = np.zeros(4, np.float32) + grav = np.array([0, 0, -1], np.float32) + av = np.zeros(3, np.float32) + dp = np.zeros(29, np.float32) + dv = np.zeros(29, np.float32) + parts = [] + for cur in (cmd, grav, av, dp, dv): + parts.append(np.tile(cur, 8)) # 静止: 历史=当前 + parts.append(np.concatenate([a] + list(hist))) # last_action: 当前+7历史 + obs = np.concatenate(parts).astype(np.float32) + assert obs.shape[0] == 776, obs.shape + return obs + + +def run(a0, label, ticks=600): + a = a0.astype(np.float32).copy() + hist = [np.zeros(29, np.float32) for _ in range(HIST)] + traj = [] + for _ in range(ticks): + obs = make_obs(a, hist) + out = sess.run(None, {"obs": obs[None, :]})[0].reshape(-1) + hist.insert(0, a.copy()) + hist.pop() + a = out.astype(np.float32) + traj.append(float(np.abs(a).max())) + traj = np.array(traj) + print(f"{label}: |a|max 首帧={traj[0]:.3f} 10tick={traj[9]:.3f} " + f"50tick={traj[49]:.3f} 200tick={traj[199]:.3f} " + f"末帧={traj[-1]:.3f} 峰值={traj.max():.3f}", flush=True) + return traj + + +if __name__ == "__main__": + obs0 = make_obs(np.zeros(29, np.float32), [np.zeros(29, np.float32)] * HIST) + print("obs dim =", obs0.shape[0]) + run(np.zeros(29, np.float32), "E1a 零启动(精确不动点)") + run((np.random.RandomState(0).randn(29) * 1e-3).astype(np.float32), + "E1b 微扰1e-3") + run((np.random.RandomState(1).randn(29) * 5e-2).astype(np.float32), + "E1c 扰动5e-2") + run((np.random.RandomState(2).randn(29) * 0.5).astype(np.float32), + "E1d 扰动0.5(量级≈倒地)") diff --git a/tests/e3_official_stand.py b/tests/e3_official_stand.py new file mode 100644 index 0000000..7c17ee8 --- /dev/null +++ b/tests/e3_official_stand.py @@ -0,0 +1,74 @@ +#!/usr/bin/env python3 +"""E3: 官方 evaluator 零指令站立对照实验. + +问题: 站立过久/复位后摔倒, 是我们 console 闭环的偏差, 还是官方 velocity +策略在"零指令静止站立"下的固有属性? 本实验用上游 eval_mujoco_sim2sim 的 +原版 obs builder + 原版 _update_policy + 原版 PD, 零指令站 N 步, 记录 +骨盆高度与动作幅值轨迹。不碰运行中的真实系统(独立 MuJoCo 实例)。 +""" +import sys +import time +from pathlib import Path + +import numpy as np + +REPO = Path(__file__).resolve().parents[1] +sys.path.insert(0, str(REPO / "console" / "robot_service")) +from rs_paths import SCENE_DEFAULT, UPSTREAM, VEL_ONNX # noqa: E402 + +sys.path.insert(0, str(UPSTREAM)) +from omegaconf import OmegaConf # noqa: E402 + +if not OmegaConf.has_resolver("now"): + OmegaConf.register_new_resolver("now", lambda fmt="%Y-%m-%d": time.strftime(fmt)) +from holomotion.src.evaluation.eval_mujoco_sim2sim import ( # noqa: E402 + MujocoEvaluator, + process_config, +) + +STEPS = int(sys.argv[1]) if len(sys.argv) > 1 else 12000 # 50Hz -> 240s + +override = OmegaConf.create( + { + "model_type": "holomotion", + "ckpt_onnx_path": str(VEL_ONNX), + "robot_xml_path": str(SCENE_DEFAULT), + "use_gpu": False, + "headless": True, + } +) +ev = MujocoEvaluator(process_config(override)) +ev.setup() +ev.keyboard_handler = None # 零指令: velocity_command 保持初始零 +ev.reset_state_teleport() +ev.counter = 0 + +zs, amax = [], [] +t0 = time.time() +for i in range(STEPS): + ev._update_policy() + ev.counter += 1 + ev._apply_control(sleep=False) + zs.append(float(ev.d.qpos[2])) + a = getattr(ev, "actions_onnx", None) + amax.append(float(np.abs(a).max()) if a is not None else float("nan")) + if i % 250 == 0: + print(f"step {i:6d} z={zs[-1]:.3f} |a|max={amax[-1]:.3f} " + f"({time.time()-t0:.0f}s)", flush=True) + +zs = np.array(zs) +amax = np.array(amax) +fallen = np.where(zs < 0.3)[0] +print("=== E3 RESULT ===") +print(f"steps={STEPS} ({STEPS/50:.0f}s sim, {time.time()-t0:.0f}s wall)") +print(f"z: start={zs[0]:.3f} min={zs.min():.3f} end={zs[-1]:.3f}") +print(f"|a|max: 首帧={amax[0]:.3f} p50={np.nanpercentile(amax,50):.3f} " + f"end={amax[-1]:.3f}") +if len(fallen): + k = fallen[0] + print(f"FALL at step {k} ({k/50:.1f}s): 官方闭环零指令也倒 -> 策略固有属性") + # 摔前动作幅值轨迹 + for j in range(max(0, k - 500), min(STEPS, k + 5), 50): + print(f" step {j}: z={zs[j]:.3f} |a|max={amax[j]:.3f}") +else: + print("NO FALL: 官方闭环零指令站满全程 -> 我们 console 闭环有偏差, 需逐项对拍") diff --git a/tests/e4_replay_obs.py b/tests/e4_replay_obs.py new file mode 100644 index 0000000..bd77fc4 --- /dev/null +++ b/tests/e4_replay_obs.py @@ -0,0 +1,155 @@ +#!/usr/bin/env python3 +"""E4: 回放判别 —— service 是否"忠实"地把遥测变成目标. + +用 fall dump 里的逐帧 q/dq/quat/w(50Hz) 喂与 service 完全同款的 +FastObsBuilder + velocity ONNX, last_action 链自举(从记录 tgt 反解): + 偏差≈0 => service 忠实 => 摔倒是分离架构的时序/闭环失稳 + 偏差大 => service 内部状态与遥测不符 => 有泄漏源(历史/指令/映射) + +用法: python e4_replay_obs.py [起始帧] [帧数] +""" +import json +import sys +import time +from pathlib import Path + +import numpy as np +import onnxruntime as ort + +REPO = Path(__file__).resolve().parents[1] +sys.path.insert(0, str(REPO / "console" / "robot_service")) +from obs_fast import FastObsBuilder # noqa: E402 + +fall_path = sys.argv[1] +i0 = int(sys.argv[2]) if len(sys.argv) > 2 else 0 +n_frames = int(sys.argv[3]) if len(sys.argv) > 3 else 0 + +# ---- 模型与顺序 ---- +so = ort.SessionOptions() +so.intra_op_num_threads = 4 +so.inter_op_num_threads = 1 +sess = ort.InferenceSession( + str(REPO / "checkpoints/HoloMotion_velocity_tracking_model/exported/velocity_model.onnx"), + so, providers=["CPUExecutionProvider"]) +meta = sess.get_modelmeta().custom_metadata_map +onnx_names = [s for s in meta["joint_names"].split(",") if s] +vel_def = np.fromstring(meta["default_joint_pos"], sep=",", dtype=np.float32) +vel_scale = np.fromstring(meta["action_scale"], sep=",", dtype=np.float32) + +# body 顺序(mjcf_dof_names): 从 body 侧官方 evaluator 离线取一次 +sys.path.insert(0, str(REPO / "upstream")) +from omegaconf import OmegaConf # noqa: E402 +if not OmegaConf.has_resolver("now"): + OmegaConf.register_new_resolver("now", lambda fmt="%Y-%m-%d": time.strftime(fmt)) +from holomotion.src.evaluation.eval_mujoco_sim2sim import ( # noqa: E402 + MujocoEvaluator, process_config, +) +from rs_paths import SCENE_DEFAULT, VEL_ONNX # noqa: E402 +ov = OmegaConf.create({"model_type": "holomotion", "ckpt_onnx_path": str(VEL_ONNX), + "robot_xml_path": str(SCENE_DEFAULT), + "use_gpu": False, "headless": True}) +ev = MujocoEvaluator(process_config(ov)) +ev.setup() +body_names = list(ev.mjcf_dof_names) # 遥测 q 的顺序 +to_body = np.array([body_names.index(n) for n in onnx_names], dtype=np.int64) + +# ---- adapter 桩: 只暴露 velocity 路径用到的 _get_obs_* ---- +class Adapter: + def __init__(self): + self.q_onnx = np.zeros(29, np.float32) + self.dq_onnx = np.zeros(29, np.float32) + self.root_q = np.array([1, 0, 0, 0], np.float32) + self.w_world = np.zeros(3, np.float32) + self.cmd = np.zeros(3, np.float32) + self.last_action = np.zeros(29, np.float32) + self.defaults = vel_def + + def _get_obs_actor_velocity_command(self): + out = np.zeros(4, np.float32) + out[1:] = self.cmd + out[0] = float(np.linalg.norm(self.cmd) > 0.1) + return out + + def _get_obs_actor_projected_gravity(self): + q = self.root_q.astype(np.float64) + w, x, y, z = q + return np.array([2*(-z*x + w*y), -2*(z*y + w*x), + 1 - 2*(w*w + z*z)], dtype=np.float32) + + def _get_obs_actor_rel_robot_root_ang_vel(self): + q = self.root_q.astype(np.float64)[None, :] + v = self.w_world.astype(np.float64)[None, :] + w2, x, y, z = q[:, 0:1], q[:, 1:2], q[:, 2:3], q[:, 3:4] + c1 = np.concatenate([y*v[..., 2:3] - z*v[..., 1:2], + z*v[..., 0:1] - x*v[..., 2:3], + x*v[..., 1:2] - y*v[..., 0:1]], axis=1) + t2 = np.concatenate([y*c1[..., 2:3] - z*c1[..., 1:2], + z*c1[..., 0:1] - x*c1[..., 2:3], + x*c1[..., 1:2] - y*c1[..., 0:1]], axis=1) + out = v - 2*w2*c1 + 2*t2 + return out[0].astype(np.float32) + + def _get_obs_actor_dof_pos(self): + return (self.q_onnx - self.defaults).astype(np.float32) + + def _get_obs_actor_dof_vel(self): + return self.dq_onnx.astype(np.float32) + + def _get_obs_actor_last_action(self): + return self.last_action + + +VEL_TERMS = ["actor_velocity_command", "actor_projected_gravity", + "actor_rel_robot_root_ang_vel", "actor_dof_pos", + "actor_dof_vel", "actor_last_action"] +def atomic_list(names, hist): + return [{n: {"history_length": hist, "scale": 1.0}} for n in names] + +ad = Adapter() +builder = FastObsBuilder(dof_names_onnx=onnx_names, default_angles_onnx=vel_def, + evaluator=ad, + obs_policy_cfg={"atomic_obs_list": atomic_list(VEL_TERMS, 8)}) + +# ---- 回放 ---- +d = json.load(open(fall_path)) +ring = d["ring"] +N = len(ring["t"]) +if not n_frames: + n_frames = N - i0 +a = np.zeros(29, np.float32) +errs, rec_amp = [], [] +first_rec = None +for i in range(i0, min(N, i0 + n_frames)): + ad.q_onnx = np.array(ring["q"][i], np.float32)[to_body] + ad.dq_onnx = np.array(ring["dq_full"][i], np.float32)[to_body] + ad.root_q = np.array(ring["quat"][i], np.float32) + ad.w_world = np.array(ring["w3"][i], np.float32) + ad.last_action = a + obs = builder.build_policy_obs() + out = sess.run(None, {"obs": obs[None, :]})[0].reshape(-1).astype(np.float32) + tgt_replay = out * vel_scale + vel_def + tgt_rec = ring["tgt"][i] + if tgt_rec is not None: + rec = np.array(tgt_rec, np.float32) + if first_rec is None: + first_rec = i + errs.append(float(np.abs(tgt_replay - rec).max())) + rec_amp.append(float(np.abs(rec - vel_def).max())) + a = out # last_action 链: 用回放动作(与 service 同构) + # 前 8 tick 用记录目标反解 last_action 做种子(live 传送帧的真实初值, + # 否则回放从 0 热启动必然与 live 混沌错位, 判别失效) + if i - i0 < 8 and tgt_rec is not None: + a = (np.array(tgt_rec, np.float32) - vel_def) / vel_scale + +errs = np.array(errs) +rec_amp = np.array(rec_amp) +print(f"replay {i0}..{i0+n_frames} ({len(errs)} 帧有记录 tgt, 从 {first_rec})") +print(f"|tgt_replay - tgt_recorded| max: p50={np.percentile(errs,50):.4f} " + f"p90={np.percentile(errs,90):.4f} max={errs.max():.4f}") +print(f"记录 tgt 相对 default 摆幅: p50={np.percentile(rec_amp,50):.3f} " + f"max={rec_amp.max():.3f}") +print("发散时间线(每50帧=1s的 max|Δtgt|):", end=" ") +for b in range(0, len(errs), 50): + print(f"{errs[b:b+50].max():.3f}", end=" ") +print() +print("判别: 首桶小后增长 => service 忠实+混沌发散; 首桶即大 => 内部状态泄漏") diff --git a/tests/e6_arch_delay.py b/tests/e6_arch_delay.py new file mode 100644 index 0000000..82a1b30 --- /dev/null +++ b/tests/e6_arch_delay.py @@ -0,0 +1,248 @@ +#!/usr/bin/env python3 +"""E6: 受控架构对照 —— 分离闭环的 float32 量化 + 目标时延 是否破坏站立稳定. + +三组同物理同策略的闭环, 只差"架构变量": + A 官方: float64 obs, 同线程零时延 (= E3, 对照组) + B split-f32: float32 遥测量化 + 目标延迟 D 个控制步 (我们架构的最小复刻) + C jitter: 延迟在 {D, D+1} 间随机抖动 (逼近真实网络抖动) +全部离线独立 MuJoCo 实例, 不碰运行中的系统。站立零指令 N 步看谁倒。 +""" +import sys +import time +from collections import deque +from pathlib import Path + +import numpy as np +import onnxruntime as ort + +REPO = Path(__file__).resolve().parents[1] +sys.path.insert(0, str(REPO / "console" / "robot_service")) +from rs_paths import SCENE_DEFAULT, UPSTREAM, VEL_ONNX # noqa: E402 + +sys.path.insert(0, str(UPSTREAM)) +from omegaconf import OmegaConf # noqa: E402 +if not OmegaConf.has_resolver("now"): + OmegaConf.register_new_resolver("now", lambda fmt="%Y-%m-%d": time.strftime(fmt)) +from holomotion.src.evaluation.eval_mujoco_sim2sim import ( # noqa: E402 + MujocoEvaluator, process_config, +) +from obs_fast import FastObsBuilder # noqa: E402 + +STEPS = int(sys.argv[1]) if len(sys.argv) > 1 else 12000 + + +def make_ev(): + ov = OmegaConf.create({ + "model_type": "holomotion", "ckpt_onnx_path": str(VEL_ONNX), + "robot_xml_path": str(SCENE_DEFAULT), "use_gpu": False, + "headless": True}) + ev = MujocoEvaluator(process_config(ov)) + ev.setup() + ev.keyboard_handler = None + ev.reset_state_teleport() + ev.counter = 0 + return ev + + +so = ort.SessionOptions() +so.intra_op_num_threads = 4 +so.inter_op_num_threads = 1 +sess = ort.InferenceSession(str(VEL_ONNX), so, providers=["CPUExecutionProvider"]) +meta = sess.get_modelmeta().custom_metadata_map +onnx_names = [s for s in meta["joint_names"].split(",") if s] +vel_def = np.fromstring(meta["default_joint_pos"], sep=",", dtype=np.float32) +vel_scale = np.fromstring(meta["action_scale"], sep=",", dtype=np.float32) + + +class Ad: + """最小 adapter: 与 service.ObsAdapter 的 velocity 路径同构.""" + def __init__(self): + self.q_onnx = np.zeros(29, np.float32) + self.dq_onnx = np.zeros(29, np.float32) + self.root_q = np.array([1., 0, 0, 0], np.float32) + self.w_world = np.zeros(3, np.float32) + self.cmd = np.zeros(3, np.float32) + self.last_action = np.zeros(29, np.float32) + self.defaults = vel_def + + def _get_obs_actor_velocity_command(self): + out = np.zeros(4, np.float32) + out[1:] = self.cmd + out[0] = float(np.linalg.norm(self.cmd) > 0.1) + return out + + def _get_obs_actor_projected_gravity(self): + w, x, y, z = self.root_q.astype(np.float64) + return np.array([2*(-z*x + w*y), -2*(z*y + w*x), + 1 - 2*(w*w + z*z)], np.float32) + + def _get_obs_actor_rel_robot_root_ang_vel(self): + q = self.root_q.astype(np.float64)[None, :] + v = self.w_world.astype(np.float64)[None, :] + w2, x, y, z = q[:, 0:1], q[:, 1:2], q[:, 2:3], q[:, 3:4] + c1 = np.concatenate([y*v[..., 2:3]-z*v[..., 1:2], + z*v[..., 0:1]-x*v[..., 2:3], + x*v[..., 1:2]-y*v[..., 0:1]], axis=1) + t2 = np.concatenate([y*c1[..., 2:3]-z*c1[..., 1:2], + z*c1[..., 0:1]-x*c1[..., 2:3], + x*c1[..., 1:2]-y*c1[..., 0:1]], axis=1) + return (v - 2*w2*c1 + 2*t2)[0].astype(np.float32) + + def _get_obs_actor_dof_pos(self): + return (self.q_onnx - self.defaults).astype(np.float32) + + def _get_obs_actor_dof_vel(self): + return self.dq_onnx.astype(np.float32) + + def _get_obs_actor_last_action(self): + return self.last_action + + +def make_builder(ad=None): + terms = [{n: {"history_length": 8, "scale": 1.0}} for n in [ + "actor_velocity_command", "actor_projected_gravity", + "actor_rel_robot_root_ang_vel", "actor_dof_pos", + "actor_dof_vel", "actor_last_action"]] + if ad is None: + ad = Ad() + b = FastObsBuilder(dof_names_onnx=onnx_names, default_angles_onnx=vel_def, + evaluator=ad, obs_policy_cfg={"atomic_obs_list": terms}) + return b, ad + + +def run_official(steps): + ev = make_ev() + amax, zs = [], [] + for _ in range(steps): + ev._update_policy() + ev.counter += 1 + ev._apply_control(sleep=False) + zs.append(float(ev.d.qpos[2])) + a = getattr(ev, "actions_onnx", None) + amax.append(float(np.abs(a).max()) if a is not None else np.nan) + return np.array(zs), np.array(amax) + + +def run_split(steps, delay, jitter=False, seed=0, f32=True): + ev = make_ev() + builder, ad = make_builder() # 同一个 ad 贯通 builder 与遥测喂入 + body_names = list(ev.mjcf_dof_names) + to_body = np.array([body_names.index(n) for n in onnx_names], np.int64) + pelvis = __import__("mujoco").mj_name2id( + ev.m, __import__("mujoco").mjtObj.mjOBJ_BODY, "pelvis") + import mujoco + rng = np.random.RandomState(seed) + pipe = deque(maxlen=delay + 2) # 目标传输管道: body 每步取最新可用 + zs, amax = [], [] + a = np.zeros(29, np.float32) + quant = (lambda x: x.astype(np.float32).astype(np.float64)) if f32 else (lambda x: x) + for k in range(steps): + # --- service 侧: 从"遥测"(float32 化的机体现态) 建 obs -> 目标 --- + q_body = quant(ev.d.qpos[ev.actuator_qpos_indices]) + dq_body = quant(ev.d.qvel[ev.actuator_qvel_indices]) + vel6 = np.zeros(6) + mujoco.mj_objectVelocity(ev.m, ev.d, mujoco.mjtObj.mjOBJ_BODY, pelvis, vel6, 0) + ad.q_onnx = q_body[to_body].astype(np.float32) + ad.dq_onnx = dq_body[to_body].astype(np.float32) + ad.root_q = ev.d.xquat[pelvis].astype(np.float32) + ad.w_world = vel6[:3].astype(np.float32) + ad.last_action = a + obs = builder.build_policy_obs() + out = sess.run(None, {"obs": obs[None, :]})[0].reshape(-1).astype(np.float32) + a = out + tgt = (out * vel_scale + vel_def).astype(np.float32) + d_eff = delay + (1 if (jitter and rng.rand() < 0.3) else 0) + pipe.append((k + d_eff, tgt)) + # --- body 侧: 取当前可用最新目标, 施加 PD 并步进 --- + while len(pipe) > 1 and pipe[1][0] <= k: + pipe.popleft() # 只留最新到达的 + tgt_apply = pipe[0][1] if pipe[0][0] <= k else None + if tgt_apply is None: + mujoco.mj_forward(ev.m, ev.d) # 目标未到: 冻结(同 body 黑暗期) + else: + ev.target_dof_pos_onnx = tgt_apply + ev.target_dof_pos_mu = tgt_apply[ev.onnx_to_mu] + for i, dof_name in enumerate(ev.mjcf_dof_names): + ev.target_dof_pos_by_name[dof_name] = float(ev.target_dof_pos_mu[i]) + ev._apply_control(sleep=False) + zs.append(float(ev.d.qpos[2])) + amax.append(float(np.abs(out).max())) + return np.array(zs), np.array(amax) + + +def report(name, zs, amax): + fallen = np.where(zs < 0.3)[0] + fall_s = f"FALL@{fallen[0]/50:.1f}s" if len(fallen) else "站满全程" + print(f"{name:24s} z_end={zs[-1]:.3f} z_min={zs.min():.3f} " + f"|a|end={amax[-1]:.3f} p90={np.nanpercentile(amax,90):.3f} {fall_s}", + flush=True) + + +if __name__ == "__main__": + print(f"E6: {STEPS} steps ({STEPS/50:.0f}s sim each)") + report("A 官方(零时延f64)", *run_official(STEPS)) + report("D0 split f32 D=0", *run_split(STEPS, delay=0, f32=True)) + report("D1 split f64 D=1", *run_split(STEPS, delay=1, f32=False)) + report("B split f32 D=1", *run_split(STEPS, delay=1)) + report("B split f32 D=2", *run_split(STEPS, delay=2)) + report("C split f32 D=1+抖动", *run_split(STEPS, delay=1, jitter=True, seed=0)) + + +# ---------------------------------------------------------------- E8: blackout +def run_split_blackout(steps, seed=0, blackout_ms=600, jitter_ms=25): + """E6 split + body 端 0.6s 目标静默期(物理冻结) + 相位随机化: + 复刻 live 复位路径: 传送->静默(service 在冻结遥测上继续 tick)-> + 恢复->压缩沉降. 捕获率=进入振荡盆地的概率.""" + import random + rng = np.random.RandomState(seed) + ev = make_ev() + builder, ad = make_builder() + body_names = list(ev.mjcf_dof_names) + to_body = np.array([body_names.index(n) for n in onnx_names], np.int64) + import mujoco + pelvis = mujoco.mj_name2id(ev.m, mujoco.mjtObj.mjOBJ_BODY, "pelvis") + blackout = blackout_ms / 1000.0 + rng.rand() * jitter_ms / 1000.0 + zs, amax = [], [] + a = np.zeros(29, np.float32) + # 模拟传送: 站到默认位形但不下沉(blackout 期间物理冻结) + ev.reset_state_teleport() + k_unfreeze = int(blackout * 50) + for k in range(steps): + vel6 = np.zeros(6) + mujoco.mj_objectVelocity(ev.m, ev.d, mujoco.mjtObj.mjOBJ_BODY, pelvis, vel6, 0) + ad.q_onnx = ev.d.qpos[ev.actuator_qpos_indices].astype(np.float32).astype(np.float64)[to_body].astype(np.float32) + ad.dq_onnx = ev.d.qvel[ev.actuator_qvel_indices].astype(np.float32).astype(np.float64)[to_body].astype(np.float32) + ad.root_q = ev.d.xquat[pelvis].astype(np.float32) + ad.w_world = vel6[:3].astype(np.float32) + ad.last_action = a + obs = builder.build_policy_obs() + out = sess.run(None, {"obs": obs[None, :]})[0].reshape(-1).astype(np.float32) + a = out + tgt = (out * vel_scale + vel_def).astype(np.float32) + if k < k_unfreeze: + mujoco.mj_forward(ev.m, ev.d) # blackout: 冻结, 目标被丢弃 + else: + ev.target_dof_pos_onnx = tgt + ev.target_dof_pos_mu = tgt[ev.onnx_to_mu] + for i, nm in enumerate(ev.mjcf_dof_names): + ev.target_dof_pos_by_name[nm] = float(ev.target_dof_pos_mu[i]) + ev._apply_control(sleep=False) + zs.append(float(ev.d.qpos[2])) + amax.append(float(np.abs(out).max())) + return np.array(zs), np.array(amax) + + +def e8_main(trials=20): + cap = 0 + for s in range(trials): + zs, amax = run_split_blackout(6000, seed=s) # 120s + fallen = np.where(zs < 0.3)[0] + ok = "OK " if not len(fallen) else f"FALL@{fallen[0]/50:.1f}s" + end_a = float(np.abs(amax[-500:]).max()) + if len(fallen) or end_a > 0.55: + cap += 1 + tag = "CAPTURED(振荡)" + else: + tag = "stable" + print(f"seed={s:2d} {ok:12s} |a|尾段max={end_a:.3f} {tag}", flush=True) + print(f"E8 捕获率: {cap}/{trials}") diff --git a/tests/e9_beat_delay.py b/tests/e9_beat_delay.py new file mode 100644 index 0000000..a8ecc78 --- /dev/null +++ b/tests/e9_beat_delay.py @@ -0,0 +1,91 @@ +#!/usr/bin/env python3 +"""E9: 拍频延迟假设 —— "好/坏纪元"=机体/服务自由时钟拍相位扫过延迟分水岭. + +live 实测(run9f fall_004): 纯站立 170s 数值冻结后, T-16s 起 dq 以 τ≈2.2s +指数发散至倒塌; 目标时龄 20/40ms 双峰. 假设: 目标"帧发出→被采用"延迟在 +2↔3 控制拍间随拍相位游走, 交替延迟=参数激励, 激发站立不动点不稳定模态. +E6 已证 D=1,2 常数稳定且 D=1↔2 随机抖动 30% 稳定 -> 分水岭在 D=3 附近. + +变体: 常数 D=3/4; 2↔3 每步交替; 2↔3 慢块交替(拍频扫边界的真实形状); +D=2 基线 + 每 500 步单发 3(稀疏毛刺, 应可衰减). +""" +import sys +import time +from collections import deque +from pathlib import Path + +import numpy as np + +from e6_arch_delay import ( # noqa: E402 (同实验台复用) + make_ev, make_builder, onnx_names, vel_def, vel_scale, sess, +) +import mujoco # noqa: E402 +import onnxruntime as ort # noqa: F401 (import order side-effect) + +STEPS = int(sys.argv[1]) if len(sys.argv) > 1 else 12000 # 240s + + +def run_split_delayfn(steps, delay_fn, seed=0): + """delay_fn(k) -> 第 k 帧遥测算出的目标的"到达拍"= k + delay_fn(k).""" + ev = make_ev() + builder, ad = make_builder() + body_names = list(ev.mjcf_dof_names) + to_body = np.array([body_names.index(n) for n in onnx_names], np.int64) + pelvis = mujoco.mj_name2id(ev.m, mujoco.mjtObj.mjOBJ_BODY, "pelvis") + pipe = deque(maxlen=8) + zs, amax, dqs = [], [], [] + a = np.zeros(29, np.float32) + for k in range(steps): + q_body = ev.d.qpos[ev.actuator_qpos_indices].astype(np.float32) + dq_body = ev.d.qvel[ev.actuator_qvel_indices].astype(np.float32) + vel6 = np.zeros(6) + mujoco.mj_objectVelocity(ev.m, ev.d, mujoco.mjtObj.mjOBJ_BODY, pelvis, vel6, 0) + ad.q_onnx = q_body[to_body] + ad.dq_onnx = dq_body[to_body] + ad.root_q = ev.d.xquat[pelvis].astype(np.float32) + ad.w_world = vel6[:3].astype(np.float32) + ad.last_action = a + obs = builder.build_policy_obs() + out = sess.run(None, {"obs": obs[None, :]})[0].reshape(-1).astype(np.float32) + a = out + tgt = (out * vel_scale + vel_def).astype(np.float32) + pipe.append((k + delay_fn(k), tgt)) + while len(pipe) > 1 and pipe[1][0] <= k: + pipe.popleft() + tgt_apply = pipe[0][1] if pipe[0][0] <= k else None + if tgt_apply is None: + mujoco.mj_forward(ev.m, ev.d) + else: + ev.target_dof_pos_onnx = tgt_apply + ev.target_dof_pos_mu = tgt_apply[ev.onnx_to_mu] + for i, nm in enumerate(ev.mjcf_dof_names): + ev.target_dof_pos_by_name[nm] = float(ev.target_dof_pos_mu[i]) + ev._apply_control(sleep=False) + zs.append(float(ev.d.qpos[2])) + amax.append(float(np.abs(out).max())) + dqs.append(float(np.abs(ev.d.qvel[ev.actuator_qvel_indices]).max())) + return np.array(zs), np.array(amax), np.array(dqs) + + +def report(name, zs, amax, dqs): + fallen = np.where(zs < 0.3)[0] + # 发散起点: |dq| 首次持续 >1e-3 (噪声底 1e-5 量级) + grow = np.where((dqs > 1e-3))[0] + grow_s = f"发散起@{grow[0]/50:.0f}s" if len(grow) else "未发散" + fall_s = f"FALL@{fallen[0]/50:.1f}s" if len(fallen) else "站满全程" + print(f"{name:28s} z_min={zs.min():.3f} |a|尾={np.percentile(amax[-500:],95):.3f} " + f"{grow_s:12s} {fall_s}", flush=True) + + +if __name__ == "__main__": + print(f"E9: {STEPS} steps ({STEPS/50:.0f}s) — 拍频延迟分水岭探测", flush=True) + report("D2 常数(基线)", *run_split_delayfn(STEPS, lambda k: 2)) + report("D3 常数", *run_split_delayfn(STEPS, lambda k: 3)) + report("D4 常数", *run_split_delayfn(STEPS, lambda k: 4)) + report("D2/3 每步交替", *run_split_delayfn(STEPS, lambda k: 2 + (k % 2))) + report("D2/3 块交替(500步=10s)", + *run_split_delayfn(STEPS, lambda k: 2 + ((k // 500) % 2))) + report("D2/3 慢扫(相位锯齿)", + *run_split_delayfn(STEPS, lambda k: 2 + (1 if (k % 2000) >= 1900 else 0))) + report("D2 + 稀疏单发3(每500)", + *run_split_delayfn(STEPS, lambda k: 3 if k % 500 == 0 else 2))