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:
parent
026f819ff3
commit
7dd3a9c0f5
436
console/robot_service/dance_soak.py
Normal file
436
console/robot_service/dance_soak.py
Normal 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
71
tests/e1_stand_loop.py
Normal 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(量级≈倒地)")
|
||||
74
tests/e3_official_stand.py
Normal file
74
tests/e3_official_stand.py
Normal 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
155
tests/e4_replay_obs.py
Normal 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
248
tests/e6_arch_delay.py
Normal 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
91
tests/e9_beat_delay.py
Normal 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))
|
||||
Loading…
x
Reference in New Issue
Block a user