Robot_Arm/ml/sim/closed_loop.py

"""闭环仿真（验证 / 挖错 / 门禁共用，PyTorch 批量，无梯度）——按部署节奏运行：
50 Hz 执行；队列 ≤ 3 步时推理一次（已提交前缀 r = 队列长度），提交接下来 3 步；
冷启动用价值头对候选末态打分（短前瞻），之后按与上一条**真正提交**的计划的一致性选候选；
接近目标 < 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, 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)
    T = torch.tensor([t['T'] for t in tasks], dtype=torch.float32, device=device)
    R = torch.tensor([t['R'] if t.get('R') else [0.] * 9 for t in tasks], dtype=torch.float32, device=device)
    # 无老师参考（挖错/门禁题）时 exec_s/travel 为 None → NaN；调用方据此区分口径
    ex = torch.tensor([t['exec_s'] if t.get('exec_s') else float('nan') for t in tasks], dtype=torch.float32, device=device)
    tr = torch.tensor([t['travel'] if t.get('travel') else float('nan') for t in tasks], dtype=torch.float32, device=device)
    return q0, T, R, ex, tr


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)
    e = (T - p).norm(dim=-1)
    pose = R.abs().sum(-1) > 0
    tr = (Rq * R).sum(-1)
    oe = torch.where(pose, torch.acos(((tr - 1) / 2).clamp(-1, 1)) / D2R, torch.zeros_like(e))
    return e, oe


def execution_points(kin, q, increments):
    points, midpoints = [], []
    for delta in increments.unbind(1):
        nxt = kin.clamp_box(q + delta)
        midpoints.append(q + 0.5 * (nxt - q))
        points.append(nxt)
        q = nxt
    return torch.stack(points, 1), torch.stack(midpoints, 1)


def dls_steps(kin, q, T, R, n=3, scale=0.3, lam=0.5, analytic=True):
    """局部 DLS 精修：n 步增量 [B,n,6]（analytic：几何雅可比，1 次 FK；否则 7 次批量有限差分）"""
    step = torch.tensor(STEP, device=q.device)
    B = q.shape[0]
    pose = (R.abs().sum(-1) > 0).float()
    Rt = R.view(-1, 3, 3)
    def err(x, Tb, Rtb, poseb, xf=None):
        xf = xf if xf is not None else kin.cum_xf(x)
        p = kin.fk(x, xf); Rq = kin.orient(x, xf)
        M = Rtb @ Rq.transpose(1, 2)
        rv = 0.5 * torch.stack([M[:, 2, 1] - M[:, 1, 2], M[:, 0, 2] - M[:, 2, 0], M[:, 1, 0] - M[:, 0, 1]], -1) * 100 * poseb[:, None]
        return torch.cat([Tb - p, rv], -1)
    out = []; x = q.clone()
    ar = torch.arange(6, device=q.device)
    for _ in range(n):
        xf = kin.cum_xf(x)
        e0 = err(x, T, Rt, pose, xf)
        if analytic:
            # 几何角速度 Jw 仅在姿态误差接近零时等于该 skew 残差的负导数。
            Jv, Jw = kin.jac(x, xf)
            J = torch.cat([Jv, 100 * Jw * pose[:, None, None]], 1)
        else:
            dx = torch.zeros(B, 6, 6, device=q.device); dx[:, ar, ar] = 1e-3
            xp = (x[:, None, :] + dx).reshape(B * 6, 6)
            ep = err(xp, T.repeat_interleave(6, 0), Rt.repeat_interleave(6, 0), pose.repeat_interleave(6, 0))
            J = -((ep - e0.repeat_interleave(6, 0)) / 1e-3).view(B, 6, 6).transpose(1, 2)   # [B,6(err),6(q)]
        A = J @ J.transpose(1, 2) + lam ** 2 * torch.eye(6, device=q.device)
        y = torch.linalg.solve(A, e0.unsqueeze(-1))
        dq = (J.transpose(1, 2) @ y).squeeze(-1)
        sc = (dq.abs() / (step * scale)).max(-1, keepdim=True).values.clamp(min=1)
        dq = dq / sc
        out.append(dq); x = x + dq
    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', oracle=None, replan='r3', sub_rule='cross',
            fb_gain=0.0, fb_scale=0.0, fb_near=0.2, fb_fallback=0.0, safety_partial=0, commit_n=3, collect=None,
            fb_exec=0.0, exec_safety=0, fb_ki=0.0, fb_decay=0.9, fb_imax=0.5, fb_every=1, safety_try=0, sel_w=0.1, dls_safety=0, fb_clear=0.0, fb_cmin=0.3, sub_refresh=0, fb_block_gain=1.0,
            inc_cap=0, gap_margin=0.0, cert_batch=0, backoff=0, len_w=0.0, progress=None):
    """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。除默认值外均为诊断用。
    inc_cap=1：把「策略动作 + 目标反馈」的合计增量再按每轴 VMAX·DT 限一次幅（默认关；关时反馈是在 ±1 限幅之后叠加的，第30轮实测最大 1.66×VMAX）。
    gap_margin>0：安全层要求间隙大于该毫米余量而不是 >0；宽相位 cap 自动抬到 margin+0.5，否则 0.5 的截顶值会让余量门限恒假。
    backoff>0：全拒（连第 1 步都不安全）时不再站住，把首选候选按 1/2、1/4…（共 backoff 档）缩步一起送进同一次认证，
               取能通过的第 1 档执行；缩步候选不参与"前缀更长者优先"的比较，只在原候选全被拒时顶上。需 cert_batch=1。
    select='ep_len' + len_w>0：在末态误差打分上再减 len_w·Σ|Δ|/STEP（每轴归一的候选累计行程）。§9.2 要行程≤规划器×1.10，
               而实测行程比 2.4~2.6 没有任何机制在管；len_w=0 与 select='endpoint' 逐位相同（有回归断言）。
    两者默认值下行为与第32轮逐位相同。"""
    if backoff and not cert_batch: raise ValueError('backoff 目前只实现在 cert_batch=1 口径')
    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)
    gcap = max(0.5, gap_margin + 0.5)               # 余量门限必须落在截顶值之内才有意义
    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 = 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; last_ok = torch.zeros(B, dtype=torch.bool, device=device)
    fb_i = torch.zeros(B, 6, device=device)   # 积分项：累积持续同向的目标反馈（慢变偏差）
    fb_cache = None; pit = 0                  # fb_every>1：复用上一次规划的目标反馈（误差方向慢变）
    last_brake = 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)
    bo = torch.zeros(B, device=device)   # 缩步顶上的次数（backoff>0 才会计）
    first_hit = torch.full((B,), float('nan'), device=device); dls_it = torch.full((B,), -1, dtype=torch.long, device=device)
    # 2026-10-20（轮次33）：速度/加速度按「实际执行位移」算，不看指令——第30轮证明反馈是在限幅之后叠加的
    vratio = torch.zeros(B, device=device); aratio = torch.zeros(B, device=device); prev_exec = torch.zeros(B, 6, device=device)
    rev = torch.zeros(B, device=device); nact = torch.zeros(B, device=device); prev_sgn = torch.zeros(B, 6, device=device)
    traj = [q.clone()] if return_traj else None
    plans = {k: [] for k in ('it', 'r', 'qe', 'commit', 'points', 'midpoints', 'gaps', 'certified')} 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 replan == 'r3' else (queue.shape[1] == 0):
            r = queue.shape[1]
            pre = torch.zeros(B, 3, 6, device=device)
            qe = q
            if r:
                prefix_points, _ = execution_points(kin, q, queue)
                pre[:, :r] = prefix_points - torch.cat([q[:, None], prefix_points[:, :-1]], 1)
                qe = prefix_points[:, -1]
            e, oe = goal_err(kin, qe, T, R)
            near = (e < 5) & ((R.abs().sum(-1) == 0) | (oe < 5))
            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)
            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)
            if sub_refresh and not cold:   # 子目标头作为反应式规划：没有待走途经点时，用当前状态的预测补一个（训练标签在每个状态都有 sub）
                need = sub[:, 12] > 0.5
                newsubs = q[:, None] + sub[:, :12].float().view(B, 2, 6) * 180
                rf = need & (nsub <= 0)
                subs = torch.where(rf[:, None, None], newsubs, subs)
                nsub = torch.where(rf, torch.full_like(nsub, 2), nsub)
                sub_prev = torch.where(rf[:, None], q, sub_prev)
                sub_t = torch.where(rf, torch.full_like(sub_t, tnow), sub_t)
            inc = act.float().clamp(-1, 1) * astep
            if fb_gain > 0:   # 实验（2026-10-20）：目标反馈加在所有候选上（安全层也评估带反馈的候选）
                if fb_cache is None or fb_every <= 1 or pit % fb_every == 0:
                    fb = dls_steps(kin, qe, T, R, n=1); fb_cache = fb
                else:
                    fb = fb_cache
                pit += 1
                gm_ = None
                if fb_clear > 0:   # 近障碍衰减：反馈方向可能把候选推进障碍，贴近时让模型自己的绕行占主导
                    dgc = kin.gaps(qe, fb_clear)['gm'].min(-1).values
                    gm_ = torch.clamp(dgc / fb_clear, fb_cmin, 1.0)
                if fb_ki > 0:
                    fb_i = fb_decay * fb_i + fb[:, 0] * (~done)[:, None]
                    lim = step * fb_imax
                    fb_i = fb_i.clamp(-lim, lim)
                    inc = inc + fb_gain * fb[:, :, None, :] + fb_ki * fb_i[:, None, None, :]
                elif fb_scale > 0:   # 距离衰减：远离目标全增益，贴近目标减弱（减少过冲/刹停）
                    dg, _ = goal_err(kin, qe, T, R)
                    gdec = fb_gain * torch.clamp(dg / fb_scale, fb_near, 1.0)
                    if gm_ is not None: gdec = gdec * gm_
                    inc = inc + gdec[:, None, None, None] * fb[:, :, None, :]
                else:
                    gv = torch.full((B,), fb_gain, device=device, dtype=fb.dtype)
                    if gm_ is not None: gv = gv * gm_
                    if fb_block_gain != 1.0:   # 上一次被安全层全拒：减弱目标反馈，让模型自己的绕行候选有机会通过认证
                        gv = torch.where(last_brake, gv * fb_block_gain, gv)
                    inc = inc + gv[:, None, None, None] * fb[:, :, None, :]
            if inc_cap:
                inc = torch.maximum(torch.minimum(inc, step), -step)   # 反馈之后才限幅：限的是真正要执行的每轴增量
            if cold:
                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 select in ('endpoint', 'adapt', 'ep_len', 'ep_waste'):
                qa = fut[:, :, -1].reshape(B * K, 6)
                e_end, _ = goal_err(kin, qa, T.repeat_interleave(K, 0), R.repeat_interleave(K, 0))
                score = -e_end.view(B, K)
                if select == 'ep_len' and len_w > 0:   # 行程惩罚：每轴按 STEP 归一的累计路程。§9.2 要“行程≤规划器×1.10”，
                    ln = ((fut[:, :, 1:] - fut[:, :, :-1]).abs() / step).sum(-1).sum(-1)   # 而现在 2.4 倍没人管
                    score = score - len_w * ln
                if select == 'ep_waste' and len_w > 0:   # 轮次 39 的分解证明行程超出主要是同一轴来回抖（换向步占 23.5%、
                    dv = fut[:, :, 1:] - fut[:, :, :-1]   # 净位移只占总行程 44.5%），绕远只占一小部分 ⇒ 罚"白费的比例"：
                    tot = dv.abs().sum(-2).sum(-1)        # 同轴来回会在净位移里相互抵消，所以 waste = 1 − 净/总 ∈ [0,1]。
                    net = dv.sum(-2).abs().sum(-1)       # dv 是 [B,K,H-1,6]：时间在第 -2 轴（写错成 -1 就变成"对轴求和"，等于总行程）
                    frac = (1.0 - net / tot.clamp_min(1e-6)).clamp(0, 1)
                    score = score - len_w * frac * e.view(B, 1)   # 罚度按"当前离目标多远"缩放：近端不会把进度项压死
                if select == 'adapt' and last is not None:
                    n = min(fut.shape[2], last.shape[1] - (commit_n + 1))
                    w = 0.85 ** torch.arange(n, device=device, dtype=torch.float32)
                    d2 = (((fut[:, :, :n] - last[:, None, commit_n + 1:commit_n + 1 + n]) / step) ** 2).sum(-1)
                    sc = torch.where(last_ok[:, None], -(d2 * w).sum(-1), torch.zeros_like(d2[..., 0]))
                    score = torch.where(last_brake[:, None], sc, score)
            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),
                              torch.zeros(B * K, device=device), qa, torch.zeros(B * K, device=device))
                v2, _ = model.heads(model.encode(f2))
                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]。JS 部署（ik-policy.js）同一处同样修正。
                n = min(fut.shape[2], last.shape[1] - (commit_n + 1))
                w = 0.85 ** torch.arange(n, device=device, dtype=torch.float32)
                d2 = (((fut[:, :, :n] - last[:, None, commit_n + 1:commit_n + 1 + n]) / step) ** 2).sum(-1)
                # 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]))
                if select == 'mix':   # 一致性 + 端点进度（w=sel_w/mm）
                    qa = fut[:, :, -1].reshape(B * K, 6)
                    e_end, _ = goal_err(kin, qa, T.repeat_interleave(K, 0), R.repeat_interleave(K, 0))
                    score = score - sel_w * e_end.view(B, K)
            order = torch.sort(score, dim=-1, descending=True, stable=True).indices
            pick = order[:, 0]
            if select == 'average':   # 2026-10-20：候选平均（平滑误差）——所有候选置为 K 条动作的均值
                inc = inc.mean(1, keepdim=True).expand_as(inc).contiguous()
                K_eff = 1
            else:
                K_eff = K
            commit = inc[ar, pick, r:r + commit_n]
            if collect is not None:   # 反馈蒸馏：收集与部署完全一致的 (特征, 复合动作=模型输出+目标反馈)
                collect.append((feat.detach().float().cpu(), inc[ar, pick].detach().float().cpu()))
            committed = torch.ones(B, dtype=torch.bool, device=device)
            braked = torch.zeros(B, dtype=torch.bool, device=device)
            if return_traj:
                cert_pts = torch.full((B, commit_n, 6), float('nan'), device=device)
                cert_mids = cert_pts.clone()
                cert_gaps = torch.full((B, 2 * commit_n), float('nan'), device=device)
                certified = torch.zeros(B, commit_n, dtype=torch.bool, device=device)
            if safety and safety_partial:   # 2026-10-20：允许「部分安全前缀」——执行到第一个不安全步为止，而不是全拒刹停
                maxlen = torch.zeros(B, dtype=torch.long, device=device)
                chosen = torch.zeros(B, commit_n, 6, device=device); chosen_k = pick.clone()
                ntry = min(K_eff, safety_try) if safety_try > 0 else K_eff
                if cert_batch:   # 一次 gaps 覆盖全部候选：典型路径略贵，但把「前面候选被拒→串行调用 K 次」的最坏情况封成 1 次
                    cks = inc[ar[:, None], order[:, :ntry], r:r + commit_n]        # [B,ntry,cn,6]
                    if backoff:   # 缩步档与正常候选并进同一次认证，不额外调用几何
                        sc = (0.5 ** torch.arange(1, backoff + 1, device=device, dtype=inc.dtype))[None, :, None, None]
                        cands = torch.cat([cks, inc[ar, order[:, 0], r:r + commit_n][:, None] * sc], 1)
                    else:
                        cands = cks
                    nc = cands.shape[1]
                    pts, mids = execution_points(kin, qe.repeat_interleave(nc, 0), cands.reshape(B * nc, commit_n, 6))
                    gaps6 = kin.gaps(torch.cat([pts, mids], 1).reshape(-1, 6), gcap)['gm'].min(-1).values.view(B, nc, 2 * commit_n)
                    stepok = (gaps6 > gap_margin)[:, :, :commit_n] & (gaps6 > gap_margin)[:, :, commit_n:]
                    pl_all = torch.cumprod(stepok.long(), -1).sum(-1)       # [B,nc] 每条候选的安全前缀长度
                    plen = pl_all[:, :ntry]
                    maxlen = plen.max(-1).values
                    best = plen.argmax(-1)                # 与串行同口径：严格更大者胜 ⇒ 首个最大值
                    if backoff:   # 缩步候选只在全拒时顶上：不参与「前缀更长者优先」的比较，所以正常路径逐位不变
                        full0 = maxlen == 0
                        bb = pl_all[:, ntry:].argmax(-1)   # 平手取缩得最少的（档 0 = 1/2）
                        mv = pl_all[ar, ntry + bb]
                        bo += (full0 & (mv > 0))
                        best = torch.where(full0, ntry + bb, best)
                        maxlen = torch.where(full0, mv, maxlen)
                        chosen_k = torch.where(full0, order[:, 0], order[ar, best.clamp(max=ntry - 1)])
                    else:
                        chosen_k = order[ar, best]
                    chosen = cands[ar, best]
                    if return_traj:
                        cert_pts = pts.view(B, nc, commit_n, 6)[ar, best]
                        cert_mids = mids.view(B, nc, commit_n, 6)[ar, best]
                        cert_gaps = gaps6[ar, best]
                else:
                    for kk in range(ntry):
                        ck = inc[ar, order[:, kk], r:r + commit_n]
                        pts, mids = execution_points(kin, qe, ck)
                        allq = torch.cat([pts, mids], 1).reshape(-1, 6)
                        gaps6 = kin.gaps(allq, gcap)['gm'].min(-1).values.view(B, 2 * commit_n)
                        ok6 = gaps6 > gap_margin
                        okstep = ok6[:, :commit_n] & ok6[:, commit_n:]
                        plen = torch.cumprod(okstep.long(), 1).sum(1)
                        take = plen > maxlen
                        chosen = torch.where(take[:, None, None], ck, chosen); chosen_k = torch.where(take, order[:, kk], chosen_k)
                        if return_traj:
                            cert_pts = torch.where(take[:, None, None], pts, cert_pts)
                            cert_mids = torch.where(take[:, None, None], mids, cert_mids)
                            cert_gaps = torch.where(take[:, None], gaps6, cert_gaps)
                        maxlen = torch.maximum(maxlen, plen)
                        if B == 1 and int(maxlen[0]) >= commit_n: break   # 实时单题：首个全安全候选即停（安全层 16 点→2-4 点）
                msk = (torch.arange(commit_n, device=device)[None] < maxlen[:, None])
                commit = torch.where(msk[..., None], chosen, torch.zeros_like(chosen))
                if return_traj: certified = msk.clone()
                brakes += (maxlen == 0).float(); braked = maxlen == 0
                pick = chosen_k; committed = maxlen > 0
            elif safety:
                ok_any = torch.zeros(B, dtype=torch.bool, device=device)
                chosen = torch.zeros(B, commit_n, 6, device=device); chosen_k = pick.clone()
                ntry = min(K_eff, safety_try) if safety_try > 0 else K_eff
                for kk in range(ntry):
                    ck = inc[ar, order[:, kk], r:r + commit_n]
                    pts, mids = execution_points(kin, qe, ck)
                    allq = torch.cat([pts, mids], 1).reshape(-1, 6)
                    gaps6 = kin.gaps(allq, gcap)['gm'].min(-1).values.view(B, 2 * commit_n)
                    okk = (gaps6 > gap_margin).all(-1)
                    take = okk & ~ok_any
                    chosen = torch.where(take[:, None, None], ck, chosen); chosen_k = torch.where(take, order[:, kk], chosen_k); ok_any |= okk
                    if return_traj:
                        cert_pts = torch.where(take[:, None, None], pts, cert_pts)
                        cert_mids = torch.where(take[:, None, None], mids, cert_mids)
                        cert_gaps = torch.where(take[:, None], gaps6, cert_gaps)
                commit = torch.where(ok_any[:, None, None], chosen, torch.zeros_like(chosen))
                if return_traj: certified = ok_any[:, None].expand(-1, commit_n).clone()
                brakes += (~ok_any).float(); braked = ~ok_any
                pick = chosen_k; committed = ok_any
            if safety and fb_gain > 0 and fb_fallback > 0:
                fbcom = torch.zeros(B, commit_n, 6, device=device)
                fbcom[:, :1] = dls_steps(kin, qe, T, R, n=1) * fb_fallback
                pts, mids = execution_points(kin, qe, fbcom)
                allq = torch.cat([pts, mids], 1).reshape(-1, 6)
                gaps6 = kin.gaps(allq, gcap)['gm'].min(-1).values.view(B, 2 * commit_n)
                takef = braked & (gaps6 > gap_margin).all(-1)
                commit = torch.where(takef[:, None, None], fbcom, commit)
                if return_traj:
                    cert_pts = torch.where(takef[:, None, None], pts, cert_pts)
                    cert_mids = torch.where(takef[:, None, None], mids, cert_mids)
                    cert_gaps = torch.where(takef[:, None], gaps6, cert_gaps)
                    certified = certified | takef[:, None]
            if near.any():
                ref = dls_steps(kin, qe, T, R, n=commit_n)
                take = near
                if dls_safety and safety:   # 2026-10-20：近目标 DLS 接管也要过安全层（部署端 _certCommit 本来就会验；不过则退回候选计划）
                    pts, mids = execution_points(kin, qe, ref)
                    allq = torch.cat([pts, mids], 1).reshape(-1, 6)
                    gaps6 = kin.gaps(allq, gcap)['gm'].min(-1).values.view(B, 2 * commit_n)
                    okd = (gaps6 > gap_margin).all(-1)
                    take = near & okd
                    if return_traj:
                        cert_pts = torch.where(take[:, None, None], pts, cert_pts)
                        cert_mids = torch.where(take[:, None, None], mids, cert_mids)
                        cert_gaps = torch.where(take[:, None], gaps6, cert_gaps)
                if return_traj:
                    certified = torch.where(take[:, None], torch.full_like(certified, bool(dls_safety and safety)), certified)
                commit = torch.where(take[:, None, None], ref, commit)
                dls_it = torch.where(take & (dls_it < 0), torch.full_like(dls_it, it), dls_it)
                committed = committed & ~take
            # 2026-10-19（F5）：last 记真正提交的那条候选（安全层换了候选时是换后的那条）；刹停 / DLS 接管的行无效
            if fb_ki > 0:   # 刹停/精修接管时清积分（防止原地累积到限幅后突然冲出去）
                fb_i = torch.where(committed[:, None], fb_i, torch.zeros_like(fb_i))
            last = torch.cat([qe[:, None], fut[ar, pick]], 1); last_ok = committed; last_brake = braked
            if return_traj:
                plans['it'].append(it + r)
                plans['r'].append(r)
                for key, value in (('qe', qe), ('commit', commit), ('points', cert_pts), ('midpoints', cert_mids), ('gaps', cert_gaps), ('certified', certified)):
                    plans[key].append(value)
            queue = torch.cat([queue, commit], 1)
            cold = False
        d = queue[:, 0]; queue = queue[:, 1:]
        d = torch.where(done[:, None], torch.zeros_like(d), d)
        qp = q; q = kin.clamp_box(q + d)
        travel += d.abs().sum(-1)
        j1_acc = j1_acc + d[:, 0]; j1_max = torch.maximum(j1_max, j1_acc); j1_min = torch.minimum(j1_min, j1_acc)
        j1_first = torch.where((j1_first == 0) & (j1_acc.abs() >= FIRST_DEG), torch.sign(j1_acc), j1_first)
        if it + 1 == n_start: moved_start = travel.clone()
        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
        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)
        act_ = ((~done) & (tn <= limit + 1e-6)).float()          # 只计自己时限内的移动（与 col 同口径）
        exec_d = (q - qp).abs()
        vratio = torch.maximum(vratio, (exec_d / step).max(-1).values * act_)       # 实际速度 / VMAX
        aratio = torch.maximum(aratio, ((exec_d - prev_exec) / step).max(-1).values * act_)   # 每控制步的速度变化 / VMAX
        prev_exec = exec_d
        sgn = torch.sign(q - qp)      # 换向率：轮次 39 的分解证明"行程超出 2.4 倍"主要是同一轴来回抖，不是绕远
        rev += ((sgn != 0) & (prev_sgn != 0) & (sgn == -prev_sgn)).float().sum(-1) * act_
        prev_sgn = torch.where(sgn != 0, sgn, prev_sgn)      # 零步不重置方向（站住再走同向不算换向）
        nact += act_
        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, tn), t_done); done |= newly
        if progress is not None and (it == 0 or (it + 1) % 50 == 0 or it + 1 == nmax):
            progress(it + 1, nmax)
        if done.all(): break
    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(),
           'time_ok_1p3': (ok & has_ref & (t_done <= 1.3 * ex)).float().sum().item() / max(1, int(has_ref.sum())) if has_ref.any() else 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(),
           'backoff_mean': bo.mean().item(),
           'start_fail': start_fail.float().mean().item(), 'goal_ratio_2s': ratio2.median().item(),
           'speed_ratio_mean': vratio.mean().item(), 'over_vmax_rate': (vratio > 1.001).float().mean().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(), '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(), 'backoff': bo.cpu(), 'first_hit': first_hit.cpu(), 'dls_it': dls_it.cpu(),
           'goal_ratio_2s': ratio2.cpu(), 'speed_ratio': vratio.cpu(), 'accel_ratio': aratio.cpu(),
           # 口径与离线分解一致：换向的 (步,轴) 对数 / 活动的 (步,轴) 对数 ∈ [0,1]（除以 6 个轴）
           'reverse_rate': (rev / (6.0 * nact.clamp_min(1.0))).cpu()}
    if return_traj:
        per['traj'] = torch.stack(traj, 1).cpu()
        per['plans'] = {k: torch.tensor(v) if k in ('it', 'r') else torch.stack(v, 1).cpu() for k, v in plans.items()}
    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'),
                '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)
        w = r['n']; pers.append((r, p))
        for k, v in r.items():
            if k == 'n' or v is None or isinstance(v, str): continue
            agg[k] = agg.get(k, 0.0) + v * w
    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.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')
    ap.add_argument('--tasks', default=None, help='任务 jsonl（覆盖 --samples/--split）')
    ap.add_argument('--split', default='val'); ap.add_argument('--n', type=int, default=1000)
    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()
    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()]
    else: tasks = [t for t in load_tasks([a.samples]) if t['split'] == a.split]
    tasks = tasks[:a.n]
    res, pers = evaluate(model, kin, tasks, a.device, K=a.K, safety=a.safety)
    by = {}
    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)
    if a.out:                                    # 先写 .tmp 再改名：中断不会留下半截结果被当成已完成（M5 续跑按文件存在跳过）
        open(a.out + '.tmp', 'w').write(s); os.replace(a.out + '.tmp', a.out)


if __name__ == '__main__':
    main()