Robot_Arm/ml/train.py

"""预训练（方案 §6）：MeanFlow 一步头 + FK 损失 + 平滑 + 价值头 + 子目标头。
  python -m ml.train --samples ml/data/samples --out ml/runs/base [--config ml/configs/base.json]
  吞吐摸底：python -m ml.train --samples ml/data/samples --out ml/runs/bench_base --steps 2000 --no-eval
  通过线（方案 R10）：全程 60000 步 ≤ 3.5 h，即 samples_per_s ≥ 5000（3090 实测 6.2 k/s ≈ 2.8 h）；日志 eta_h 为剩余小时数。
每 eval_every 步做一次闭环验证（ml/sim/closed_loop.py，部署节奏），按 success 存 best.pt；连续 patience_evals 次不升即停。
2026-10-18 首次正式预训练（21850 步，闭环到达 0–0.2%、穿柱 77–97%）后改：
  - FK / 平滑损失不再加在 one_step(随机噪声) 上（那样等于对任意噪声回归同一个 Y → 条件均值，多峰头塌成单峰），
    改加在 MF 分支里 r==t 且 t ≤ fk_tmax 的样本的配对去噪估计 x̂ = z − t·u 上（见 ml/model/meanflow.py）；
  - p_neq 前 p_neq_warm 步为 0（纯 flow matching 先把瞬时速度场学稳，再开 r≠t 的自举目标）；
  - 每次评估加开环指标（val 样本上一步采样的 16 步末端 FK 误差 mm、K 个候选的多样性），
    塌缩或学偏当场可见；best / 早停按 success 为主、开环误差为次（success 同为 0 时也能分出好坏，不会 10000 步就误停）。"""
import argparse, json, math, os, sys, time
import torch

from ml.common.kin_torch import Kin
from ml.common.features import STEP, D2R
from ml.common.ckpt import Saver, atomic_save, rng_state, set_rng_state
from ml.common import vram
from ml.data import Samples, load_tasks
from ml.model.policy import build, wta_loss
from ml.model.meanflow import meanflow_loss
from ml.sim.closed_loop import evaluate


def fk_points(kin, q, idx):
    """选定胶囊端点 + TCP 的世界坐标 [B, 2*len(idx)+1, 3]"""
    xf = kin.cum_xf(q)
    a, b = kin.caps_world(q, xf)
    return torch.cat([a[:, idx], b[:, idx], kin.fk(q, xf)[:, None]], 1)


def q_from_feat(X):
    s, c = X[:, 0:12:2], X[:, 1:12:2]
    return torch.atan2(s, c) / D2R


@torch.no_grad()
def open_loop(model, kin, vdata, stepv, fk_idx, n=1024, K=8):
    """开环指标（val 样本，老师状态上）：
    ol_fk_mm  候选 0 的 16 步末端与老师 Y 末端的胶囊点平均距离（mm）；
    ol_best_mm K 个候选里最接近老师的那条的同一误差（多峰时它应明显小于 ol_fk_mm）；
    ol_div    K 个候选间标准差 / 动作平均幅度（塌成单峰时接近 0；首次正式预训练 last.pt 实测 0.20）。"""
    g = torch.Generator(device=vdata.X.device); g.manual_seed(123)
    X, Y, _, _ = vdata.batch(min(n, vdata.n), gen=g)
    act, _, _ = model.sample_k(X, K)
    act = act.float().clamp(-1, 1)
    q = q_from_feat(X)
    qt = q + (Y * stepv).sum(1)
    qh = q[:, None] + (act * stepv).sum(2)                                   # [B,K,6]
    pt = fk_points(kin, qt, fk_idx)[:, None]
    ph = fk_points(kin, qh.reshape(-1, 6), fk_idx).view(X.shape[0], K, -1, 3)
    err = (ph - pt).norm(dim=-1).mean(-1)                                     # [B,K] mm
    div = (act.std(1).mean() / act.abs().mean().clamp(min=1e-6)).item()
    return {'ol_fk_mm': err[:, 0].mean().item(), 'ol_best_mm': err.min(1).values.mean().item(), 'ol_div': div}


def main():
    ap = argparse.ArgumentParser()
    ap.add_argument('--samples', nargs='+', default=['ml/data/samples']); ap.add_argument('--out', default='ml/runs/base')
    ap.add_argument('--config', default='ml/configs/base.json'); ap.add_argument('--steps', type=int, default=None)
    ap.add_argument('--resume', default=None); ap.add_argument('--no-eval', action='store_true')
    ap.add_argument('--init', default=None, help='只载入权重、优化器与步数从 0 开始（DAgger 微调用）')
    ap.add_argument('--lr', type=float, default=None); ap.add_argument('--warmup', type=int, default=None)
    ap.add_argument('--stop-at', type=int, default=0, help='自检用：跑到第 N 步按停止请求处理（存 last.pt、rc 143）')
    ap.add_argument('--device', default='cuda' if torch.cuda.is_available() else 'cpu'); ap.add_argument('--seed', type=int, default=0)
    a = ap.parse_args()
    cfg = json.load(open(a.config)); tc = cfg['train']
    if a.steps: tc['steps'] = a.steps
    if a.lr: tc['lr'] = a.lr
    if a.warmup is not None: tc['warmup'] = a.warmup
    if a.init: tc['p_neq_warm'] = 0          # 微调起点已训过 r≠t，不再预热
    os.makedirs(a.out, exist_ok=True)
    torch.manual_seed(a.seed)
    dev = a.device
    vram.limit()
    if dev.startswith('cuda'):
        torch.backends.cuda.matmul.allow_tf32 = True; torch.backends.cudnn.allow_tf32 = True   # MF 分支改 fp32 后用 TF32 保吞吐
    kin = Kin(device=dev)
    data = Samples(a.samples, device=dev, split='train')
    val_tasks = [t for t in load_tasks(a.samples) if t['split'] == 'val'][:tc['eval_tasks']]
    try: vdata = Samples(a.samples, device=dev, split='val')
    except SystemExit: vdata = None
    if vdata is not None and vdata.n == 0: vdata = None
    print(f'train samples {data.n}, val tasks {len(val_tasks)}, device {dev}', flush=True)
    model = build(cfg).to(dev)
    print(f'params {sum(p.numel() for p in model.parameters()) / 1e6:.2f} M', flush=True)
    opt = torch.optim.AdamW(model.parameters(), lr=tc['lr'], betas=tuple(tc['betas']), weight_decay=tc['wd'])
    step0, best, bad_evals = 0, -1.0, 0
    if a.init and not a.resume:
        ck = torch.load(a.init, map_location=dev); model.load_state_dict(ck['model'])
        print(f'微调：权重取自 {a.init}（第 {ck.get("step")} 步），优化器重置', flush=True)
    if a.resume:
        ck = torch.load(a.resume, map_location=dev); model.load_state_dict(ck['model'])
        if 'opt' in ck: opt.load_state_dict(ck['opt'])
        step0, best, bad_evals = ck.get('step', 0), ck.get('best', -1.0), ck.get('bad_evals', 0)
        if set_rng_state(ck.get('rng')): print('续训：随机数状态已恢复（batch 顺序与不断点一致）', flush=True)
        print(f'续训：从第 {step0} 步继续（共 {tc["steps"]} 步），当前最好 success {best:.4f}', flush=True)
    total = tc['steps']
    lr_at = lambda s: tc['lr'] * (s / tc['warmup'] if s < tc['warmup'] else 0.5 * (1 + math.cos(math.pi * (s - tc['warmup']) / max(1, total - tc['warmup']))))
    amp = dev.startswith('cuda') and tc.get('amp') == 'bf16'
    stepv = torch.tensor(STEP, device=dev)
    fk_idx = torch.linspace(0, kin.cap_a.shape[0] - 1, tc['fk_caps'], device=dev).long()
    log = open(os.path.join(a.out, 'log.jsonl'), 'a' if a.resume else 'w')   # 新训练覆盖旧日志，续训才追加
    if not val_tasks and not a.no_eval:
        print('没有 val 任务（样本太少），本次跳过闭环验证', flush=True); a.no_eval = True
    t0, seen, diverged = time.time(), 0, False
    saver = Saver(tc.get('ckpt_min', 10))            # 2026-10-18：每 10 分钟 + 收到停止请求时存 last.pt（原子写）
    def save(name, step, extra=None):
        d = {'model': model.state_dict(), 'opt': opt.state_dict(), 'cfg': cfg, 'step': step, 'best': best, 'bad_evals': bad_evals, 'fp': kin.fp}
        if name == 'last.pt': d['rng'] = rng_state()
        if extra: d.update(extra)
        atomic_save(d, os.path.join(a.out, name))
        if name == 'last.pt': saver.mark()
    step = step0 - 1                                 # 续训时已训满：循环一次不跑，下面照常收尾
    for step in range(step0, total):
        for gpar in opt.param_groups: gpar['lr'] = lr_at(step)
        X, Y, V, SG = data.batch(tc['batch'])
        model.train()
        with torch.autocast('cuda', dtype=torch.bfloat16, enabled=amp):
            mem = model.encode(X)
            if model.head == 'wta':
                l_mf, xh = wta_loss(model, mem, Y); mf_raw = l_mf.detach(); xh = xh.float()
            else:
                p_neq = tc['p_neq'] if step >= tc.get('p_neq_warm', 5000) else 0.0
                l_mf, mf_raw, aux = meanflow_loss(model, mem, Y, p_neq=p_neq, return_aux=True)
                sel = ((aux['r'] == aux['t']) & (aux['t'] <= tc.get('fk_tmax', 0.3))).nonzero(as_tuple=True)[0]
                tt = aux['t'][sel][:, None, None]
                xh = aux['z'][sel] - tt * aux['u'][sel]          # 配对去噪估计（fp32）
            v, s = model.heads(mem)
        l_val = (v.float() - V).abs().mean(); l_sub = (s.float() - SG).abs().mean()
        Xf, Yf = (X, Y) if model.head == 'wta' else (X[sel], Y[sel])
        l_sm = torch.zeros((), device=dev); l_fk = torch.zeros((), device=dev)
        if xh.shape[0] > 0:
            d2 = xh[:, 2:] - 2 * xh[:, 1:-1] + xh[:, :-2]
            l_sm = d2.pow(2).mean()
        if tc['w_fk'] > 0 and xh.shape[0] > 0:
            q = q_from_feat(Xf)
            ks = torch.tensor([3, 7, 11, 15], device=dev)
            qh = q[:, None] + torch.cumsum(xh.clamp(-1, 1) * stepv, 1)[:, ks]
            qt = q[:, None] + torch.cumsum(Yf * stepv, 1)[:, ks]
            B4 = qh.shape[0] * 4
            ph = fk_points(kin, qh.reshape(B4, 6), fk_idx); pt = fk_points(kin, qt.reshape(B4, 6), fk_idx)
            l_fk = ((ph - pt).norm(dim=-1) / 10).mean()
        loss = tc['w_mf'] * l_mf + tc['w_fk'] * l_fk + tc['w_smooth'] * l_sm + tc['w_value'] * l_val + tc['w_sub'] * l_sub
        opt.zero_grad(set_to_none=True)
        loss.backward()
        gn = torch.nn.utils.clip_grad_norm_(model.parameters(), tc['grad_clip'])
        if not torch.isfinite(loss):
            print('非有限损失 → 训练发散（M5 门禁：触发 WTA 回退条件）', flush=True); save('diverged.pt', step); diverged = True; break
        opt.step(); seen += X.shape[0]
        if saver.stop() or (a.stop_at and step + 1 >= a.stop_at):
            save('last.pt', step + 1); log.close()
            print(f'收到停止请求：已存档 last.pt（第 {step + 1} 步），下次运行自动续训', flush=True); sys.exit(143)
        if saver.due(): save('last.pt', step + 1)
        if step % tc['log_every'] == 0:
            el = time.time() - t0
            rec = {'step': step, 'total': total, 't': time.time(), 'loss': loss.item(), 'mf': mf_raw.item(), 'fk_cm': l_fk.item(), 'smooth': l_sm.item(),
                   'value': l_val.item(), 'sub': l_sub.item(), 'gnorm': gn.item(), 'lr': lr_at(step), 'samples_per_s': seen / max(el, 1e-6),
                   'eta_h': (total - step - 1) * tc['batch'] / max(seen / max(el, 1e-6), 1e-6) / 3600, 'vram': vram.peak_gib()}
            print(json.dumps(rec), flush=True); log.write(json.dumps(rec) + '\n'); log.flush()
        if not a.no_eval and (step + 1) % tc['eval_every'] == 0:
            model.eval()
            # bs=64 + 归还缓存池：WDDM 卡满时 15.9 GiB 的评估峰值会被驱动换出到内存，eval 从 2 分钟变 1 小时
            res, _ = evaluate(model, kin, val_tasks, dev, bs=64)
            if vdata is not None: res.update(open_loop(model, kin, vdata, stepv, fk_idx))
            torch.cuda.empty_cache()
            res['step'] = step + 1; res['t'] = time.time()
            print('EVAL', json.dumps(res), flush=True); log.write(json.dumps({'eval': res}) + '\n'); log.flush()
            save('last.pt', step + 1)
            sc = res['success'] - 1e-3 * min(res.get('ol_fk_mm', 0.0), 500.0)    # 到达率为主，开环误差（mm）只在到达率打平时起作用
            if sc > best + 1e-5:
                best = sc; bad_evals = 0; save('best.pt', step + 1, {'eval': res})
            else:
                bad_evals += 1
                if bad_evals >= tc['patience_evals']: print('闭环到达率不再上升，停止', flush=True); break
    if diverged:      # 发散的权重不能覆盖 last.pt / best.pt：last.pt 保留发散前最后一次存档，M5 门禁会因此判不过 → 自动 WTA
        print('训练发散，last.pt / best.pt 保留发散前的存档', flush=True); return
    save('last.pt', step + 1)
    if a.no_eval: save('best.pt', step + 1)
    print(f'done, best score（到达率 − 0.001×开环 mm）{best:.4f}, {(time.time() - t0) / 3600:.2f} h', flush=True)


if __name__ == '__main__':
    main()