test: 摔倒根因排查工具链——dance_soak 压测驱动与 E1-E9 离线实验

- dance_soak.py: 等价网页指令的自动驱动器(5舞循环+行走+站立探针,
  摔倒自动落盘150s现场并复位续测, ring buffer 含 z/dq/q/tgt/时龄)
- e9_beat_delay.py: 延迟分水岭实验——常数2拍稳240s, 站稳后切3拍
  62s起τ≈2.2s发散→215s倒; 交替/孤立毛刺可衰减(定投修复的依据)
- e6/e4/e3/e1: 分离架构延迟对照 / 回放判别 / 官方站立基线 /
  last_action 极限环(调查链存档)

Co-Authored-By: Claude Code <noreply@anthropic.com>
This commit is contained in:
d-robotics 2026-10-13 13:58:03 +08:00
parent 026f819ff3
commit 7dd3a9c0f5
6 changed files with 1075 additions and 0 deletions

View File

@ -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-<pid>。
用法:
python dance_soak.py --target-dances 200 # ≥200 支舞
python dance_soak.py --target-dances 200 --dry-walk # 只行走不跳舞(快验链路)
输出: logs/soak_<ts>/ {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()

71
tests/e1_stand_loop.py Normal file
View File

@ -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(量级≈倒地)")

View File

@ -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 闭环有偏差, 需逐项对拍")

155
tests/e4_replay_obs.py Normal file
View File

@ -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 <fall.json> [起始帧] [帧数]
"""
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 忠实+混沌发散; 首桶即大 => 内部状态泄漏")

248
tests/e6_arch_delay.py Normal file
View File

@ -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}")

91
tests/e9_beat_delay.py Normal file
View File

@ -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))