diag: H 段（老师 oracle 走 rollout / 前缀消融 / 子目标越过顶点规则 / 分轴与分类别指标）；训练：EMA、选模不落在 p_neq 预热、闭环分数选模、24000 步余弦、p_neq 0.5 + p_edge 0.1、子目标头切断抄输入、前缀/dq 增广；仿真：last 记真正提交的候选、碰撞与到达按每题时限、回放门禁读 pass+指纹并走 rollout；数据：动作标签按 1.25·STEP 归一化（act_scale 随 meta/ckpt/ONNX 传递，旧模型 1.0）、收回亏空结转、DAgger 带 qPrev 与起步速度、打包签名变更自动重打；ik-policy.js 同步（actScale、子目标规则、DLS 时清 lastPlan）；本机执行清单与 README 更新
This commit is contained in:
Hark
+153
-47
@@ -1,19 +1,23 @@
|
||||
"""闭环仿真（验证 / 挖错 / 门禁共用，PyTorch 批量，无梯度）——按部署节奏运行：
|
||||
50 Hz 执行；队列 ≤ 3 步时推理一次（已提交前缀 r = 队列长度），提交接下来 3 步；
|
||||
冷启动用价值头对候选末态打分（短前瞻），之后按与上一计划的一致性选候选；
|
||||
接近目标 < 5 mm 时用局部 DLS 精修；子目标头在新目标时给出途经点（到达 < 5° 或 1.5 s 超时后切下一个）。
|
||||
冷启动用价值头对候选末态打分（短前瞻），之后按与上一条**真正提交**的计划的一致性选候选；
|
||||
接近目标 < 5 mm 时用局部 DLS 精修；子目标头在新目标时给出途经点，按「越过顶点」规则切下一个（与标签同口径，见 sub_advance）。
|
||||
safety=False 时不做任何碰撞拦截（暴露真实错误，挖错用）；safety=True 时近似部署安全层（候选提交步有干涉则换下一条，全不过则停一步）。
|
||||
动作反归一化：inc = clamp(act, −1, 1) · act_scale · STEP；act_scale 来自 checkpoint（旧模型缺省 1.0，见 ml/common/ckpt.load_policy）。
|
||||
碰撞与到达只在每题自己的时限内计（短题与长题同批时不把超时后的部分算进去）。
|
||||
python -m ml.sim.closed_loop --ckpt ml/runs/base/best.pt --samples ml/data/samples --split val --n 1000 [--safety]"""
|
||||
import argparse, json, math, os
|
||||
import torch
|
||||
|
||||
from ml.common.kin_torch import Kin
|
||||
from ml.common.features import features, STEP, H, GAP_CAP, D2R, wrap180
|
||||
from ml.common.features import features, STEP, VMAX, H, GAP_CAP, D2R, wrap180
|
||||
|
||||
DT = 0.02
|
||||
|
||||
|
||||
FIRST_DEG = 5.0 # R7 首次方向判定阈值（度）：小于它的抖动不算选边
|
||||
SUB_LEAD_S = 0.02 # 子目标「越过顶点」提前量（s）：标签在老师 τ > τ_顶点 − 0.02 时就换到下一个顶点（build_dataset.cjs remain 过滤）
|
||||
SUB_REACH_DEG, SUB_TIMEOUT_S = 5.0, 1.5 # 旧规则（sub_rule='timeout'）：到达 < 5° 或 1.5 s 超时
|
||||
|
||||
def tasks_to_tensors(tasks, device):
|
||||
q0 = torch.tensor([t['q0'] for t in tasks], dtype=torch.float32, device=device)
|
||||
@@ -28,6 +32,12 @@ def tasks_to_tensors(tasks, device):
|
||||
NOREF_SEC = 6.0 # 无老师参考题的定长时限（s）
|
||||
|
||||
|
||||
def time_limit(ex, max_sec=None):
|
||||
"""每题时限（s）：有老师 = 老师时长 × 2（至少 3 s）；无老师 = NOREF_SEC；max_sec 覆盖全部。"""
|
||||
if max_sec is not None: return torch.full_like(ex, max_sec)
|
||||
return torch.where(torch.isfinite(ex), torch.clamp(2 * ex, min=3.0), torch.full_like(ex, NOREF_SEC))
|
||||
|
||||
|
||||
def goal_err(kin, q, T, R):
|
||||
xf = kin.cum_xf(q)
|
||||
p = kin.fk(q, xf); Rq = kin.orient(q, xf).reshape(-1, 9)
|
||||
@@ -64,58 +74,139 @@ def dls_steps(kin, q, T, R, n=3, scale=0.3, lam=0.5):
|
||||
return torch.stack(out, 1)
|
||||
|
||||
|
||||
def jdiff(kin, a, b):
|
||||
"""a − b（度），连续轴取最短弧。"""
|
||||
d = a - b
|
||||
return torch.where(kin.cont, wrap180(d), d)
|
||||
|
||||
|
||||
def sub_advance(kin, subs, nsub, sub_prev, sub_t, qe, tnow, rule='cross'):
|
||||
"""子目标推进（每次规划前调用，每次最多推进一个）。subs [B,M,6] 待走途经点（绝对角），nsub [B] 剩余个数，
|
||||
sub_prev [B,6] 上一个已越过的点（冷启动时 = 起点），sub_t [B] 当前子目标开始时刻。返回 (subs, nsub, sub_prev, sub_t)。
|
||||
rule='cross'（默认）：qe 沿拐角平分面法向越过当前顶点（提前 SUB_LEAD_S 秒）即切到下一个。
|
||||
2026-10-19（审查 review_data_actions C5 / review_model_train P0-1）：标签里 sub = 老师「下一个尚未经过的顶点」，
|
||||
老师 τ 一过顶点（−0.02 s）立刻换；旧规则要等最大关节差 < 5° 或 1.5 s 超时，策略在顶点附近 5–10° 进不去时
|
||||
sub 一直指着身后的点，这种「sub 已在身后」的特征序列训练里从未出现。运行时没有老师 τ，用几何等价物：
|
||||
进入方向 a = 顶点 − 上一点、离开方向 b = 下一顶点 − 顶点（最后一个顶点取 a），各轴按 VMAX 折成秒（与 τ 同量纲），
|
||||
法向 n = â + b̂；(qe − 顶点)·n̂ ≥ −SUB_LEAD_S 判为越过。没有超时（标签也没有）。
|
||||
rule='timeout'：旧部署口径，到达 < 5° 或 1.5 s 超时（诊断对照用）。
|
||||
ik-policy.js Runner（subRule）与 apg_finetune.py 用同一规则，三处同步改。"""
|
||||
has = nsub > 0
|
||||
if not bool(has.any()): return subs, nsub, sub_prev, sub_t
|
||||
cur = subs[:, 0]
|
||||
if rule == 'timeout':
|
||||
dd = jdiff(kin, cur, qe).abs().max(-1).values
|
||||
adv = has & ((dd < SUB_REACH_DEG) | (tnow - sub_t > SUB_TIMEOUT_S))
|
||||
elif rule == 'cross':
|
||||
vmax = torch.tensor(VMAX, device=qe.device, dtype=qe.dtype)
|
||||
un = lambda v: v / v.norm(dim=-1, keepdim=True).clamp(min=1e-9)
|
||||
a = jdiff(kin, cur, sub_prev) / vmax
|
||||
nxt = subs[:, 1] if subs.shape[1] > 1 else cur
|
||||
b = torch.where((nsub > 1)[:, None], jdiff(kin, nxt, cur) / vmax, a)
|
||||
n = un(un(a) + un(b))
|
||||
proj = (jdiff(kin, qe, cur) / vmax * n).sum(-1)
|
||||
adv = has & (proj >= -SUB_LEAD_S)
|
||||
else:
|
||||
raise ValueError('sub_rule: ' + str(rule))
|
||||
subs = torch.where(adv[:, None, None], torch.cat([subs[:, 1:], subs[:, -1:]], 1), subs)
|
||||
sub_prev = torch.where(adv[:, None], cur, sub_prev)
|
||||
nsub = torch.where(adv, nsub - 1, nsub); sub_t = torch.where(adv, torch.full_like(sub_t, tnow), sub_t)
|
||||
return subs, nsub, sub_prev, sub_t
|
||||
|
||||
|
||||
class TeacherOracle:
|
||||
"""老师当策略（2026-10-19，审查 review_closedloop D1 / review_data_actions §4.1）：
|
||||
规划时从当前时间下标 it 取老师 50 Hz 轨迹的 16 步增量，按与 build_dataset.cjs 相同的标签口径
|
||||
act = clamp(Δ / (act_scale·STEP), −1, 1) 交给 rollout()，其余（队列、前缀、提交、qe、DLS、done、碰撞、时限）一行不改。
|
||||
traj [B,Tmax,6]：每题老师轨迹，末帧重复补齐（按 id 从 ml/data/shard_*.jsonl 取）。
|
||||
sigma > 0：每次规划给 act 加 N(0, σ²)（σ 以 STEP 为单位，各候选独立），测闭环对残差噪声的敏感度。
|
||||
跟随时第 it 步的 q 应等于 traj[:, it]（DLS 接管或到达冻结之后除外）。"""
|
||||
head = 'oracle'
|
||||
|
||||
def __init__(self, traj, act_scale=1.0, sigma=0.0):
|
||||
self.traj, self.act_scale, self.sigma = traj, float(act_scale), float(sigma)
|
||||
|
||||
def plan(self, it, K, g):
|
||||
B, Tm, _ = self.traj.shape
|
||||
dev = self.traj.device
|
||||
idx = (it + torch.arange(H + 1, device=dev)).clamp(max=Tm - 1)
|
||||
seg = self.traj[:, idx]
|
||||
astep = torch.tensor(STEP, device=dev) * self.act_scale
|
||||
act = ((seg[:, 1:] - seg[:, :-1]) / astep).clamp(-1, 1)
|
||||
act = act[:, None].expand(B, K, H, 6).contiguous()
|
||||
if self.sigma > 0:
|
||||
act = act + torch.randn(B, K, H, 6, device=dev, generator=g) * (self.sigma / self.act_scale)
|
||||
return act
|
||||
|
||||
|
||||
@torch.no_grad()
|
||||
def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None, return_traj=False, use_sub=True, sub_override=None, qp0=None, steps=1, z_scale=1.0, select='consist'):
|
||||
"""use_sub=False：不用子目标（sf=0）；sub_override=(subs [B,2,6] 绝对关节角, need [B] bool)：冷启动时用给定途经点代替子目标头；qp0 [B,6]：起点的上一帧（默认 = q0，即静止起步）。steps：MeanFlow 采样段数；z_scale：噪声缩放（0 = 只取「均值」样本）；select：consist（部署口径：冷启动价值前瞻、之后一致性）/ value（每次都价值前瞻）/ first（不选，取第 0 条）。均为诊断用"""
|
||||
def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None, return_traj=False, use_sub=True, sub_override=None,
|
||||
qp0=None, steps=1, z_scale=1.0, select='consist', oracle=None, replan='r3', sub_rule='cross'):
|
||||
"""use_sub=False：不用子目标（sf=0）；sub_override=(subs [B,M,6] 绝对关节角, n [B] 个数 或 need [B] bool)：冷启动时用给定途经点代替子目标头；
|
||||
qp0 [B,6]：起点的上一帧（默认 = q0，即静止起步）。steps：MeanFlow 采样段数；z_scale：噪声缩放；
|
||||
select：consist（部署口径：冷启动价值前瞻、之后与上一条已提交计划的一致性）/ value（每次都价值前瞻）/ first（不选，取第 0 条）。
|
||||
oracle：TeacherOracle，代替 model 出动作（model 可为 None，须配 select='first'、use_sub=False）。
|
||||
replan：'r3'（部署：队列 ≤ 3 步就规划，稳态 r=3）/ 'empty'（队列空了才规划，每次 r=0，前缀消融用）。
|
||||
sub_rule：子目标推进规则，见 sub_advance。除默认值外均为诊断用。"""
|
||||
g = torch.Generator(device=device); g.manual_seed(seed)
|
||||
q0, T, R, ex, tr = tasks_to_tensors(tasks, device)
|
||||
B = q0.shape[0]
|
||||
step = torch.tensor(STEP, device=device)
|
||||
act_scale = oracle.act_scale if oracle is not None else float(getattr(model, 'act_scale', 1.0))
|
||||
astep = step * act_scale # 动作反归一化：每单位 act 对应的度数
|
||||
has_ref = torch.isfinite(ex)
|
||||
limit = torch.where(has_ref, torch.clamp(2 * ex, min=3.0), torch.full_like(ex, NOREF_SEC)) if max_sec is None else torch.full_like(ex, max_sec)
|
||||
limit = time_limit(ex, max_sec)
|
||||
nmax = int(math.ceil(limit.max().item() / DT))
|
||||
q, qp = q0.clone(), (qp0.to(device) if qp0 is not None else q0.clone())
|
||||
queue = torch.zeros(B, 0, 6, device=device)
|
||||
last = None
|
||||
subs = torch.zeros(B, 2, 6, device=device); nsub = torch.zeros(B, dtype=torch.long, device=device); sub_t = torch.zeros(B, device=device)
|
||||
last = None; last_ok = torch.zeros(B, dtype=torch.bool, device=device)
|
||||
subs = torch.zeros(B, 2, 6, device=device); nsub = torch.zeros(B, dtype=torch.long, device=device)
|
||||
sub_t = torch.zeros(B, device=device); sub_prev = q0.clone()
|
||||
done = torch.zeros(B, dtype=torch.bool, device=device); t_done = torch.full((B,), float('nan'), device=device)
|
||||
col = torch.zeros(B, device=device); travel = torch.zeros(B, device=device); brakes = torch.zeros(B, device=device)
|
||||
first_hit = torch.full((B,), float('nan'), device=device); dls_it = torch.full((B,), -1, dtype=torch.long, device=device)
|
||||
traj = [q.clone()] if return_traj else None
|
||||
n_start = int(round(0.5 / DT)); moved_start = None # R2 起步失败：0.5 s 内关节总行程 < 1°
|
||||
k2s = int(round(2.0 / DT)); e2 = None
|
||||
e0, _ = goal_err(kin, q0, T, R)
|
||||
j1_acc = torch.zeros(B, device=device); j1_max = torch.zeros(B, device=device); j1_min = torch.zeros(B, device=device)
|
||||
j1_first = torch.zeros(B, device=device) # 2026-10-18 第五轮（审查 N-3）：J1 累计偏离首次超过 FIRST_DEG 时的方向（真正的"首次方向"）
|
||||
cold = True
|
||||
ar = torch.arange(B, device=device)
|
||||
for it in range(nmax):
|
||||
tnow = it * DT
|
||||
if queue.shape[1] <= 3:
|
||||
if (queue.shape[1] <= 3) if replan == 'r3' else (queue.shape[1] == 0):
|
||||
r = queue.shape[1]
|
||||
pre = torch.zeros(B, 3, 6, device=device); pre[:, :r] = queue[:, :r]
|
||||
qe = q + queue.sum(1)
|
||||
e, oe = goal_err(kin, qe, T, R)
|
||||
near = (e < 5) & ((R.abs().sum(-1) == 0) | (oe < 5))
|
||||
# 子目标推进
|
||||
if (nsub > 0).any():
|
||||
cur = subs[:, 0]
|
||||
dd = torch.where(kin.cont, wrap180(cur - qe), cur - qe).abs().max(-1).values
|
||||
adv = (nsub > 0) & ((dd < 5) | (tnow - sub_t > 1.5))
|
||||
subs = torch.where(adv[:, None, None], torch.cat([subs[:, 1:], subs[:, 1:]], 1), subs)
|
||||
nsub = torch.where(adv, nsub - 1, nsub); sub_t = torch.where(adv, torch.full_like(sub_t, tnow), sub_t)
|
||||
subs, nsub, sub_prev, sub_t = sub_advance(kin, subs, nsub, sub_prev, sub_t, qe, tnow, sub_rule)
|
||||
sf = (nsub > 0).float()
|
||||
feat = features(kin, q, qp, T, R, pre, torch.full((B,), r, device=device), subs[:, 0], sf)
|
||||
z = torch.randn(B * K, H, 6, device=device, generator=g) * z_scale
|
||||
act, val, sub = model.sample_k(feat, K, z, steps=steps)
|
||||
inc = act.clamp(-1, 1) * step
|
||||
if oracle is not None:
|
||||
act = oracle.plan(it, K, g); sub = torch.zeros(B, 13, device=device)
|
||||
else:
|
||||
z = torch.randn(B * K, H, 6, device=device, generator=g) * z_scale
|
||||
act, val, sub = model.sample_k(feat, K, z, steps=steps)
|
||||
inc = act.float().clamp(-1, 1) * astep
|
||||
if cold:
|
||||
sub0 = sub
|
||||
need = sub0[:, 12] > 0.5
|
||||
subs = q[:, None] + sub0[:, :12].view(B, 2, 6) * 180
|
||||
if sub_override is not None: subs, need = sub_override[0].to(device), sub_override[1].to(device)
|
||||
if not use_sub: need = torch.zeros_like(need)
|
||||
nsub = torch.where(need, torch.full_like(nsub, 2), torch.zeros_like(nsub)); sub_t.fill_(tnow)
|
||||
need = sub[:, 12] > 0.5
|
||||
subs = q[:, None] + sub[:, :12].float().view(B, 2, 6) * 180
|
||||
if sub_override is not None:
|
||||
subs = sub_override[0].to(device).float()
|
||||
n_ov = sub_override[1].to(device)
|
||||
nsub_ov = n_ov.long() if n_ov.dtype != torch.bool else n_ov.long() * subs.shape[1]
|
||||
need = nsub_ov > 0
|
||||
nsub = nsub_ov
|
||||
else:
|
||||
nsub = torch.where(need, torch.full_like(nsub, 2), torch.zeros_like(nsub))
|
||||
if not use_sub: nsub = torch.zeros_like(nsub)
|
||||
sub_t.fill_(tnow); sub_prev = q.clone()
|
||||
fut = qe[:, None, None] + torch.cumsum(inc[:, :, r:], 2) # [B,K,H-r,6]
|
||||
if select == 'first':
|
||||
score = torch.zeros(B, K, device=device); score[:, 0] = 1
|
||||
elif last is None or select == 'value':
|
||||
elif cold or select == 'value':
|
||||
# 短前瞻：候选末态的价值
|
||||
qa = fut[:, :, -1].reshape(B * K, 6); qb = fut[:, :, -2].reshape(B * K, 6)
|
||||
f2 = features(kin, qa, qb, T.repeat_interleave(K, 0), R.repeat_interleave(K, 0), torch.zeros(B * K, 3, 6, device=device),
|
||||
@@ -124,32 +215,38 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None,
|
||||
score = -v2.view(B, K)
|
||||
else:
|
||||
# 2026-10-19 修错位：last[i] = 上一计划从 qe_old 走 i 步后的位置；提交 3 步后 qe_new = last[3]，
|
||||
# 新候选 fut[0] 是 qe_new 再走 1 步 = 对应 last[4]。原来对齐 last[3]，等于奖励「比上一计划慢一步」的候选，
|
||||
# 每次重规划都挑最拖的那条，闭环越走越慢。JS 部署（ik-policy.js）同一处同样修正。
|
||||
# 新候选 fut[0] 是 qe_new 再走 1 步 = 对应 last[4]。JS 部署（ik-policy.js）同一处同样修正。
|
||||
n = min(fut.shape[2], last.shape[1] - 4)
|
||||
w = 0.85 ** torch.arange(n, device=device, dtype=torch.float32)
|
||||
d2 = (((fut[:, :, :n] - last[:, None, 4:4 + n]) / step) ** 2).sum(-1)
|
||||
score = -(d2 * w).sum(-1)
|
||||
order = score.argsort(-1, descending=True)
|
||||
# 2026-10-19（审查 review_closedloop F5）：上一次是刹停或 DLS 接管的行没有「已提交计划」，
|
||||
# 与 JS（lastPlan = null → 一致性项缺席、各候选同分 → 取第 0 条）一致：同分，稳定排序取第 0 条
|
||||
score = torch.where(last_ok[:, None], -(d2 * w).sum(-1), torch.zeros_like(d2[..., 0]))
|
||||
order = torch.sort(score, dim=-1, descending=True, stable=True).indices
|
||||
pick = order[:, 0]
|
||||
commit = inc[torch.arange(B), pick, r:r + 3]
|
||||
commit = inc[ar, pick, r:r + 3]
|
||||
committed = torch.ones(B, dtype=torch.bool, device=device)
|
||||
if safety:
|
||||
ok_any = torch.zeros(B, dtype=torch.bool, device=device)
|
||||
chosen = torch.zeros(B, 3, 6, device=device)
|
||||
chosen = torch.zeros(B, 3, 6, device=device); chosen_k = pick.clone()
|
||||
for kk in range(K):
|
||||
ck = inc[torch.arange(B), order[:, kk], r:r + 3]
|
||||
ck = inc[ar, order[:, kk], r:r + 3]
|
||||
pts = qe[:, None] + torch.cumsum(ck, 1)
|
||||
mids = torch.cat([qe[:, None], pts[:, :-1]], 1) + 0.5 * ck
|
||||
allq = torch.cat([pts, mids], 1).reshape(-1, 6)
|
||||
okk = (kin.gaps(allq, 0.5)['gm'].min(-1).values > 0).view(B, 6).all(-1)
|
||||
take = okk & ~ok_any
|
||||
chosen = torch.where(take[:, None, None], ck, chosen); ok_any |= okk
|
||||
chosen = torch.where(take[:, None, None], ck, chosen); chosen_k = torch.where(take, order[:, kk], chosen_k); ok_any |= okk
|
||||
commit = torch.where(ok_any[:, None, None], chosen, torch.zeros_like(chosen))
|
||||
brakes += (~ok_any).float()
|
||||
pick = chosen_k; committed = ok_any
|
||||
if near.any():
|
||||
ref = dls_steps(kin, qe, T, R)
|
||||
commit = torch.where(near[:, None, None], ref, commit)
|
||||
last = torch.cat([qe[:, None], fut[torch.arange(B), pick]], 1)
|
||||
dls_it = torch.where(near & (dls_it < 0), torch.full_like(dls_it, it), dls_it)
|
||||
committed = committed & ~near
|
||||
# 2026-10-19（F5）：last 记真正提交的那条候选（安全层换了候选时是换后的那条）；刹停 / DLS 接管的行无效
|
||||
last = torch.cat([qe[:, None], fut[ar, pick]], 1); last_ok = committed
|
||||
queue = torch.cat([queue, commit], 1)
|
||||
cold = False
|
||||
d = queue[:, 0]; queue = queue[:, 1:]
|
||||
@@ -162,15 +259,21 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None,
|
||||
if return_traj: traj.append(q.clone())
|
||||
mid = qp + 0.5 * (q - qp)
|
||||
gm = kin.gaps(torch.cat([q, mid], 0), 0.5)['gm'].min(-1).values
|
||||
hit = ((gm[:B] <= 0) | (gm[B:] <= 0)) & ~done
|
||||
tn = (it + 1) * DT
|
||||
# 2026-10-19（审查 review_closedloop F4）：只计每题自己时限内的碰撞（同批长题会把短题多仿真几秒）
|
||||
hit = ((gm[:B] <= 0) | (gm[B:] <= 0)) & ~done & (tn <= limit + 1e-6)
|
||||
col += hit.float()
|
||||
first_hit = torch.where(hit & torch.isnan(first_hit), torch.full_like(first_hit, tn), first_hit)
|
||||
e, oe = goal_err(kin, q, T, R)
|
||||
if it + 1 == k2s: e2 = e.clone()
|
||||
newly = (~done) & (e < 1) & (oe < 0.5)
|
||||
t_done = torch.where(newly, torch.full_like(t_done, (it + 1) * DT), t_done); done |= newly
|
||||
t_done = torch.where(newly, torch.full_like(t_done, tn), t_done); done |= newly
|
||||
if done.all(): break
|
||||
ok = done & (t_done <= limit)
|
||||
ok = done & (t_done <= limit + 1e-6)
|
||||
if moved_start is None: moved_start = travel.clone()
|
||||
e_end, _ = goal_err(kin, q, T, R)
|
||||
if e2 is None: e2 = e_end
|
||||
ratio2 = e2 / e0.clamp(min=1e-3)
|
||||
start_fail = (moved_start < 1.0) & ~(done & (t_done <= 0.5))
|
||||
okr = ok & has_ref & torch.isfinite(tr)
|
||||
res = {'n': B, 'n_ref': int(has_ref.sum()), 'success': ok.float().mean().item(),
|
||||
@@ -178,20 +281,22 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None,
|
||||
'collision_rate': (col > 0).float().mean().item(), 'collision_steps_mean': col.mean().item(),
|
||||
'travel_ratio_median': (travel[okr] / tr[okr].clamp(min=1)).median().item() if okr.any() else None,
|
||||
'time_mean_s': t_done[ok].mean().item() if ok.any() else None, 'brakes_mean': brakes.mean().item(),
|
||||
'start_fail': start_fail.float().mean().item(),
|
||||
'start_fail': start_fail.float().mean().item(), 'goal_ratio_2s': ratio2.median().item(),
|
||||
'time_budget': 'teacher×2(≥3 s)' if bool(has_ref.all()) else ('fixed %.0f s' % (max_sec or NOREF_SEC) if not has_ref.any() else 'mixed')}
|
||||
# 绕行侧：J1 累计位移的正向/负向极值哪个更大（+1 / −1），供 R7 左右比例门禁
|
||||
# 首次方向；全程没偏离到 FIRST_DEG 的（基本不绕 J1）退回净方向
|
||||
side = torch.where(j1_first != 0, j1_first, torch.where(j1_max >= -j1_min, torch.ones_like(j1_max), -torch.ones_like(j1_max)))
|
||||
per = {'done': done.cpu(), 't_done': t_done.cpu(), 'col': col.cpu(), 'travel': travel.cpu(), 'err': e_end.cpu(),
|
||||
'side': side.cpu(), 'start_fail': start_fail.cpu(), 'brakes': brakes.cpu()}
|
||||
per = {'done': done.cpu(), 'ok': ok.cpu(), 't_done': t_done.cpu(), 'limit': limit.cpu(), 'col': col.cpu(), 'travel': travel.cpu(), 'err': e_end.cpu(),
|
||||
'side': side.cpu(), 'start_fail': start_fail.cpu(), 'brakes': brakes.cpu(), 'first_hit': first_hit.cpu(), 'dls_it': dls_it.cpu(),
|
||||
'goal_ratio_2s': ratio2.cpu()}
|
||||
if return_traj: per['traj'] = torch.stack(traj, 1).cpu()
|
||||
return res, per
|
||||
|
||||
|
||||
def evaluate(model, kin, tasks, device, K=8, safety=False, bs=256, seed=0):
|
||||
if not tasks: # 小样本摸底时 val 可能为空
|
||||
return {'n': 0, 'success': float('nan'), 'collision_rate': float('nan'), 'start_fail': float('nan'), 'brakes_mean': float('nan')}, []
|
||||
return {'n': 0, 'success': float('nan'), 'collision_rate': float('nan'), 'start_fail': float('nan'), 'brakes_mean': float('nan'),
|
||||
'goal_ratio_2s': float('nan')}, []
|
||||
agg, pers = {}, []
|
||||
for i in range(0, len(tasks), bs):
|
||||
r, p = rollout(model, kin, tasks[i:i + bs], device, K=K, safety=safety, seed=seed + i)
|
||||
@@ -202,12 +307,14 @@ def evaluate(model, kin, tasks, device, K=8, safety=False, bs=256, seed=0):
|
||||
n = sum(r['n'] for r, _ in pers)
|
||||
out = {k: v / n for k, v in agg.items()}; out['n'] = n
|
||||
out['n_ref'] = sum(r['n_ref'] for r, _ in pers)
|
||||
# 中位数不能按批加权平均：全体任务重算（2026-10-19，选模分数用它）
|
||||
out['goal_ratio_2s'] = torch.cat([p['goal_ratio_2s'] for _, p in pers]).median().item()
|
||||
out['time_budget'] = pers[0][0]['time_budget'] if len({r['time_budget'] for r, _ in pers}) == 1 else 'mixed'
|
||||
return out, pers
|
||||
|
||||
|
||||
def main():
|
||||
from ml.model.policy import build
|
||||
from ml.common.ckpt import load_policy
|
||||
from ml.data import load_tasks
|
||||
ap = argparse.ArgumentParser()
|
||||
ap.add_argument('--ckpt', required=True); ap.add_argument('--samples', default='ml/data/samples')
|
||||
@@ -216,8 +323,7 @@ def main():
|
||||
ap.add_argument('--safety', action='store_true'); ap.add_argument('--K', type=int, default=8)
|
||||
ap.add_argument('--device', default='cuda' if torch.cuda.is_available() else 'cpu'); ap.add_argument('--out', default=None)
|
||||
a = ap.parse_args()
|
||||
ck = torch.load(a.ckpt, map_location=a.device)
|
||||
model = build(ck['cfg']).to(a.device); model.load_state_dict(ck['model']); model.eval()
|
||||
model, ck = load_policy(a.ckpt, a.device)
|
||||
kin = Kin(device=a.device)
|
||||
assert ck.get('fp') in (None, kin.fp), '碰撞模型指纹与 checkpoint 不符'
|
||||
if a.tasks: tasks = [json.loads(l) for l in open(a.tasks, encoding='utf-8') if l.strip()]
|
||||
@@ -225,8 +331,8 @@ def main():
|
||||
tasks = tasks[:a.n]
|
||||
res, pers = evaluate(model, kin, tasks, a.device, K=a.K, safety=a.safety)
|
||||
by = {}
|
||||
flat_done = torch.cat([p['done'] for _, p in pers]); flat_col = torch.cat([p['col'] for _, p in pers])
|
||||
for t, d, c in zip(tasks, flat_done.tolist(), flat_col.tolist()):
|
||||
flat_ok = torch.cat([p['ok'] for _, p in pers]); flat_col = torch.cat([p['col'] for _, p in pers])
|
||||
for t, d, c in zip(tasks, flat_ok.tolist(), flat_col.tolist()):
|
||||
k = t.get('cat', '?') + '/' + t.get('mode', '?'); b = by.setdefault(k, [0, 0, 0]); b[0] += 1; b[1] += int(d); b[2] += int(c > 0)
|
||||
res['by_cat_mode'] = {k: {'n': v[0], 'success': v[1] / v[0], 'collision': v[2] / v[0]} for k, v in by.items()}
|
||||
s = json.dumps(res, indent=1); print(s)
|
||||
|
||||
Reference in New Issue
Block a user