From 6ecd33db0c5036d7c34ee98d4e61adf5fc1a517c Mon Sep 17 00:00:00 2001 From: cbb <2625274797@qq.com> Date: Tue, 20 Oct 2026 19:48:23 +0800 Subject: [PATCH] =?UTF-8?q?exp:=20=E8=BD=AE=E6=AC=A133(=E4=B8=8A)=E2=80=94?= =?UTF-8?q?=E2=80=94=E5=AE=9E=E9=99=85=E6=89=A7=E8=A1=8C=E9=80=9F=E5=BA=A6?= =?UTF-8?q?=E5=81=9A=E6=88=90=E4=B8=80=E7=AD=89=E6=8C=87=E6=A0=87=E5=B9=B6?= =?UTF-8?q?=E8=87=AA=E8=AF=81=EF=BC=9B=E9=BB=98=E8=AE=A4=E8=B7=AF=E5=BE=84?= =?UTF-8?q?=E9=80=90=E4=BD=8D=E5=A4=8D=E7=8E=B0=E7=AC=AC30=E8=BD=AE?= =?UTF-8?q?=EF=BC=8C256=E9=A2=98=E5=85=A8=E9=83=A8=E8=B6=85VMAX?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - rollout 逐题记录 max|Δq|/STEP（1.0=VMAX）与每步速度变化比；eval_cl 汇总 p50/p95/max、over_vmax_rate - 自证：用 round30_after trace 独立重算得 1.660004×VMAX、超速度占比 1.0，与新指标逐位一致 - 默认参数下 base 复现第30轮 after 的每个旧指标（0.57421875/0.390625/19.25/2.444952726/0.209975064 等） - 新开关 inc_cap（反馈叠加后再按每轴 VMAX·DT 限幅）、gap_margin（余量门限+自动抬宽相位 cap， 否则现 cap=0.5 会让 >0.5 的余量恒假）；col 仍按 gap<=0 判真实碰撞，不与余量混用 - 只读核查：ik-policy.js:355 同样在 ±1·STEP 之后叠加反馈；JS 的 5 mm 余量是投影式护栏，非同机制 - 口径提醒：round30 plans['gaps'] 在 cap=0.5 截顶，不能据此算"≥1mm 占比"；中位最小间隙 0.046 mm 可用 - 测试 26→30 项（限幅灵敏度、余量拒/受、cap 抬升生效） --- docs/experiment_round33_base.json | 70 +++++++++++++++++++++++++++++++ docs/探索日志.md | 11 +++++ ml/eval/eval_cl.py | 9 +++- ml/sim/closed_loop.py | 36 +++++++++++----- tests/test_eval_cl.py | 61 +++++++++++++++++++++++++++ 5 files changed, 175 insertions(+), 12 deletions(-) create mode 100644 docs/experiment_round33_base.json diff --git a/docs/experiment_round33_base.json b/docs/experiment_round33_base.json new file mode 100644 index 0000000..66a38b9 --- /dev/null +++ b/docs/experiment_round33_base.json @@ -0,0 +1,70 @@ +{ + "config": { + "ckpt": "ml/runs/base_dag2_dag1/best.pt", + "samples": "ml/data/samples", + "tasks": null, + "split": "val", + "n": 256, + "K": 8, + "commit": 2, + "fb": 2.2, + "ki": 0.0, + "fb_scale": 0.0, + "fb_near": 0.2, + "fb_fallback": 0.0, + "kdecay": 0.9, + "kimax": 0.5, + "fb_every": 1, + "safety_try": 0, + "sel_w": 0.1, + "dls_safety": 1, + "fb_clear": 0.0, + "fb_cmin": 0.3, + "sub_refresh": 0, + "fb_block_gain": 0.5, + "dump": "ml/runs/round33_base_rows.json", + "out": "docs/experiment_round33_base.json", + "trace": "ml/runs/round33_base.pt", + "select": "endpoint", + "replan": "empty", + "safety": 1, + "partial": 1, + "inc_cap": 0, + "gap_margin": 0.0, + "max_sec": null, + "seed": 0, + "dls": "analytic", + "device": "cuda" + }, + "batch_size": 256, + "batch_seed_rule": "same seed per batch", + "checkpoint_sha256": "f7f686fd8ee92fb6948fbcc4ee954636065ad87bb54f661c4f8f575bc16c2ce1", + "tasks_sha256": "249bd93457825cda98c59e64d9a096d3473c69aeb3553de1dea6d9e56be71772", + "kin_fp": "2ebcbf17", + "act_scale": 1.0, + "metrics": { + "n": 256, + "n_ref": 256, + "success": 0.57421875, + "collision_free_success": 0.57421875, + "time_ok_1p3": 0.390625, + "collision_rate": 0.0, + "collision_steps_mean": 0.0, + "travel_ratio_median": 2.4449527263641357, + "time_mean_s": 2.3076188564300537, + "brakes_mean": 19.25, + "start_fail": 0.0, + "goal_ratio_2s": 0.20997506380081177, + "err_fail_med": 229.68746948242188, + "err_success_med": 0.18899640440940857, + "dls_share": 0.625, + "time_budget": "teacher×2(≥3 s)", + "speed_ratio_p50": 1.6600004434585571, + "speed_ratio_p95": 1.6600035429000854, + "speed_ratio_max": 1.660003662109375, + "over_vmax_rate": 1.0, + "accel_ratio_p95": 1.5835107564926147, + "accel_ratio_max": 1.6600004434585571 + }, + "simulation_wall_s": 296.2702505999914 +} diff --git a/docs/探索日志.md b/docs/探索日志.md index bce2dd8..4aa9ebf 100644 --- a/docs/探索日志.md +++ b/docs/探索日志.md @@ -642,6 +642,17 @@ - **机理定位与下一步**：成本 = 单次调用固定开销 × 由 `log(bound/间隙)` 决定的层数。要同时拿到"证得出"和"证得起"，必须把**间隙抬起来**（安全层要求正余量，而不是 `gap>0`）并把**每步位移压回去**（反馈叠加在 ±1 限幅之后，导致第30轮实测最大 1.66×VMAX，界也随之变大）。顺带记下一个坑：安全层现在是 `kin.gaps(allq, 0.5)`，截顶值 0.5 mm 低于任何 >0.5 mm 的余量门限，直接把余量判据变成恒假——加余量必须同时抬这个 cap。 - **回归与判据灵敏度**：`python -m unittest tests.test_eval_cl` **26 项通过**（新增 3 项均匀细分）。含解析对照：合成恒间隙段（界 1 mm/度、间隙 0.1 mm）在 n=5 必判证不出、n=6 必判安全，且 `cap_required_mm` 等于 `1/n+2eps`；薄障碍在 n=4/8/64 判碰撞而 n=2 只判证不出（不误放行）；`n` 给成张量被类型门拒绝。二次加密反证在 n=8..128 全部 0 误放行。 +## 轮次 33：把速度做成可观测的硬约束，再测限幅与余门的代价（exp: `exp/dagger-deploy`） + +- **仪表先行**：`rollout()` 现在逐题记录**实际执行**的速度比 `max_j|Δq_j|/STEP`（STEP=VMAX·DT，所以 1.0 = VMAX）和按控制步的速度变化比；`eval_cl` 汇总 `speed_ratio_p50/p95/max`、`over_vmax_rate`、`accel_ratio_p95/max`，并把逐题值放进 `per`（极值/分位不进按 n 加权的批聚合）。 +- **仪表自证**：用第30轮 after 的 trace 独立重算得 max **1.660004×VMAX**、超速度题占比 **1.0**，与新指标在同一批上逐位一致（`speed_ratio_max` 1.660003662、`over_vmax_rate` 1.0）；这是"先证判据有灵敏度"，不是拿新数当结论。 +- **默认路径未变**：`--inc_cap 0 --gap_margin 0`（新默认）跑出的每个旧指标与第30轮 after 完全相同——success 0.57421875、无碰撞到达 0.57421875、time_ok 0.390625、碰撞 0、刹停 19.25、行程比 2.4449527263641357、ratio2s 0.20997506380081177、成功题误差 0.18899640440940857、dls_share 0.625（`docs/experiment_round33_base.json`）。整批墙钟 296.3 s 与第30轮 248.8 s 不可比：这期间我在同机跑了 CPU 分析，属自我污染，仪表开销需独占另测，不作为结论。 +- **两个新开关**：`inc_cap=1` 在**反馈叠加之后**再按每轴 VMAX·DT 限幅（此前 `inc=clamp(act,±1)·astep` 之后又加 `gain×fb`，所以 1.66× 是结构性的）；`gap_margin>0` 让安全层要求间隙 > 余量而非 >0，并把宽相位 cap 自动抬到 `margin+0.5`——不抬的话 `gaps(allq, 0.5)` 的截顶值 0.5 会让任何 >0.5 的余量门限恒假。`col`（真实碰撞计数）仍按 `gap≤0` 判，不随余量改变，所以两者不会互相顶替。 +- **部署侧同病**（只读核查，未改 JS）：`webapp/js/ik-policy.js:355` 也是 `inc[s][j] += fbGain*fbStep[j]` 在 ±1·STEP 之后叠加，所以浏览器控制器同样能超 VMAX；JS 的余量是 `o.margin=5` 的**投影式**护栏（`project()` 每轴 ≤projMax，只处理 gap1（灵敏度）、限幅后恰为 1.0、0.3 mm 间隙在余量 0 时通过而余量 0.5 时刹停、5 mm 间隙在余量 0.5 时仍通过（证明 cap 抬升生效，否则该测试会恒假）。 + ## 分支清理（2026-10-20） - `feat/fb-mix`（8e5431c）：反馈混合方案的中间分支，**已验证是 `exp/dagger-deploy` 的祖先**（所有提交都在后者历史里），本地/远端分支删除，内容保留在 exp/dagger-deploy。 diff --git a/ml/eval/eval_cl.py b/ml/eval/eval_cl.py index 04e143c..d944610 100644 --- a/ml/eval/eval_cl.py +++ b/ml/eval/eval_cl.py @@ -44,6 +44,11 @@ def run(model, kin, tasks, device, bs=256, **kw): 'time_budget': ('fixed %.3f s' % kw['max_sec']) if kw.get('max_sec') is not None else (per[0][0]['time_budget'] if len({r['time_budget'] for r, _, _ in per}) == 1 else 'mixed'), } + if 'speed_ratio' in flat: # 实际执行速度：每题时限内 max|Δq|/STEP（1.0 = VMAX），加速度同口径按每控制步的速度变化 + v, acc = flat['speed_ratio'], flat['accel_ratio'] + out.update({'speed_ratio_p50': median(v), 'speed_ratio_p95': v.quantile(0.95).item(), + 'speed_ratio_max': v.max().item(), 'over_vmax_rate': (v > 1.001).float().mean().item(), + 'accel_ratio_p95': acc.quantile(0.95).item(), 'accel_ratio_max': acc.max().item()}) return out, per @@ -85,6 +90,8 @@ def main(): 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('--inc_cap', type=int, default=0, help='1=反馈叠加后再按每轴 VMAX·DT 限幅') + ap.add_argument('--gap_margin', type=float, default=0.0, help='安全层要求的间隙余量 mm（>0 时宽相位 cap 自动抬到 margin+0.5）') 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']) @@ -111,7 +118,7 @@ def main(): 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_scale=a.fb_scale, fb_near=a.fb_near, fb_fallback=a.fb_fallback, fb_ki=a.ki, fb_decay=a.kdecay, fb_imax=a.kimax, fb_every=a.fb_every, safety_try=a.safety_try, sel_w=a.sel_w, dls_safety=a.dls_safety, fb_clear=a.fb_clear, fb_cmin=a.fb_cmin, sub_refresh=a.sub_refresh, fb_block_gain=a.fb_block_gain, - return_traj=bool(a.trace)) + inc_cap=a.inc_cap, gap_margin=a.gap_margin, return_traj=bool(a.trace)) wall = time.perf_counter() - t0 result = {**meta, 'metrics': r, 'simulation_wall_s': wall} print(json.dumps(json_finite(result), ensure_ascii=False, allow_nan=False), flush=True) diff --git a/ml/sim/closed_loop.py b/ml/sim/closed_loop.py index b0677af..62ae7ed 100644 --- a/ml/sim/closed_loop.py +++ b/ml/sim/closed_loop.py @@ -163,17 +163,21 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=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, - progress=None): + inc_cap=0, gap_margin=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。除默认值外均为诊断用。""" + sub_rule：子目标推进规则，见 sub_advance。除默认值外均为诊断用。 + inc_cap=1：把「策略动作 + 目标反馈」的合计增量再按每轴 VMAX·DT 限一次幅（默认关；关时反馈是在 ±1 限幅之后叠加的，第30轮实测最大 1.66×VMAX）。 + gap_margin>0：安全层要求间隙大于该毫米余量而不是 >0；宽相位 cap 自动抬到 margin+0.5，否则 0.5 的截顶值会让余量门限恒假。 + 两者默认值下行为与第32轮逐位相同。""" 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) @@ -190,6 +194,8 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None, 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) + # 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) traj = [q.clone()] if return_traj else None plans = {k: [] for k in ('it', '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° @@ -254,6 +260,8 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None, 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 @@ -325,8 +333,8 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None, 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, 0.5)['gm'].min(-1).values.view(B, 2 * commit_n) - ok6 = gaps6 > 0 + 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 @@ -350,8 +358,8 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None, 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, 0.5)['gm'].min(-1).values.view(B, 2 * commit_n) - okk = (gaps6 > 0).all(-1) + 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: @@ -367,8 +375,8 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None, 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, 0.5)['gm'].min(-1).values.view(B, 2 * commit_n) - takef = braked & (gaps6 > 0).all(-1) + 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) @@ -381,8 +389,8 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None, 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, 0.5)['gm'].min(-1).values.view(B, 2 * commit_n) - okd = (gaps6 > 0).all(-1) + 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) @@ -418,6 +426,11 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None, 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 e, oe = goal_err(kin, q, T, R) if it + 1 == k2s: e2 = e.clone() newly = (~done) & (e < 1) & (oe < 0.5) @@ -438,13 +451,14 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None, '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(), '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(), 'first_hit': first_hit.cpu(), 'dls_it': dls_it.cpu(), - 'goal_ratio_2s': ratio2.cpu()} + 'goal_ratio_2s': ratio2.cpu(), 'speed_ratio': vratio.cpu(), 'accel_ratio': aratio.cpu()} if return_traj: per['traj'] = torch.stack(traj, 1).cpu() per['plans'] = {k: torch.tensor(v) if k == 'it' else torch.stack(v, 1).cpu() for k, v in plans.items()} diff --git a/tests/test_eval_cl.py b/tests/test_eval_cl.py index 7eb9c84..46b685a 100644 --- a/tests/test_eval_cl.py +++ b/tests/test_eval_cl.py @@ -27,6 +27,7 @@ class EvalMetricsTest(unittest.TestCase): 'travel': [6.0, 0.0, 8.0], 'brakes': [2.0, 4.0, 6.0], 'start_fail': [False, True, False], 'goal_ratio_2s': [0.3, 0.9, 0.1], 'err': [0.2, 200.0, 0.8], 'dls_it': [4, -1, 20], + 'speed_ratio': [1.66, 0.9, 1.2], 'accel_ratio': [0.3, 0.2, 0.5], }.items()} def evaluate(self, tasks, bs=2, **kw): @@ -53,6 +54,12 @@ class EvalMetricsTest(unittest.TestCase): self.assertAlmostEqual(result['goal_ratio_2s'], 0.3, places=6) self.assertEqual(result['err_fail_med'], 200.0) self.assertEqual(result['brakes_mean'], 4.0) + # 极值/分位按逐题值跨批汇总，不能进按 n 加权求均值的批聚合 + self.assertAlmostEqual(result['speed_ratio_p50'], 1.2, places=6) + self.assertAlmostEqual(result['speed_ratio_p95'], 1.614, places=6) + self.assertAlmostEqual(result['speed_ratio_max'], 1.66, places=5) + self.assertAlmostEqual(result['over_vmax_rate'], 2 / 3, places=6) # 1.66 与 1.2 都算超 VMAX，0.9 不算 + self.assertAlmostEqual(result['accel_ratio_max'], 0.5, places=6) def test_empty_conditional_metrics_and_explicit_limit(self): task = {**self.tasks[0], 'exec_s': None, 'travel': None} @@ -550,5 +557,59 @@ class SegmentCertificateTest(unittest.TestCase): (p.detach() - point).norm(dim=-1), atol=1e-8, rtol=0) +class SpeedAndMarginTest(unittest.TestCase): + """轮次33：反馈叠加后的每轴速度限幅，以及安全层的正余量门限（含 cap 截顶坑）。""" + STEP = [1.2, 1.2, 1.8, 1.8, 2.4, 3.6] + + def rollout_one(self, teacher_step, gap_value, fb=None, **kwargs): + class FlatKin: + def clamp_box(self, q): + return q + + def gaps(self, q, cap): + return {'gm': torch.full((q.shape[0], 49), float(gap_value))} + + commit_n = 2 + traj = torch.zeros(1, commit_n + 1, 6) + traj[0, 1:] = torch.tensor(teacher_step)[None].repeat(commit_n, 1).cumsum(0) # teacher_step = 每步各轴的度数增量 + patches = { + 'features': lambda *args: torch.zeros(1, 83), + 'goal_err': lambda kin, q, T, R: (torch.full((q.shape[0],), 10.0), torch.zeros(q.shape[0])), + } + with patch.multiple('ml.sim.closed_loop', **patches): + with patch('ml.sim.closed_loop.dls_steps', return_value=torch.tensor([[[1.0, 0, 0, 0, 0, 0]]])): + _, per = rollout(None, FlatKin(), [{'q0': [0] * 6, 'T': [10, 0, 0]}], 'cpu', K=1, safety=True, + max_sec=0.02 * commit_n, return_traj=True, use_sub=False, select='first', + oracle=TeacherOracle(traj), replan='empty', commit_n=commit_n, + **kwargs) + return per + + def test_feedback_over_speed_is_measured_not_assumed(self): + # 策略给满 STEP（J1 +1.2°）+ 2.2×1.0° 反馈 = 3.4° > VMAX·DT + per = self.rollout_one([1.2, 0, 0, 0, 0, 0], 5.0, fb_gain=2.2) + self.assertAlmostEqual(per['traj'][0, 1, 0].item(), 3.4, places=5) + self.assertAlmostEqual(per['speed_ratio'][0].item(), 3.4 / 1.2, places=5) + + def test_inc_cap_limits_actual_step_to_vmax(self): + per = self.rollout_one([1.2, 0, 0, 0, 0, 0], 5.0, fb_gain=2.2, inc_cap=1) + self.assertAlmostEqual(per['traj'][0, 1, 0].item(), 1.2, places=5) + self.assertLessEqual(per['speed_ratio'][0].item(), 1.0 + 1e-6) + + def test_margin_rejects_near_obstacle_step_that_passes_at_zero(self): + moved = self.rollout_one([0.5, 0, 0, 0, 0, 0], 0.3, safety_partial=1) + self.assertAlmostEqual(moved['traj'][0, -1, 0].item(), 1.0, places=5) + self.assertEqual(moved['brakes'].item(), 0.0) + braked = self.rollout_one([0.5, 0, 0, 0, 0, 0], 0.3, gap_margin=0.5, safety_partial=1) + self.assertEqual(braked['brakes'].item(), 1.0) + torch.testing.assert_close(braked['traj'], torch.zeros(1, 3, 6), rtol=0, atol=0) + + def test_margin_below_cap_still_accepts_clear_step(self): + # 余量 0.5 必须仍能通过 5 mm 间隙：若宽相位 cap 没抬到 margin+0.5，截顶值 0.5 会让门限恒假 + per = self.rollout_one([0.5, 0, 0, 0, 0, 0], 5.0, gap_margin=0.5, safety_partial=1) + self.assertEqual(per['brakes'].item(), 0.0) + self.assertAlmostEqual(per['traj'][0, -1, 0].item(), 1.0, places=5) + self.assertTrue((per['plans']['gaps'] > 0.5).all().item()) + + if __name__ == '__main__': unittest.main()