Files
Robot_Arm/ml/eval/diag_cl.py
T

317 lines
20 KiB
Python
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
#!/usr/bin/env python
"""闭环失败定位(只读,手动运行,n=256 约 35–50 分钟,显存约 3–6 GB)。
PYTHONUTF8=1 python -m ml.eval.diag_cl --ckpt ml/runs/base/last.pt --n 256 --out ml/runs/diag_cl.json
任务 = val 集前 n 题;老师轨迹按任务 id 从 ml/data/shard_*.jsonl 取(取不到的题不进 H 段,数量记在 H_tasks_missing_teacher)。
模型经 ckpt.load_policy 载入(有 EMA 用 EMA;act_scale 随 checkpoint,旧 checkpoint = 1.0)。
段落:
A. 特征口径:val 任务起点(非增广、老师状态)上,Python features() 与数据集 X 的逐组最大差 / 平均差。
B. 同一噪声下两份特征得到的动作差(16 步末端关节差,度)。
C. 旧部署口径(1 段采样、一致性选择、子目标旧规则 timeout)的失败形态:首撞时刻、碰撞步数、首撞组对、最小目标误差、
目标误差进度比、开安全层的到达 / 碰撞 / 刹停。到达 = 时限内到达,碰撞只计时限内(2026-10-19 起)。
H(2026-10-19,三份审查 review_closedloop / review_model_train / review_data_actions 的决定性实验):
H1 老师 oracle 走完整 rollout()(closed_loop.TeacherOracle;select='first'、use_sub=False):
H1_oracle:额外含 oracle 字段——DLS 接管前与老师轨迹的最大关节偏差、到达 / 碰撞、完成时刻 − 老师时长;
H1_oracle_noise_<σ>(σ = 0.05 / 0.1 / 0.2,STEP 为单位,每次规划重采):闭环对残差噪声的敏感度。
H2 前缀消融:H2_open_loop(r=3 的 val 行,前缀 47:68 原样 vs 置零,执行窗口 3..5 的误差);
H2_replan_empty_s{1,4}:队列空了才规划(每次 r=0、提交 3 步),其余同部署。
H3 子目标:H3_headsub_cross_s{1,4}(子目标头 + 越过顶点规则 = 新部署默认)、H3_headsub_timeout_s{1,4}(旧规则 = 旧部署)、
H3_teachersub_cross_s{1,4} / H3_teachersub_timeout_s{1,4}(老师真实途经点全表)、H3_nosub_s{1,4}(不用子目标)。
H4 指标(上面每个闭环变体都输出同一组键,见 metrics());H4_open_loop:按前缀长度 r=0..3 的执行窗口误差。
_s1 / _s4 = MeanFlow 1 段(部署)/ 4 段采样。"""
import argparse, json, math, os
import torch
from ml.common.ckpt import load_policy
from ml.common.kin_torch import Kin
from ml.common.features import features, STEP, H
from ml.data import Samples, load_tasks, read_meta
from ml.sim.closed_loop import rollout, goal_err, DT, TeacherOracle
from ml.eval.replay_teacher import load_teacher, pad_traj, oracle_raw, oracle_summary
# 83 维特征分组(与 features.py 拼接顺序一致)
GROUPS = [('sincos', 0, 12), ('joint_norm', 12, 15), ('dq', 15, 21), ('p', 21, 24), ('Rq', 24, 30), ('T', 30, 33),
('R', 33, 39), ('T-p', 39, 42), ('T-p_clamp', 42, 45), ('mode', 45, 47), ('prefix', 47, 65), ('mask', 65, 68),
('sub', 68, 74), ('sub_flag', 74, 75), ('gaps', 75, 83)]
PROG_T = (0.5, 1.0, 2.0) # 目标误差 / 起点误差 的取样时刻(s)
K1, K2 = int(round(1.0 / DT)), int(round(2.0 / DT)) # 1 s / 2 s 对应的步数
def pct(v, p):
v = sorted(x for x in v if x is not None and x == x)
return v[min(len(v) - 1, int(len(v) * p))] if v else None
def qt(t, p):
t = t[torch.isfinite(t)]
return round(float(t.kthvalue(max(1, min(len(t), int(math.ceil(p * len(t)))))).values), 4) if len(t) else None
def rnd(x, n=4):
if isinstance(x, list): return [rnd(v, n) for v in x]
return None if x is None or x != x else round(float(x), n)
def pad_len(tr, n):
"""轨迹 [B,T,6] 末帧重复补到至少 n 帧(到达后冻结,补帧与冻结等价)。"""
if tr.shape[1] >= n: return tr
return torch.cat([tr, tr[:, -1:].expand(-1, n - tr.shape[1], -1)], 1)
def batch_stats(kin, ch, per, TR):
"""一批的逐题量(CPU)。窗口:幅度 / 净位移 / TCP 用前 1 s,变号率用前 2 s 的 3 步块。"""
dev = TR.device
stp = torch.tensor(STEP, device=dev)
tr = pad_len(per['traj'].to(dev), K2 + 1); te = pad_len(TR, K2 + 1)
B = tr.shape[0]
amp = lambda x: (x[:, 1:K1 + 1] - x[:, :K1]).abs().sum(1) # [B,6] 前 1 s 关节行程
net = lambda x: (x[:, K1] - x[:, 0]).abs() # [B,6] 前 1 s 净位移
def tcp(x):
p = kin.fk(x[:, :K1 + 1].reshape(-1, 6)).view(B, K1 + 1, 3)
return (p[:, 1:] - p[:, :-1]).norm(dim=-1).sum(1), (p[:, -1] - p[:, 0]).norm(dim=-1)
def flips(x):
idx = torch.arange(0, K2 + 1, 3, device=dev)
b = x[:, idx[1:]] - x[:, idx[:-1]] # [B,nb,6] 3 步块增量
big = b.abs() > 0.1 * stp
v = big[:, 1:] & big[:, :-1]
f = v & (torch.sign(b[:, 1:]) != torch.sign(b[:, :-1]))
return f.sum(1).float(), v.sum(1).float()
tl_p, tn_p = tcp(tr); tl_t, tn_t = tcp(te)
fp_, vp_ = flips(tr); ft_, vt_ = flips(te)
ex = torch.tensor([t.get('exec_s') or float('nan') for t in ch])
return {'ok': per['ok'], 'col': per['col'] > 0, 'isD': torch.tensor([t.get('cat') == 'D' for t in ch]),
'pose': torch.tensor([bool(t.get('R')) for t in ch]), 'key': [t.get('cat', '?') + '/' + t.get('mode', '?') for t in ch],
'r2': per['goal_ratio_2s'], 'err': per['err'], 'first_hit': per['first_hit'], 'brakes': per['brakes'],
'tratio': per['t_done'] / ex,
'amp_p': amp(tr).cpu(), 'amp_t': amp(te).cpu(), 'net_p': net(tr).cpu(), 'net_t': net(te).cpu(),
'tl_p': tl_p.cpu(), 'tl_t': tl_t.cpu(), 'tn_p': tn_p.cpu(), 'tn_t': tn_t.cpu(),
'fl_p': fp_.cpu(), 'fv_p': vp_.cpu(), 'fl_t': ft_.cpu(), 'fv_t': vt_.cpu()}
def metrics(stats):
"""H4 指标(每个闭环变体同一组键):
reach / collision:cat≠D、时限内(D 类目标不可达,单列在 D);
goal_ratio_2s_p50:2 s 时目标误差 / 起点误差;end_goal_err_mm_p50;first_hit_s_p50(撞了的题);t_done_over_teacher_p50(到达的题);
joint_amp_ratio[6]:前 1 s 各轴关节行程 Σ|Δq| 策略 / 老师(全体题合计;J6 只算 pose 题,pos 题老师冻结 J6);
joint_net_ratio[6]:前 1 s 各轴净位移 |q(1s) − q(0)| 策略 / 老师(同上);j6_pos_amp_deg_p50:pos 题策略 J6 前 1 s 行程(度,老师为 0);
tcp_path_ratio_p50 / tcp_net_ratio_p50:前 1 s TCP 路程 / 净位移 策略 / 老师(逐题比,老师 > 1 mm 的题);
sign_flip_rate[6] / sign_flip_rate_teacher[6]:前 2 s 相邻 3 步块增量变号的比例(两块都 > 0.1·STEP 才计);
by_cat_mode:各 cat/mode 的 n、reach、collision、goal_ratio_2s_p50(不含 D);D:n、collision、end_goal_err_mm_p50、goal_ratio_2s_p50。"""
c = {k: (sum((s[k] for s in stats), []) if isinstance(stats[0][k], list) else torch.cat([s[k] for s in stats])) for k in stats[0]}
keys = c['key']
m = ~c['isD']; pose = c['pose']
def jr(a, b):
out = []
for j in range(6):
sel = m & pose if j == 5 else m
den = float(b[sel, j].sum()); out.append(float(a[sel, j].sum()) / den if den > 1e-6 else None)
return rnd(out)
tl_ok = m & (c['tl_t'] > 1); tn_ok = m & (c['tn_t'] > 1)
out = {'n': int(m.sum()),
'reach': rnd(c['ok'][m].float().mean()) if m.any() else None,
'collision': rnd(c['col'][m].float().mean()) if m.any() else None,
'goal_ratio_2s_p50': qt(c['r2'][m], .5), 'end_goal_err_mm_p50': qt(c['err'][m], .5),
'first_hit_s_p50': qt(c['first_hit'][m], .5), 't_done_over_teacher_p50': qt(c['tratio'][m & c['ok']], .5),
'brakes_p50': qt(c['brakes'][m], .5),
'joint_amp_ratio': jr(c['amp_p'], c['amp_t']), 'joint_net_ratio': jr(c['net_p'], c['net_t']),
'j6_pos_amp_deg_p50': qt(c['amp_p'][m & ~pose, 5], .5),
'tcp_path_ratio_p50': qt(c['tl_p'][tl_ok] / c['tl_t'][tl_ok], .5), 'tcp_net_ratio_p50': qt(c['tn_p'][tn_ok] / c['tn_t'][tn_ok], .5),
'sign_flip_rate': rnd((c['fl_p'][m].sum(0) / c['fv_p'][m].sum(0).clamp(min=1)).tolist()),
'sign_flip_rate_teacher': rnd((c['fl_t'][m].sum(0) / c['fv_t'][m].sum(0).clamp(min=1)).tolist())}
by = {}
for i, k in enumerate(keys):
if not bool(m[i]): continue
by.setdefault(k, []).append(i)
out['by_cat_mode'] = {k: {'n': len(ix), 'reach': rnd(c['ok'][ix].float().mean()), 'collision': rnd(c['col'][ix].float().mean()),
'goal_ratio_2s_p50': qt(c['r2'][ix], .5)} for k, ix in sorted(by.items())}
d = c['isD']
out['D'] = {'n': int(d.sum()), 'collision': rnd(c['col'][d].float().mean()) if d.any() else None,
'end_goal_err_mm_p50': qt(c['err'][d], .5), 'goal_ratio_2s_p50': qt(c['r2'][d], .5)}
return out
def teacher_subs(recs, device):
"""老师真实途经点全表(path 去掉起终点)→ (subs [B,M,6], n [B]),供 rollout(sub_override=...)。"""
vs = [r['path'][1:-1] for r in recs]
M = max(1, max(len(v) for v in vs))
subs = torch.zeros(len(vs), M, 6, device=device); n = torch.zeros(len(vs), dtype=torch.long, device=device)
for i, v in enumerate(vs):
if v:
x = torch.tensor(v, dtype=torch.float32, device=device); subs[i, :len(v)] = x; subs[i, len(v):] = x[-1]; n[i] = len(v)
return subs, n
def main():
ap = argparse.ArgumentParser()
ap.add_argument('--ckpt', required=True); ap.add_argument('--samples', default='ml/data/samples')
ap.add_argument('--shards', default='ml/data/shard_*.jsonl')
ap.add_argument('--n', type=int, default=256); ap.add_argument('--bs', type=int, default=64)
ap.add_argument('--device', default='cuda' if torch.cuda.is_available() else 'cpu'); ap.add_argument('--out', default='ml/runs/diag_cl.json')
a = ap.parse_args()
dev = a.device
model, ck = load_policy(a.ckpt, dev)
kin = Kin(device=dev)
astep = torch.tensor(STEP, device=dev) * model.act_scale # 动作 → 度
data_scale = float(read_meta(a.samples).get('act_scale', 1.0))
out = {'ckpt': a.ckpt, 'step': ck.get('step'), 'weights': 'ema' if ck.get('ema') is not None else 'model',
'act_scale_ckpt': model.act_scale, 'act_scale_data': data_scale}
tasks = [t for t in load_tasks([a.samples]) if t['split'] == 'val'][:a.n]
by_tid = {t['tid']: t for t in tasks}
def save():
s = json.dumps(out, indent=1, ensure_ascii=False)
os.makedirs(os.path.dirname(a.out) or '.', exist_ok=True)
open(a.out + '.tmp', 'w', encoding='utf-8').write(s); os.replace(a.out + '.tmp', a.out)
return s
# ---------- A / B:起点特征口径 ----------
vd = Samples(a.samples, device=dev, split='val')
tid = vd.TID.long()
first = torch.ones_like(tid, dtype=torch.bool); first[1:] = tid[1:] != tid[:-1] # 每题第一行 = t=0 样本
idx = [i for i in first.nonzero().flatten().tolist() if int(tid[i]) in by_tid]
X0 = vd.X[idx].float()
tk = [by_tid[int(tid[i])] for i in idx]
q0 = torch.tensor([t['q0'] for t in tk], dtype=torch.float32, device=dev)
qx = torch.atan2(X0[:, 0:12:2], X0[:, 1:12:2]) * 180 / torch.pi
dqx = torch.remainder(qx - q0 + 180, 360) - 180
clean = dqx.abs().max(-1).values < 1e-2 # 未被噪声增广的起点
out['A_rows'] = int(clean.sum()); out['A_rows_aug_skipped'] = int((~clean).sum())
if clean.any():
Xc = X0[clean]; qc = q0[clean]; tc = [t for t, c in zip(tk, clean.tolist()) if c]
T = torch.tensor([t['T'] for t in tc], dtype=torch.float32, device=dev)
R = torch.tensor([t['R'] if t.get('R') else [0.] * 9 for t in tc], dtype=torch.float32, device=dev)
B = qc.shape[0]
qpc = torch.tensor([t.get('qp0') or t['q0'] for t in tc], dtype=torch.float32, device=dev)
pre = Xc[:, 47:65].view(B, 3, 6) * torch.tensor(STEP, device=dev) # 前缀 / 子目标直接取数据集里的,只比状态相关部分
pl = Xc[:, 65:68].sum(-1).round()
sf = Xc[:, 74]
sub = qc + Xc[:, 68:74] * 180
Fp = features(kin, qc, qpc, T, R, pre, pl, sub, sf)
out['A_maxdiff'] = {g: (Fp[:, s:e] - Xc[:, s:e]).abs().max().item() for g, s, e in GROUPS}
out['A_meandiff'] = {g: (Fp[:, s:e] - Xc[:, s:e]).abs().mean().item() for g, s, e in GROUPS}
with torch.no_grad():
z = torch.randn(B * 8, H, 6, device=dev, generator=torch.Generator(device=dev).manual_seed(7))
a1, _, _ = model.sample_k(Xc, 8, z); a2, _, _ = model.sample_k(Fp, 8, z)
d = ((a1.clamp(-1, 1) - a2.clamp(-1, 1)) * astep).sum(2).abs().max(-1).values # [B,K] 16 步末端关节差(度)
mag = (a1.clamp(-1, 1) * astep).sum(2).abs().max(-1).values
out['B_act_diff_deg'] = {'mean': d.mean().item(), 'p90': d.flatten().kthvalue(max(1, int(0.9 * d.numel()))).values.item(),
'ref_motion_deg_mean': mag.mean().item()}
# ---------- H4 / H2 开环:执行窗口误差(val 行,老师状态) ----------
# 闭环稳态 r=3,真正执行的是 act[r:r+3];原来的 ol_fk_mm 是 16 步终点误差,前缀那几步几乎不会错,看不出执行窗口。
with torch.no_grad():
pl_all = vd.X[:, 65:68].sum(-1).round().long()
gsel = torch.Generator(device=dev); gsel.manual_seed(321)
def rows_with(r, n=1024):
ix = (pl_all == r).nonzero().flatten()
return ix[torch.randperm(len(ix), device=dev, generator=gsel)[:n]] if len(ix) else ix
def win_err(X, Y, r, steps, k0=None):
k0 = r if k0 is None else k0
z = torch.randn(X.shape[0] * 8, H, 6, device=dev, generator=torch.Generator(device=dev).manual_seed(11))
act, _, _ = model.sample_k(X, 8, z, steps=steps); act = act.float().clamp(-1, 1)
e = ((act[:, :, k0:k0 + 3] - Y[:, None, k0:k0 + 3]) * astep).sum(2).abs() # [N,K,6] 窗口累计误差(度)
emax = e.max(-1).values.flatten()
res = {'n_rows': int(X.shape[0]), 'deg_mean': rnd(emax.mean()), 'deg_p90': qt(emax, .9), 'per_joint_deg_mean': rnd(e.mean((0, 1)).tolist())}
if r > 0 and k0 == r:
ce = ((act[:, :, :r] - Y[:, None, :r]) * astep).abs().max(-1).values.max(-1).values.flatten()
res['prefix_copy_err_deg_mean'] = rnd(ce.mean())
return res
ol4, ol2 = {}, {}
for steps in (1, 4):
for r in range(4):
ix = rows_with(r)
if len(ix): ol4['s%d_r%d' % (steps, r)] = win_err(vd.X[ix].float(), vd.Y[ix].float(), r, steps)
ix = rows_with(3)
if len(ix):
X3 = vd.X[ix].float(); Y3 = vd.Y[ix].float(); X3z = X3.clone(); X3z[:, 47:68] = 0
ol2['s%d_prefix_orig' % steps] = win_err(X3, Y3, 3, steps)
ol2['s%d_prefix_zeroed' % steps] = win_err(X3z, Y3, 0, steps, k0=3)
out['H4_open_loop'] = ol4; out['H2_open_loop'] = ol2
del vd
if dev.startswith('cuda'): torch.cuda.empty_cache()
# ---------- C:旧部署口径的闭环失败形态(子目标旧规则,便于与前几轮对比) ----------
first_t, col_steps, pairs, min_err, reach = [], [], {}, [], 0
prog = {ts: [] for ts in PROG_T}
reach_s, brakes_s, col_s = 0, [], 0
for i in range(0, len(tasks), a.bs):
ch = tasks[i:i + a.bs]
with torch.no_grad():
_, per = rollout(model, kin, ch, dev, K=8, safety=False, seed=i, return_traj=True, sub_rule='timeout')
tr = per['traj'].to(dev) # [B,Tn,6]
B, Tn, _ = tr.shape
flat = tr.reshape(-1, 6) # 分块 ≤1024 点(同 dagger/collect.py 的显存修复)
gm = torch.cat([kin.gaps(flat[s:s + 1024], 0.5)['gm'] for s in range(0, flat.shape[0], 1024)]).view(B, Tn, 49)
inlim = (torch.arange(Tn, device=dev)[None] * DT) <= per['limit'].to(dev)[:, None] + 1e-6
hit = (gm.min(-1).values <= 0) & inlim # [B,Tn] 只计时限内
Tt = torch.tensor([t['T'] for t in ch], dtype=torch.float32, device=dev)
Rt = torch.tensor([t['R'] if t.get('R') else [0.] * 9 for t in ch], dtype=torch.float32, device=dev)
e, _ = goal_err(kin, tr.reshape(-1, 6), Tt.repeat_interleave(Tn, 0), Rt.repeat_interleave(Tn, 0))
e = e.view(B, Tn)
for b in range(B):
for ts in PROG_T:
k = min(Tn - 1, int(round(ts / DT))); prog[ts].append((e[b, k] / e[b, 0].clamp(min=1e-3)).item())
for b in range(B):
if per['ok'][b]: reach += 1
min_err.append(e[b].min().item()); col_steps.append(int(per['col'][b]))
h = hit[b].nonzero().flatten()
if len(h):
k = int(h[0]); first_t.append(k * DT)
pr = int(gm[b, k].argmin()); key = '%d-%d' % (pr // 7, pr % 7); pairs[key] = pairs.get(key, 0) + 1
rs, ps = rollout(model, kin, ch, dev, K=8, safety=True, seed=i, sub_rule='timeout')
reach_s += int(ps['ok'].sum()); col_s += int((ps['col'] > 0).sum()); brakes_s += ps['brakes'].tolist()
n = len(tasks)
out['C'] = {'n': n, 'reach': reach / n, 'collided': len(first_t) / n,
'first_hit_s': {'p10': pct(first_t, .1), 'p50': pct(first_t, .5), 'p90': pct(first_t, .9),
'within_0.1s': sum(t <= 0.1 for t in first_t) / max(1, len(first_t))},
'col_steps': {'p50': pct(col_steps, .5), 'p90': pct(col_steps, .9)},
'first_hit_pair(group a-b)': dict(sorted(pairs.items(), key=lambda kv: -kv[1])[:8]),
'min_goal_err_mm': {'p10': pct(min_err, .1), 'p50': pct(min_err, .5), 'p90': pct(min_err, .9)},
'safety_on': {'reach': reach_s / n, 'collided': col_s / n, 'brakes_p50': pct(brakes_s, .5)}}
out['C']['goal_err_ratio_p50'] = {'%.1fs' % ts: pct(v, .5) for ts, v in prog.items()} # <1 = 在靠近目标
print(save(), flush=True)
# ---------- H:闭环变体(同一批 val 题,老师轨迹按 id 取) ----------
teach = load_teacher(a.shards, [t['id'] for t in tasks])
tH = [t for t in tasks if t['id'] in teach]
out['H_tasks'] = len(tH); out['H_tasks_missing_teacher'] = len(tasks) - len(tH)
def variant(oracle_sigma=None, teacher_sub=False, **kw):
stats, raws = [], []
for i in range(0, len(tH), a.bs):
ch = tH[i:i + a.bs]; recs = [teach[t['id']] for t in ch]
TR = pad_traj([r['traj'] for r in recs], dev)
kw2 = dict(kw)
if oracle_sigma is not None:
kw2.update(oracle=TeacherOracle(TR, act_scale=data_scale, sigma=oracle_sigma), select='first', use_sub=False)
if teacher_sub: kw2['sub_override'] = teacher_subs(recs, dev)
with torch.no_grad():
_, per = rollout(None if oracle_sigma is not None else model, kin, ch, dev, K=8, seed=i, return_traj=True, **kw2)
stats.append(batch_stats(kin, ch, per, TR))
if oracle_sigma is not None: raws.append(oracle_raw(per, TR, [len(r['traj']) for r in recs], ch))
del per
res = metrics(stats)
if oracle_sigma is not None and oracle_sigma == 0: res['oracle'] = oracle_summary(raws)
if dev.startswith('cuda'): torch.cuda.empty_cache()
return res
plan = [('H1_oracle', dict(oracle_sigma=0.0))]
plan += [('H1_oracle_noise_%g' % s, dict(oracle_sigma=s)) for s in (0.05, 0.1, 0.2)]
for st in (1, 4):
plan += [('H3_headsub_cross_s%d' % st, dict(steps=st, sub_rule='cross')),
('H3_headsub_timeout_s%d' % st, dict(steps=st, sub_rule='timeout')),
('H3_teachersub_cross_s%d' % st, dict(steps=st, sub_rule='cross', teacher_sub=True)),
('H3_teachersub_timeout_s%d' % st, dict(steps=st, sub_rule='timeout', teacher_sub=True)),
('H3_nosub_s%d' % st, dict(steps=st, use_sub=False)),
('H2_replan_empty_s%d' % st, dict(steps=st, sub_rule='cross', replan='empty'))]
for name, kw in plan:
out[name] = variant(**kw)
save(); print('[diag] %s 完成' % name, flush=True)
print(save(), flush=True)
if __name__ == '__main__':
main()