diff --git a/docs/探索日志.md b/docs/探索日志.md index ce39a32..937dc50 100644 --- a/docs/探索日志.md +++ b/docs/探索日志.md @@ -419,6 +419,34 @@ - **结论**：候选平均、冻结编码器、更稀疏规划**均未超过 36.3%**。**50 Hz 节奏下的最佳配置 = dag2 + commit=3 + consist + gain1.8 + safety_partial：到达 36.3% / 碰撞 1.6% / ratio2s 0.277**（相对非实时的 42.2% 差 5.9 pp）。 - **下一步**：若要把实时准确率再拉高，需要把单次规划再压到 ~22 ms 以支持 commit=2（40 ms 周期，预计 ~39%）；或提升模型本身精度（训练侧）。 +## 轮次 26：解析雅可比 DLS——去掉差分噪声，准确率大幅提升（exp: `exp/dagger-deploy`，新历史最佳） + +- **背景**：DLS 目标反馈此前用 6 次批量有限差分（ε=1e-3°）求雅可比。float32 下旋转项信噪比只有 ~1%，位置项 ~3.6%（与 autograd 对拍，见下），相当于每次规划往反馈方向注入 2–4% 的随机扰动。 +- **假设**：改用几何雅可比（一次 FK）可同时去噪 + 提速；闭环准确率应不降、延迟应下降。 +- **判据**：① 雅可比与 autograd 一致（rel < 1e-4）；② n=256 commit=3 到达 ≥ 36.3%（轮次 25 基线，不劣化）；③ DLS 单次延迟下降。 +- **配方**：`Kin.jac` 几何雅可比（Jv_k = a_k×(p−o_k)，Jw_k = a_k，度单位）；`dls_steps(analytic=True)` 默认；其余同轮次 25（dag2、K=8、gain1.8、safety_partial、replan=empty）。 +- **数值对拍**（`ml/eval/check_dls.py`）： + - 位置项：解析 J 与 autograd 逐项一致（rel 0.00000）；有限差分 J 自身 rel 误差 **3.58%**。 + - 姿态项：解析 J 与差分 DLS 输出 max 0.20°/rel 5%（差的部分是 rotvec 的二阶项 Jr(e)，非噪声）。 + - DLS 输出：pos-only max 0.12°，pos+rot max 0.20°。 +- **延迟**（B=1，3090）：n=1 **9.3→7.1 ms**，n=3 **27.7→19.5 ms**（每次 DLS 少 6 次批量 FK）。 +- **闭环（replan=empty，analytic，n=256）**： + +| commit | 到达 | time_ok_1p3 | 碰撞 | ratio2s | 刹停/题 | +|---|---|---|---|---|---| +| 3（FD 对照，完全复现轮次 25） | 36.3% | 16.4% | 1.6% | 0.277 | 14.3 | +| 3（解析 J） | 46.5% | 16.4% | 1.6% | 0.337 | 13.5 | +| 2（解析 J） | **50.4%** | 27.0% | 2.0% | 0.287 | 22.1 | +| 1（解析 J） | **57.4%** | **31.6%** | 3.1% | 0.251 | 39.1 | + +- **n=512 复核（commit=2，解析 J）**：到达 **49.2%** / time_ok **26.2%** / 碰撞 **2.3%** / ratio2s 0.351 / 刹停 21.0（n=256 的 50.4% 在噪声内）。 +- **结论**： + - 解析雅可比**既快又准**：commit=3 到达 36.3→46.5%（+10.2 pp），碰撞不变；commit=2 达 **49.2%**（历史最佳 42.2% 是 commit=1 + FD，现在 commit=2 就超过），碰撞 2.3%。 + - 机理：DLS 差分雅可比在 float32 下的 2–4% 噪声被慢变误差闭环放大（轮次 4/闭环根因结论：σ>0.02×STEP 即崩）；解析 J 去掉噪声后失败题里"能到 50 mm 却到不了"的那批能收敛了（err_fail 中位数 51→217 mm：失败集两极分化，成功集变大）。 + - ratio2s 在 commit=3 变差（0.277→0.337）但到达更多：慢收敛题现在"最终到得了"，不是"更快"。 + - **新历史最佳（50 Hz 目标节奏，commit=2）= 49.2% / 碰撞 2.3% / ratio2s 0.351 / time_ok 26.2%**；非实时上限（commit=1）= 57.4%（n=256，待 n=512 复核）。 +- **下一步**：① n=512 复核 commit=1；② 积分反馈（fb_ki，针对慢变偏差）在 commit=2 上叠加；③ 浏览器 JS 端同步解析 J；④ commit=2 的 40 ms 周期实测规划 P99（Python/JS）。 + ## 分支清理（2026-10-20） - `feat/fb-mix`（8e5431c）：反馈混合方案的中间分支，**已验证是 `exp/dagger-deploy` 的祖先**（所有提交都在后者历史里），本地/远端分支删除，内容保留在 exp/dagger-deploy。 diff --git a/ml/common/kin_torch.py b/ml/common/kin_torch.py index 37a28e0..165d8be 100644 --- a/ml/common/kin_torch.py +++ b/ml/common/kin_torch.py @@ -110,6 +110,19 @@ class Kin: Rs, _ = xf if xf is not None else self.cum_xf(q) return Rs[:, 6] + def jac(self, q, xf=None): + """几何雅可比（对度）：返回 (Jv [B,3,6] mm/deg, Jw [B,3,6] 单位轴/deg)""" + Rs, Ts = xf if xf is not None else self.cum_xf(q) + p = self.fk(q, (Rs, Ts)) + Jv, Jw = [], [] + for k in range(6): + d = torch.tensor(self.D[k], dtype=q.dtype, device=q.device) + a = Rs[:, k] @ d + o = torch.einsum('bij,j->bi', Rs[:, k], self.P[k].to(q.dtype)) + Ts[:, k] + Jv.append(torch.linalg.cross(a, p - o, dim=-1) * D2R) + Jw.append(a * D2R) + return torch.stack(Jv, -1), torch.stack(Jw, -1) + def caps_world(self, q, xf=None): Rs, Ts = xf if xf is not None else self.cum_xf(q) Rg, Tg = Rs[:, self.cap_g], Ts[:, self.cap_g] # [B,C,3,3], [B,C,3] diff --git a/ml/eval/check_dls.py b/ml/eval/check_dls.py new file mode 100644 index 0000000..4af9343 --- /dev/null +++ b/ml/eval/check_dls.py @@ -0,0 +1,54 @@ +"""DLS 解析雅可比对拍 + 延迟（python -m ml.eval.check_dls）""" +import time +import torch + +from ml.common.kin_torch import Kin +from ml.sim.closed_loop import dls_steps + + +def main(): + dev = 'cuda' if torch.cuda.is_available() else 'cpu' + kin = Kin(device=dev) + g = torch.Generator(device=dev); g.manual_seed(0) + B = 64 + lo, hi = kin.lo + 20, kin.hi - 20 + q = lo + (hi - lo) * torch.rand(B, 6, device=dev, generator=g) + xf = kin.cum_xf(q) + p = kin.fk(q, xf) + T = p + 6 * torch.randn(B, 3, device=dev, generator=g) + R = torch.zeros(B, 9, device=dev) + a = dls_steps(kin, q, T, R, n=3, analytic=True) + b = dls_steps(kin, q, T, R, n=3, analytic=False) + d = (a - b).abs() + print('pos-only max|Δdq| %.6f° mean %.6f° rel %.4f' % (d.max().item(), d.mean().item(), (d.norm() / b.norm()).item())) + # 带姿态目标：绕随机轴 ±8° + axis = torch.randn(B, 3, device=dev, generator=g); axis = axis / axis.norm(dim=-1, keepdim=True) + ang = (torch.rand(B, 1, device=dev, generator=g) * 16 - 8) * torch.pi / 180 + Kx = torch.zeros(B, 3, 3, device=dev) + Kx[:, 0, 1] = -axis[:, 2]; Kx[:, 0, 2] = axis[:, 1]; Kx[:, 1, 0] = axis[:, 2] + Kx[:, 1, 2] = -axis[:, 0]; Kx[:, 2, 0] = -axis[:, 1]; Kx[:, 2, 1] = axis[:, 0] + I = torch.eye(3, device=dev) + Rd = I + torch.sin(ang)[..., None] * Kx + (1 - torch.cos(ang))[..., None] * (Kx @ Kx) + Rt = kin.orient(q, xf) @ Rd + R9 = Rt.reshape(B, 9) + a = dls_steps(kin, q, T, R9, n=3, analytic=True) + b = dls_steps(kin, q, T, R9, n=3, analytic=False) + d = (a - b).abs() + print('pos+rot max|Δdq| %.6f° mean %.6f° rel %.4f' % (d.max().item(), d.mean().item(), (d.norm() / b.norm()).item())) + + for n_ in (1, 3): + for Bn in (1, 256): + qq = q[:Bn]; TT = T[:Bn]; RR = R9[:Bn] + for an in (False, True): + for _ in range(3): + dls_steps(kin, qq, TT, RR, n=n_, analytic=an) + torch.cuda.synchronize() if dev == 'cuda' else None + t0 = time.perf_counter() + for _ in range(20): + dls_steps(kin, qq, TT, RR, n=n_, analytic=an) + torch.cuda.synchronize() if dev == 'cuda' else None + print('n=%d B=%3d analytic=%d %.2f ms' % (n_, Bn, an, (time.perf_counter() - t0) / 20 * 1e3)) + + +if __name__ == '__main__': + main() diff --git a/ml/eval/eval_cl.py b/ml/eval/eval_cl.py new file mode 100644 index 0000000..ce172d2 --- /dev/null +++ b/ml/eval/eval_cl.py @@ -0,0 +1,75 @@ +"""小规模闭环评估（部署口径）： + python -m ml.eval.eval_cl --n 256 --commit 3 --fb 1.8 --partial [--ki 0.1 --select consist] +输出：到达 / 碰撞 / ratio2s / 刹停 / 精修接管 / 墙钟。直接调 rollout，参数与部署一致。""" +import argparse, functools, json, time +import torch + +from ml.common.ckpt import load_policy +from ml.common.kin_torch import Kin +from ml.data import load_tasks +from ml.sim.closed_loop import rollout +import ml.sim.closed_loop as clmod + + +def run(model, kin, tasks, device, bs=256, **kw): + agg, per = {}, [] + for i in range(0, len(tasks), bs): + r, p = rollout(model, kin, tasks[i:i + bs], device, **kw) + per.append((r, p, len(tasks[i:i + bs]))) + n = sum(w for _, _, w in per) + for r, _, w in per: + for k, v in r.items(): + if k in ('n', 'time_budget') or v is None: continue + agg[k] = agg.get(k, 0.0) + v * w + out = {k: v / n for k, v in agg.items()}; out['n'] = n + out['goal_ratio_2s'] = torch.cat([p['goal_ratio_2s'] for _, p, _ in per]).median().item() + out['err_fail_med'] = torch.cat([p['err'] for _, p, _ in per])[~torch.cat([p['ok'] for _, p, _ in per])].median().item() + out['dls_share'] = torch.cat([p['dls_it'] for _, p, _ in per]).ge(0).float().mean().item() + out['time_budget'] = per[0][0]['time_budget'] + return out + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument('--ckpt', default='ml/runs/base_dag2_dag1/best.pt') + ap.add_argument('--samples', default='ml/data/samples') + ap.add_argument('--tasks', default=None) + ap.add_argument('--split', default='val') + ap.add_argument('--n', type=int, default=256) + ap.add_argument('--K', type=int, default=8) + ap.add_argument('--commit', type=int, default=3) + ap.add_argument('--fb', type=float, default=1.8) + ap.add_argument('--ki', type=float, default=0.0) + ap.add_argument('--kdecay', type=float, default=0.9) + ap.add_argument('--kimax', type=float, default=0.5) + ap.add_argument('--select', default='consist') + ap.add_argument('--replan', default='r3') + ap.add_argument('--safety', type=int, default=1, help='1=安全层+部分前缀；0=关') + ap.add_argument('--partial', type=int, default=1) + ap.add_argument('--max_sec', type=float, default=None) + ap.add_argument('--seed', type=int, default=0) + ap.add_argument('--dls', default='analytic', choices=['analytic', 'fd']) + ap.add_argument('--device', default='cuda' if torch.cuda.is_available() else 'cpu') + a = ap.parse_args() + if a.dls == 'fd': + clmod.dls_steps = functools.partial(clmod.dls_steps, analytic=False) + model, ck = load_policy(a.ckpt, a.device) + kin = Kin(device=a.device) + assert ck.get('fp') in (None, kin.fp) + tasks = ([json.loads(l) for l in open(a.tasks, encoding='utf-8') if l.strip()] if a.tasks + else [t for t in load_tasks([a.samples]) if t['split'] == a.split]) + tasks = tasks[:a.n] + t0 = time.perf_counter() + r = run(model, kin, tasks, a.device, K=a.K, safety=bool(a.safety), seed=a.seed, max_sec=a.max_sec, + safety_partial=(a.commit if (a.safety and a.partial) else 0), commit_n=a.commit, + select=a.select, replan=a.replan, fb_gain=a.fb, fb_ki=a.ki, fb_decay=a.kdecay, fb_imax=a.kimax) + wall = time.perf_counter() - t0 + print('ckpt=%s n=%d K=%d commit=%d fb=%.2f ki=%.2f select=%s replan=%s safety=%d' % + (a.ckpt, r['n'], a.K, a.commit, a.fb, a.ki, a.select, a.replan, a.safety)) + print('success %.3f time_ok_1p3 %.3f coll %.3f ratio2s %.3f brakes %.1f dls_share %.3f err_fail %.2f wall %.1fs (%.0f ms/题)' + % (r['success'], r['time_ok_1p3'] or 0, r['collision_rate'], r['goal_ratio_2s'], r['brakes_mean'], + r['dls_share'], r['err_fail_med'], wall, wall / max(1, len(tasks)) * 1000)) + + +if __name__ == '__main__': + main() diff --git a/ml/sim/closed_loop.py b/ml/sim/closed_loop.py index f064216..c8cce4e 100644 --- a/ml/sim/closed_loop.py +++ b/ml/sim/closed_loop.py @@ -48,26 +48,33 @@ def goal_err(kin, q, T, R): return e, oe -def dls_steps(kin, q, T, R, n=3, scale=0.3, lam=0.5): - """局部 DLS 精修：n 步增量 [B,n,6]""" +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 = kin.cum_xf(x); p = kin.fk(x, xf); Rq = kin.orient(x, xf) + 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): - e0 = err(x, T, Rt, pose) - # 2026-10-20：6 次串行有限差分合并为一次批量 FK（数值等价，单次规划 ~25ms→~7ms） - 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)] + xf = kin.cum_xf(x) + e0 = err(x, T, Rt, pose, xf) + if analytic: + # J = −∂err/∂q：位置 err = T−p → +Jv；姿态 err = 100·rotvec(Rt·Rqᵀ) → +100·Jw + Jv, Jw = kin.jac(x, xf) + J = torch.cat([Jv, 100 * Jw * pose[:, None, None]], 1) # [B,6(err),6(q)] + else: + # 2026-10-20：6 次串行有限差分合并为一次批量 FK（数值等价，单次规划 ~25ms→~7ms） + 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) @@ -146,7 +153,7 @@ class TeacherOracle: 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_exec=0.0, exec_safety=0, fb_ki=0.0, fb_decay=0.9, fb_imax=0.5): """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 条）。 @@ -165,6 +172,7 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None, 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) # 积分项：累积持续同向的目标反馈（慢变偏差） 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) @@ -197,7 +205,12 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None, inc = act.float().clamp(-1, 1) * astep if fb_gain > 0: # 实验（2026-10-20）：目标反馈加在所有候选上（安全层也评估带反馈的候选） fb = dls_steps(kin, qe, T, R, n=1) - if fb_scale > 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) inc = inc + gdec[:, None, None, None] * fb[:, :, None, :] @@ -289,6 +302,8 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None, 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 接管的行无效 + 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 queue = torch.cat([queue, commit], 1) cold = False