From 9eaff7341f83b9785f0ecb60513ca4e009aab981 Mon Sep 17 00:00:00 2001 From: cbb <2625274797@qq.com> Date: Tue, 20 Oct 2026 17:06:53 +0800 Subject: [PATCH] =?UTF-8?q?exp:=20=E8=BD=AE=E6=AC=A129=E2=80=94=E2=80=94?= =?UTF-8?q?=E5=85=A8=E6=8B=92=E5=90=8E=E5=87=8F=E5=8D=8A=E5=8F=8D=E9=A6=88?= =?UTF-8?q?=E5=88=B0=E8=BE=BE55.1%=EF=BC=8C=E8=A1=A5=E9=BD=90=E6=97=A0?= =?UTF-8?q?=E7=A2=B0=E6=92=9E=E6=8C=87=E6=A0=87=E4=B8=8E=E9=A6=96=E6=92=9E?= =?UTF-8?q?=E5=9B=9E=E6=94=BE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 512题无碰撞到达54.7%、碰撞1.8%；9次首撞均在J5限位且双精度复现。保留小样本消融，撤回未经实际调度验证的50Hz达标结论，附六项回归测试及完整配置结果。 --- docs/experiment_round29.json | 62 ++ docs/experiment_round29_collisions.json | 779 ++++++++++++++++++++++++ docs/探索日志.md | 52 +- ml/eval/diag_collision.py | 56 +- ml/eval/eval_cl.py | 102 +++- ml/sim/closed_loop.py | 58 +- tests/test_eval_cl.py | 162 +++++ 7 files changed, 1227 insertions(+), 44 deletions(-) create mode 100644 docs/experiment_round29.json create mode 100644 docs/experiment_round29_collisions.json create mode 100644 tests/test_eval_cl.py diff --git a/docs/experiment_round29.json b/docs/experiment_round29.json new file mode 100644 index 0000000..e1c7e33 --- /dev/null +++ b/docs/experiment_round29.json @@ -0,0 +1,62 @@ +{ + "config": { + "ckpt": "ml/runs/base_dag2_dag1/best.pt", + "samples": "ml/data/samples", + "tasks": null, + "split": "val", + "n": 512, + "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/round29_rows.json", + "out": "docs/experiment_round29.json", + "trace": "ml/runs/round29_trace.pt", + "select": "endpoint", + "replan": "empty", + "safety": 1, + "partial": 1, + "max_sec": null, + "seed": 0, + "dls": "analytic", + "device": "cuda" + }, + "batch_size": 256, + "batch_seed_rule": "same seed per batch", + "checkpoint_sha256": "f7f686fd8ee92fb6948fbcc4ee954636065ad87bb54f661c4f8f575bc16c2ce1", + "tasks_sha256": "1fd72f71eaf8170b45782beede0d91544e1f7142e98c0642560af814382f01ef", + "kin_fp": "2ebcbf17", + "act_scale": 1.0, + "metrics": { + "n": 512, + "n_ref": 512, + "success": 0.55078125, + "collision_free_success": 0.546875, + "time_ok_1p3": 0.361328125, + "collision_rate": 0.017578125, + "collision_steps_mean": 0.1015625, + "travel_ratio_median": 2.425084352493286, + "time_mean_s": 2.322056770324707, + "brakes_mean": 19.228515625, + "start_fail": 0.0, + "goal_ratio_2s": 0.25053170323371887, + "err_fail_med": 194.51763916015625, + "err_success_med": 0.1961677223443985, + "dls_share": 0.634765625, + "time_budget": "teacher×2(≥3 s)" + }, + "simulation_wall_s": 488.4280448999925 +} diff --git a/docs/experiment_round29_collisions.json b/docs/experiment_round29_collisions.json new file mode 100644 index 0000000..5b53bc1 --- /dev/null +++ b/docs/experiment_round29_collisions.json @@ -0,0 +1,779 @@ +{ + "trace": "ml/runs/round29_trace.pt", + "n": 512, + "reported_collision_tasks": 9, + "replayed_first_hits32": 9, + "replayed_first_hits64": 9, + "scope": "Replay reported first hits only; same geometry kernel at float32/float64, not a full safety audit.", + "rows": [ + { + "i": 21, + "task": { + "tid": 1223280991, + "id": "s0001_000564", + "cat": "A", + "mode": "pose", + "q0": [ + 178.767, + 101.917, + 25.283, + 31.588, + -12.179, + -6.464 + ], + "T": [ + 320.779, + 79.294, + 350.853 + ], + "R": [ + -0.5548626629953249, + -0.1382477148217311, + -0.820374910976869, + -0.29956278115972873, + -0.8867511636570162, + 0.35204334093514156, + -0.7761375943462505, + 0.44108949556693955, + 0.4506112421399604 + ], + "exec_s": 2.86, + "travel": 581.023, + "reached": true, + "err": 0, + "split": "val" + }, + "first_hit_s": 5.659999847412109, + "dls_it": -1, + "point_order": [ + "q0", + "previous", + "midpoint", + "endpoint" + ], + "gap32_mm": [ + 0.5, + 0.5, + 0.35770511627197266, + -0.3957242965698242 + ], + "gap64_mm": [ + 0.5, + 0.5, + 0.3577231796451059, + -0.39572636491366353 + ], + "pair32": [ + 0, + 0, + 5, + 5 + ], + "pair64": [ + 0, + 0, + 5, + 5 + ], + "boundary_joints": [ + 5 + ], + "previous": [ + 139.78468322753906, + 87.256591796875, + 6.928672790527344, + 103.78778839111328, + -94.0, + 19.4688720703125 + ], + "endpoint": [ + 139.50450134277344, + 87.4175796508789, + 6.71871280670166, + 104.29376983642578, + -94.0, + 21.218488693237305 + ], + "dense_nsegments": 175, + "dense_gap64_min_mm": -0.39572636491366353 + }, + { + "i": 84, + "task": { + "tid": 1268963905, + "id": "s0002_000538", + "cat": "B", + "mode": "pose", + "q0": [ + -49.198, + 52.395, + 108.796, + 75.534, + 82.302, + -121.823 + ], + "T": [ + -85.062, + 275.378, + 888.929 + ], + "R": [ + 0.5139412462799992, + -0.4285630099475821, + -0.7431003578769356, + -0.5533754886400825, + -0.8275489878032067, + 0.09454227286372051, + -0.6554692700230204, + 0.36262435010704513, + -0.6624678231921397 + ], + "exec_s": 3.2600000000000002, + "travel": 349.09, + "reached": true, + "err": 0, + "split": "val" + }, + "first_hit_s": 5.980000019073486, + "dls_it": -1, + "point_order": [ + "q0", + "previous", + "midpoint", + "endpoint" + ], + "gap32_mm": [ + 0.5, + 0.030836105346679688, + -0.1565418243408203, + -0.11790847778320312 + ], + "gap64_mm": [ + 0.5, + 0.030838235678215753, + -0.15655898804830848, + -0.11791701257236653 + ], + "pair32": [ + 0, + 6, + 6, + 6 + ], + "pair64": [ + 0, + 6, + 6, + 6 + ], + "boundary_joints": [ + 5 + ], + "previous": [ + -69.87329864501953, + 36.559112548828125, + 2.094588279724121, + 102.22724151611328, + 89.0, + -212.1793212890625 + ], + "endpoint": [ + -69.79582214355469, + 37.351837158203125, + -0.033895254135131836, + 102.45509338378906, + 89.0, + -214.75440979003906 + ], + "dense_nsegments": 258, + "dense_gap64_min_mm": -0.16882067924099076 + }, + { + "i": 145, + "task": { + "tid": 593213945, + "id": "s0003_000414", + "cat": "B", + "mode": "pose", + "q0": [ + -49.395, + 84.783, + 2.744, + 14.841, + -86.783, + 81.932 + ], + "T": [ + 5.391, + 506.723, + 383.367 + ], + "R": [ + -0.5269859333114162, + -0.7554822816510317, + -0.38927156099983334, + 0.6402732880654818, + -0.05172298162073713, + -0.7664038424760652, + 0.5588702377848656, + -0.6531242265324508, + 0.5109724082909718 + ], + "exec_s": 2.96, + "travel": 560.55, + "reached": true, + "err": 0, + "split": "val" + }, + "first_hit_s": 5.599999904632568, + "dls_it": -1, + "point_order": [ + "q0", + "previous", + "midpoint", + "endpoint" + ], + "gap32_mm": [ + 0.5, + 0.5, + 0.3180522918701172, + -0.19827747344970703 + ], + "gap64_mm": [ + 0.5, + 0.5, + 0.318071359587325, + -0.19825289671655533 + ], + "pair32": [ + 0, + 0, + 5, + 5 + ], + "pair64": [ + 0, + 0, + 5, + 5 + ], + "boundary_joints": [ + 5 + ], + "previous": [ + -36.05080032348633, + 49.852294921875, + 22.054914474487305, + 67.3380126953125, + -94.0, + 79.87274932861328 + ], + "endpoint": [ + -35.60316848754883, + 48.9223747253418, + 21.386226654052734, + 67.87915802001953, + -94.0, + 79.27398681640625 + ], + "dense_nsegments": 93, + "dense_gap64_min_mm": -0.19825289671655533 + }, + { + "i": 164, + "task": { + "tid": 1743026973, + "id": "s0003_000814", + "cat": "C", + "mode": "pos", + "q0": [ + -65.982, + 5.595, + -64.669, + -72.912, + 85.732, + -86.377 + ], + "T": [ + -189.354, + 221.034, + 302.457 + ], + "R": null, + "exec_s": 2.3000000000000003, + "travel": 306.824, + "reached": true, + "err": 0, + "split": "val" + }, + "first_hit_s": 3.0399999618530273, + "dls_it": -1, + "point_order": [ + "q0", + "previous", + "midpoint", + "endpoint" + ], + "gap32_mm": [ + 0.5, + 0.5, + 0.5, + -0.12672901153564453 + ], + "gap64_mm": [ + 0.5, + 0.5, + 0.5, + -0.12673094091899273 + ], + "pair32": [ + 0, + 0, + 0, + 5 + ], + "pair64": [ + 0, + 0, + 0, + 5 + ], + "boundary_joints": [ + 5 + ], + "previous": [ + -33.6672248840332, + 39.63850021362305, + -60.174774169921875, + -108.2870864868164, + 88.27082061767578, + -74.56200408935547 + ], + "endpoint": [ + -32.8936882019043, + 39.478729248046875, + -58.639915466308594, + -107.65913391113281, + 89.0, + -74.92655944824219 + ], + "dense_nsegments": 154, + "dense_gap64_min_mm": -0.12673094091899273 + }, + { + "i": 175, + "task": { + "tid": 444406422, + "id": "s0003_001100", + "cat": "B", + "mode": "pos", + "q0": [ + -28.854, + 43.33, + -137.874, + -6.895, + -88.001, + -43.745 + ], + "T": [ + 379.105, + 293.88, + 653.541 + ], + "R": null, + "exec_s": 2.58, + "travel": 465.057, + "reached": true, + "err": 0, + "split": "val" + }, + "first_hit_s": 0.8399999737739563, + "dls_it": 144, + "point_order": [ + "q0", + "previous", + "midpoint", + "endpoint" + ], + "gap32_mm": [ + 0.5, + 0.4698514938354492, + -0.6612262725830078, + -0.9359521865844727 + ], + "gap64_mm": [ + 0.5, + 0.4698356780126254, + -0.6612412101671552, + -0.9359581587234 + ], + "pair32": [ + 0, + 6, + 6, + 6 + ], + "pair64": [ + 0, + 6, + 6, + 6 + ], + "boundary_joints": [ + 5 + ], + "previous": [ + -42.48794937133789, + 11.756612777709961, + -124.7530288696289, + -46.12489700317383, + -94.0, + -54.57075500488281 + ], + "endpoint": [ + -42.73639678955078, + 10.6148681640625, + -124.47537231445312, + -48.52891159057617, + -94.0, + -55.451263427734375 + ], + "dense_nsegments": 241, + "dense_gap64_min_mm": -0.9544276230224238 + }, + { + "i": 180, + "task": { + "tid": 737760595, + "id": "s0003_001280", + "cat": "B", + "mode": "pos", + "q0": [ + 41.213, + 49.353, + -149.808, + -8.981, + -88.566, + 83.45 + ], + "T": [ + 118.145, + 2.733, + 266.082 + ], + "R": null, + "exec_s": 1.78, + "travel": 228.67, + "reached": true, + "err": 0, + "split": "val" + }, + "first_hit_s": 1.1399999856948853, + "dls_it": 102, + "point_order": [ + "q0", + "previous", + "midpoint", + "endpoint" + ], + "gap32_mm": [ + 0.5, + 0.06517887115478516, + -0.0172576904296875, + 0.013133049011230469 + ], + "gap64_mm": [ + 0.5, + 0.06521150921177998, + -0.017227151210720493, + 0.013145878955246104 + ], + "pair32": [ + 0, + 5, + 5, + 5 + ], + "pair64": [ + 0, + 5, + 5, + 5 + ], + "boundary_joints": [ + 5 + ], + "previous": [ + 1.8212896585464478, + -9.327469825744629, + -150.87704467773438, + -23.00735855102539, + -94.0, + 93.73664855957031 + ], + "endpoint": [ + 1.3598560094833374, + -9.66404914855957, + -150.52992248535156, + -23.200042724609375, + -94.0, + 92.69589233398438 + ], + "dense_nsegments": 105, + "dense_gap64_min_mm": -0.020246891242656773 + }, + { + "i": 215, + "task": { + "tid": 169730876, + "id": "s0004_000544", + "cat": "B", + "mode": "pos", + "q0": [ + 47.051, + -1.179, + -94.31, + -43.128, + 67.067, + -119.867 + ], + "T": [ + -142.696, + -139.159, + 726.163 + ], + "R": null, + "exec_s": 2.16, + "travel": 461.684, + "reached": true, + "err": 0, + "split": "val" + }, + "first_hit_s": 3.5399999618530273, + "dls_it": -1, + "point_order": [ + "q0", + "previous", + "midpoint", + "endpoint" + ], + "gap32_mm": [ + 0.5, + 0.5, + 0.5, + -0.08817481994628906 + ], + "gap64_mm": [ + 0.5, + 0.5, + 0.5, + -0.08817155079043282 + ], + "pair32": [ + 0, + 0, + 0, + 6 + ], + "pair64": [ + 0, + 0, + 0, + 6 + ], + "boundary_joints": [ + 5 + ], + "previous": [ + 34.57734298706055, + 32.23564910888672, + -71.76512908935547, + 10.783411979675293, + 88.44934844970703, + -105.16633605957031 + ], + "endpoint": [ + 34.31901931762695, + 33.24732208251953, + -72.12022399902344, + 10.171412467956543, + 89.0, + -106.89080810546875 + ], + "dense_nsegments": 173, + "dense_gap64_min_mm": -0.08817155079043282 + }, + { + "i": 266, + "task": { + "tid": 458057184, + "id": "s0005_000271", + "cat": "B", + "mode": "pos", + "q0": [ + 44.394, + 32.402, + -145.074, + -48.37, + -85.543, + 13.576 + ], + "T": [ + 184.38, + -306.445, + 433.207 + ], + "R": null, + "exec_s": 2.52, + "travel": 376.205, + "reached": true, + "err": 0, + "split": "val" + }, + "first_hit_s": 1.2200000286102295, + "dls_it": 304, + "point_order": [ + "q0", + "previous", + "midpoint", + "endpoint" + ], + "gap32_mm": [ + 0.5, + 0.5, + 0.2372283935546875, + -0.05915641784667969 + ], + "gap64_mm": [ + 0.5, + 0.5, + 0.23719947210205383, + -0.05918908805933398 + ], + "pair32": [ + 0, + 0, + 5, + 5 + ], + "pair64": [ + 0, + 0, + 5, + 5 + ], + "boundary_joints": [ + 5 + ], + "previous": [ + 20.49032974243164, + 10.35391902923584, + -172.87489318847656, + -73.93492126464844, + -93.70915985107422, + 14.725995063781738 + ], + "endpoint": [ + 20.34522819519043, + 10.012239456176758, + -172.91363525390625, + -74.4600601196289, + -94.0, + 13.266871452331543 + ], + "dense_nsegments": 146, + "dense_gap64_min_mm": -0.05918908805933398 + }, + { + "i": 325, + "task": { + "tid": 1206824904, + "id": "s0005_001755", + "cat": "B", + "mode": "pose", + "q0": [ + -68.225, + 58.481, + -31.386, + 57.2, + -31.831, + -80.259 + ], + "T": [ + 30.805, + 353.752, + 661.419 + ], + "R": [ + 0.5493387160259536, + 0.13571391775309405, + 0.8245051289125273, + -0.09525700289583322, + -0.9701190077749987, + 0.22314841283987508, + 0.8301524429129026, + -0.2011239500451364, + -0.5199962290670785 + ], + "exec_s": 2.34, + "travel": 659.756, + "reached": true, + "err": 0, + "split": "val" + }, + "first_hit_s": 3.0999999046325684, + "dls_it": -1, + "point_order": [ + "q0", + "previous", + "midpoint", + "endpoint" + ], + "gap32_mm": [ + 0.5, + 0.3628082275390625, + 0.004294395446777344, + -0.6861848831176758 + ], + "gap64_mm": [ + 0.5, + 0.3628058862147938, + 0.004307730814957722, + -0.686172816414107 + ], + "pair32": [ + 0, + 18, + 5, + 5 + ], + "pair64": [ + 0, + 18, + 5, + 5 + ], + "boundary_joints": [ + 5 + ], + "previous": [ + -39.524147033691406, + 54.68175506591797, + -18.574026107788086, + 103.78845977783203, + -94.0, + -53.657958984375 + ], + "endpoint": [ + -39.42676544189453, + 55.778987884521484, + -18.88759994506836, + 103.97470092773438, + -94.0, + -54.84250259399414 + ], + "dense_nsegments": 119, + "dense_gap64_min_mm": -0.686172816414107 + } + ] +} \ No newline at end of file diff --git a/docs/探索日志.md b/docs/探索日志.md index 53cab4d..d65a574 100644 --- a/docs/探索日志.md +++ b/docs/探索日志.md @@ -5,6 +5,13 @@ 评测约定：val 任务、部署节奏（r = 3 重规划、1 段采样、K=8）；「纯策略」= 关安全层；「部署」= 开安全层。时延：桌面 RTX 3090 上 `sample_k(B=64, K=8)` 墙钟均值（均值，非 P99）；浏览器 ONNX P99 以 §9.2 R4 的独立测量为准。 +**口径更正（2026-10-20，轮次 29 复核；下列历史原始数值保留）：** +- 轮次 24–28 的“50 Hz 可行/连续无饥饿”结论撤回。同步 `await plan(); tick()` 不是实际 20 ms 调度；`starveTicks` 只是耗时除以 20 ms 的估算。组件分位数相加不等于整次规划 P99，实测 62–68 ms 也超过 commit=3 的 60 ms。仍须严格认证下的真查看器 R9 测量，且不放松 §9.2 的 P99 < 20 ms。 +- Python 端点/中点检查不等于连续认证；JS `--seg 0` 是放松安全的消融，不是部署验收。开安全层碰撞必须为 0，不能套用“纯策略 <3%”判据。既有到达率允许途中碰撞，新指标 `collision_free_success` 才是时限内无碰撞到达率。 +- `replan=empty` 是同步仿真的零前缀配方，不是已验证的异步执行配置。所有任务（含 D）继续保留在历史对照分母内；不能与 held-out 可达任务 ≥98% 直接比较。 +- 轮次 26 的几何角速度雅可比不等于远离目标时 skew 姿态残差的精确导数，因而既改变控制律又消除差分误差；不能把全部收益归因于去噪。3.58% 是单个位置雅可比样本对拍，不是普遍噪声界。`check_dls.py` 中标 B=256 的测量实际仅有 64 行，不能引用为 B=256 性能。 +- `err_fail_med` 是批次结束时未按时到达任务的末端误差，不是每题截止时误差；仿真墙钟/题也不是单次规划延迟。查看器默认选择器仍为 consist，endpoint 仅是可选实验。 + ## 轮次 0：基线（2026-10-18，main） - **假设**：模型开环已拟合老师（末端 ~6 mm），闭环失败另有原因。 @@ -498,18 +505,51 @@ - DLS 兜底半步（全拒时朝目标走一步并认证）：52.3%（基线 52.0%），碰撞 1.6→2.7%，无收益；重复多步版本碰撞 45.7%（第二步未认证），已回退为单步且默认关闭。 - 增益调度（远 2.8/近 2.2）：47.3% / time_ok 32.8%，等同 gain2.8 常数；高增益在远处就造成过冲，按距离衰减无效。 - **浏览器同步**：`select='endpoint'` 进 Runner/bench（`--select endpoint`），冷启动跳过 8 份前瞻（省 ~80 ms P99，冷启动价值评分被端点排序替代）。nc10 bench（--seg 0 --select endpoint）：pos ok 4/5 reached、pose ok 2/4，刹车率 32.1%/25.5%（consist 48.1%/31.6%），planP50 31.9/36.5 ms、P99 63.5/67.5 ms。 -- **50 Hz 部署口径实测（Runner 增加 `starveTicks`：ceil(planMs/20) − 规划时队列步数）**： +- **同步 bench 耗时与队列耗尽估算（不是 50 Hz 调度实测）**：Runner 的 `starveTicks = max(0,ceil(planMs/20) − 规划时队列步数)`，不含真实 tick 时间戳。 -| 集合（endpoint, commit=2, --seg 0） | 到达 ok/reached | 刹车率 | planP50/P99 | 饥饿 tick/次规划 | +| 集合（endpoint, commit=2, --seg 0） | 到达 ok/reached | 刹车率 | planP50/P99 | 估算饥饿 tick/次规划 | |---|---|---|---|---| | near_column_pos (10) | 4/5 | 32.7% | 32.0/62.1 ms | 0.87 | | near_column_pose (10) | 2/3 | 25.6% | 36.8/68.0 ms | 0.78 | | route40 (20) | 12/13 | 40.4% | 44.3/68.1 ms | 1.26 | - - commit=2 的队列只有 40 ms，route 集 planP50 44 ms 已经超过 → 每次规划平均空转 ~1.3 tick（50 Hz 下机械臂会"走两步停一下"）。 - - **commit=3（60 ms 队列）可覆盖 P50 44 ms 与多数 P99**：endpoint n=256 → 47.7% / 27.0% / 1.2% / 0.245；gain2.5 反而降到 45.7% / 24.6%。**连续 50 Hz 口径选 commit=3（47.7%），极限准确率选 commit=2（52.1%，偶发空转）**。 -- **结论**：**新历史最佳（50 Hz 节奏，n=512）= 端点选择 + 解析 J + gain2.2 + commit=2 + safety_partial：到达 52.1% / time_ok 35.2% / 碰撞 2.0% / ratio2s 0.285**。相对本会话开始的历史最佳（42.2% / 29.1% / 2.9% / 0.270）到达 +9.9 pp、时间效率 +6.1 pp。commit=1 上限 59.4%（碰撞 3.9%，超 3% 线，且规划频率不可实时）。 -- **下一步**：① 碰撞 2.0% 的来源分解（端点选择使候选更贴障碍？）；② 把 `select=endpoint` 写进查看器默认（Runner 默认仍 consist）；③ route 集 safety P50 30 ms 是 planP50 的主项，继续压认证成本（候选去重/按需投影已做，剩余在 collideFree 本身）。 + - commit=2 对应 40 ms 队列，route 集 planP50 44 ms 已超过；估算提示可能饥饿，但该 bench 等规划完成才 tick，没有观测实际空转。 + - commit=3 对应 60 ms，仍低于上述 P99，不能据此宣称连续 50 Hz。其 Python 同步结果：endpoint n=256 → 47.7% / 27.0% / 1.2% / 0.245；gain2.5 为 45.7% / 24.6%。 +- **结论（经轮次 29 更正）**：端点选择 + 解析 J + gain2.2 + commit=2 + safety_partial，在 n=512 同步仿真为 **到达 52.1% / time_ok 35.2% / 碰撞 2.0% / ratio2s 0.285**。历史 42.2% 使用 commit=1 与 FD，非单变量对照。commit=1 到达 59.4% 但碰撞 3.9%；无论 2.0% 还是 3.9%，开安全层均未通过零碰撞要求，实时性亦未通过。 +- **下一步**：先查碰撞来源，保持严格认证和查看器默认；再测真实异步调度及认证成本，不因 Python 收益直接切换部署默认。 + +## 轮次 29：安全层全拒后减弱目标反馈（exp: `exp/dagger-deploy`） + +- **假设**：目标反馈将绕障候选推向障碍，连续全拒会反复停在同处；仅在上一计划被全拒时减小下一计划反馈，可能保留正常段收敛速度并减少重复刹停。 +- **判据**：同任务、同 seed、同 K、同认证下到达和 time_ok 不下降，碰撞不增加；部署门槛仍是零碰撞及真实时延达标。所有实验不重训、不扩大数据。 +- **固定配方**：`python -m ml.eval.eval_cl --n 256 --K 8 --commit 2 --fb 2.2 --partial 1 --replan empty --select endpoint --seed 0`；dag2 checkpoint、几何雅可比。下表仅改列出的参数，默认 `fb_block_gain=1,dls_safety=0`。512 题按两个 256 批、每批 seed=0，沿用轮次 28 顺序。 + +| 改动（n=256） | 到达 | time_ok_1p3 | 碰撞 | ratio2s | 刹停/题 | +|---|---|---|---|---|---| +| 原配置复跑 | 52.7% | 37.5% | 2.3% | 0.210 | 34.6 | +| `--dls_safety 1` | 52.7% | 37.5% | 2.0% | 0.210 | 34.6 | +| `--fb_clear 20 --fb_cmin 0.3` | 32.0% | 5.9% | 1.2% | 0.323 | 3.2 | +| `--sub_refresh 1` | 51.6% | 35.5% | 2.0% | 0.279 | 33.8 | +| `--select adapt` | 52.7% | 37.9% | 2.3% | 0.210 | 34.6 | +| `--fb_block_gain 0.3` | 56.2% | 38.7% | 3.1% | 0.178 | 17.2 | +| `--fb_block_gain 0.5` | **57.4%** | **39.1%** | 3.1% | 0.210 | 19.3 | +| `--fb_block_gain 0.7` | 55.5% | 38.3% | 2.3% | 0.189 | 25.0 | +| `--fb_block_gain 0.5 --dls_safety 1` | **57.4%** | **39.1%** | 2.7% | 0.210 | 19.2 | + +- **单独减半反馈 n=512**：到达 55.1% / time_ok 36.1% / 碰撞 2.5% / ratio2s 0.251 / 刹停 19.5；相对轮次 28 同规模基线 52.1% / 35.2% / 2.0% / 0.285 / 35.7，到达 +3.0 pp、碰撞也 +0.5 pp，不满足“碰撞不增加”判据。 +- **为何不采用其他变体**：所有近障碍状态都衰减反馈会明显拖慢到达；子目标重预测无收益；adapt 全拒后旧计划本来就失效，一致性分数全为 0，实质变成等分选首条而非“继续绕行”。 +- **原配置失败分解（n=256）**：135 题按时到达，121 题失败（其中 20 题后来才到，101 题始终没到）。A/B/C/D/Dr 分别 128/74/32/13/9 题，B/C 失败题刹停中位数约 96/93 次；D 留在分母，不用删题提高成绩。 +- **碰撞定位**：原配置共 6 题碰撞，只有 224 号首次碰撞紧跟 DLS 接管；另外 5 题首次碰撞早于接管或根本未接管。为 DLS 加端点/中点检查仅减少 1 题，不能解释剩余碰撞。候选认证的未夹紧累加点与执行的逐步夹紧点不同是待验证假设，尚不能断言根因。 +- **计时**：n=256 减半反馈+DLS检查整批墙钟 243.6 s，单独减半 n=512 为 443.4 s；这些不是规划时延，不能证明 50 Hz。反馈条件分支未增加几何查询，但其实际延迟收益未测。 +- **记录改进**：评估输出完整参数、checkpoint/任务 SHA256、批大小和随机种子规则；增加时限内无碰撞到达率；修正 n_ref、参考时间占比和条件指标的跨批汇总。可保存实际轨迹供不重新采样的回放，NaN 在 JSON 中写为 null，执行进度每 50 步输出。 +- **回归检查**：`python -m unittest tests.test_eval_cl -v` 六项通过，覆盖跨批分母、空条件集、非有限数、首撞探针正对照、精修被拒后保留安全候选、只有全拒后才减弱反馈。精修对照在关闭检查时确实碰撞，打开后保留候选，证明门会执行；这是合成测试，不替代真实几何验收。 +- **仍未修复的边界**：`fb_fallback=0` 保持关闭；非部分前缀分支仍有“只验一步却重复执行”问题，不能开启部署。速度限制在反馈叠加前而非叠加后，尚未验证速度/加速度符合硬约束。 +- **减半反馈 + DLS 检查的 n=512 复核（完整输出 `docs/experiment_round29.json`）**：到达 **282/512 = 55.1%**、时限内无碰撞到达 **280/512 = 54.7%**、time_ok **36.1%**、碰撞 **9/512 = 1.8%**、ratio2s **0.251**、刹停 **19.23/题**。到达题末端误差中位数 **0.196 mm**；有参考且到达题的行程比中位数 **2.425**（远高于 1.10 要求）。整批墙钟 **488.4 s**，非控制延迟。 + - 完整命令：`python -u -m ml.eval.eval_cl --n 512 --commit 2 --fb 2.2 --partial 1 --replan empty --select endpoint --fb_block_gain 0.5 --dls_safety 1 --out docs/experiment_round29.json --dump ml/runs/round29_rows.json --trace ml/runs/round29_trace.pt`。 + - 与轮次 28 同规模端点基线相比，到达 +3.0 pp、time_ok +0.9 pp、碰撞少 1 题（10→9），不足以宣称稳定安全改善；n=256 碰撞仍由 6→7 题。 +- **首撞回放（`docs/experiment_round29_collisions.json`）**：`python -m ml.eval.diag_collision --trace ml/runs/round29_trace.pt --out docs/experiment_round29_collisions.json`。9 题首次碰撞在 CPU float32/float64 均复现，均位于 **J5 限位**；q0 与首次碰撞前一点均为正间隙，首撞采样点负间隙约 **−0.017 至 −0.936 mm**。这排除了这 9 次碰撞由起点本就碰撞或 float32 单独翻号造成，但还需要记录原认证点才能证明夹紧的因果作用；0.01° 加密仅覆盖首撞段，不是整条轨迹安全审计。 + - 双精度运动学/间隙另与已有 JS 2000 个样本对拍：`python -m ml.eval.check_parity` PASS，碰撞布尔差 0、间隙最大差 2.53e−13 mm。首撞回放仍共用碰撞模型，不替代真机或独立连续认证。 +- **结论与下一步**：减少连续刹停确有小样本收益，但碰撞、行程和真实时延仍未过门槛；保留为实验选项，不更改查看器默认。下一轮记录原认证点与 J5 夹紧后实际点，修复后用同批同 seed A/B 和独立细采样复核，禁止靠关掉认证提速。 ## 分支清理（2026-10-20） diff --git a/ml/eval/diag_collision.py b/ml/eval/diag_collision.py index 7fde784..cede8ba 100644 --- a/ml/eval/diag_collision.py +++ b/ml/eval/diag_collision.py @@ -25,9 +25,58 @@ def pair_names(): return {a * 7 + b: f'{nm(a)}-{nm(b)}' for a in range(7) for b in range(a + 1, 7)} +@torch.no_grad() +def replay_first_hits(path, out=None): + saved = torch.load(path, map_location='cpu', weights_only=True) + k32, k64 = Kin(), Kin(dtype=torch.float64) + rows, offset = [], 0 + for _, per, count in saved['batches']: + for b in (per['col'] > 0).nonzero().flatten().tolist(): + it = int(round(float(per['first_hit'][b]) / DT)) + prev, q = per['traj'][b, it - 1:it + 1] + mid = prev + 0.5 * (q - prev) + points = torch.stack([per['traj'][b, 0], prev, mid, q]) + gm32 = k32.gaps(points, 0.5)['gm'] + gm64 = k64.gaps(points.double(), 0.5)['gm'] + gap32, gp32 = gm32.min(-1) + gap64, gp64 = gm64.min(-1) + nseg = max(1, math.ceil(float((q - prev).abs().max()) / 0.01)) + dense = prev.double() + torch.linspace(0, 1, nseg + 1, dtype=torch.float64)[:, None] * (q.double() - prev.double()) + dense_gap = torch.cat([k64.gaps(chunk, 0.5)['gm'].min(-1).values for chunk in dense.split(32)]) + boundary = (~k32.cont) & ((q - k32.lo).abs().lt(1e-5) | (q - k32.hi).abs().lt(1e-5)) + row = { + 'i': offset + b, 'task': saved['tasks'][offset + b], + 'first_hit_s': float(per['first_hit'][b]), 'dls_it': int(per['dls_it'][b]), + 'point_order': ['q0', 'previous', 'midpoint', 'endpoint'], + 'gap32_mm': gap32.tolist(), 'gap64_mm': gap64.tolist(), + 'pair32': gp32.tolist(), 'pair64': gp64.tolist(), + 'boundary_joints': (boundary.nonzero().flatten() + 1).tolist(), + 'previous': prev.tolist(), 'endpoint': q.tolist(), + 'dense_nsegments': nseg, 'dense_gap64_min_mm': float(dense_gap.min()), + } + rows.append(row) + print('task %d first_hit %.2fs gap32 %s gap64 %s boundary %s' % + (row['i'], row['first_hit_s'], row['gap32_mm'], row['gap64_mm'], row['boundary_joints']), flush=True) + offset += count + result = { + 'trace': path, 'n': offset, 'reported_collision_tasks': len(rows), + 'replayed_first_hits32': sum(min(r['gap32_mm'][2:]) <= 0 for r in rows), + 'replayed_first_hits64': sum(min(r['gap64_mm'][2:]) <= 0 for r in rows), + 'scope': 'Replay reported first hits only; same geometry kernel at float32/float64, not a full safety audit.', + 'rows': rows, + } + if out: + with open(out, 'w', encoding='utf-8') as f: + json.dump(result, f, indent=2, allow_nan=False) + print('replayed first hits: %d/%d float32, %d/%d float64' % + (result['replayed_first_hits32'], len(rows), result['replayed_first_hits64'], len(rows)), flush=True) + return result + + def main(): ap = argparse.ArgumentParser() - ap.add_argument('--ckpt', required=True) + ap.add_argument('--ckpt') + ap.add_argument('--trace') ap.add_argument('--samples', default='ml/data/samples') ap.add_argument('--split', default='val') ap.add_argument('--n', type=int, default=512) @@ -38,6 +87,11 @@ def main(): ap.add_argument('--device', default='cuda' if torch.cuda.is_available() else 'cpu') ap.add_argument('--out', default=None) a = ap.parse_args() + if a.trace: + replay_first_hits(a.trace, a.out) + return + if not a.ckpt: + ap.error('--ckpt or --trace is required') ck = torch.load(a.ckpt, map_location=a.device) model = build(ck['cfg']).to(a.device) diff --git a/ml/eval/eval_cl.py b/ml/eval/eval_cl.py index 3d7b5a9..990dd43 100644 --- a/ml/eval/eval_cl.py +++ b/ml/eval/eval_cl.py @@ -1,32 +1,57 @@ -"""小规模闭环评估（部署口径）： - 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 argparse, functools, hashlib, json, math, time +from pathlib import Path 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 +from ml.sim.closed_loop import rollout, tasks_to_tensors import ml.sim.closed_loop as clmod def run(model, kin, tasks, device, bs=256, **kw): - agg, per = {}, [] + per = [] for i in range(0, len(tasks), bs): - r, p = rollout(model, kin, tasks[i:i + bs], device, **kw) + t0 = time.perf_counter() + print('batch %d-%d/%d start' % (i + 1, min(i + bs, len(tasks)), len(tasks)), flush=True) + def progress(step, total): + print('batch %d step %d/%d wall %.1fs' % (i // bs + 1, step, total, time.perf_counter() - t0), flush=True) + r, p = rollout(model, kin, tasks[i:i + bs], device, progress=progress, **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 + print('batch %d done wall %.1fs' % (i // bs + 1, time.perf_counter() - t0), flush=True) + flat = {k: torch.cat([p[k] for _, p, _ in per]) for k in per[0][1] if k != 'traj'} + _, _, _, ex, tr = tasks_to_tensors(tasks, 'cpu') + ok, col = flat['ok'], flat['col'] > 0 + has_ref = torch.isfinite(ex) + okr = ok & has_ref & torch.isfinite(tr) + median = lambda x: x.median().item() if x.numel() else None + out = { + 'n': len(tasks), 'n_ref': int(has_ref.sum()), + 'success': ok.float().mean().item(), + 'collision_free_success': (ok & ~col).float().mean().item(), + 'time_ok_1p3': (ok & has_ref & (flat['t_done'] <= 1.3 * ex)).sum().item() / int(has_ref.sum()) if has_ref.any() else None, + 'collision_rate': col.float().mean().item(), + 'collision_steps_mean': flat['col'].mean().item(), + 'travel_ratio_median': median(flat['travel'][okr] / tr[okr].clamp(min=1)), + 'time_mean_s': flat['t_done'][ok].mean().item() if ok.any() else None, + 'brakes_mean': flat['brakes'].mean().item(), + 'start_fail': flat['start_fail'].float().mean().item(), + 'goal_ratio_2s': median(flat['goal_ratio_2s']), + 'err_fail_med': median(flat['err'][~ok]), + 'err_success_med': median(flat['err'][ok]), + 'dls_share': flat['dls_it'].ge(0).float().mean().item(), + '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'), + } + return out, per + + +def json_finite(value): + if isinstance(value, dict): return {k: json_finite(v) for k, v in value.items()} + if isinstance(value, list): return [json_finite(v) for v in value] + if isinstance(value, float) and not math.isfinite(value): return None + return value def main(): @@ -48,6 +73,14 @@ def main(): ap.add_argument('--fb_every', type=int, default=1) ap.add_argument('--safety_try', type=int, default=0) ap.add_argument('--sel_w', type=float, default=0.1) + ap.add_argument('--dls_safety', type=int, default=0) + ap.add_argument('--fb_clear', type=float, default=0.0) + ap.add_argument('--fb_cmin', type=float, default=0.3) + ap.add_argument('--sub_refresh', type=int, default=0) + ap.add_argument('--fb_block_gain', type=float, default=1.0) + ap.add_argument('--dump', default=None) + ap.add_argument('--out', default=None) + ap.add_argument('--trace', default=None) ap.add_argument('--select', default='consist') ap.add_argument('--replan', default='r3') ap.add_argument('--safety', type=int, default=1, help='1=安全层+部分前缀；0=关') @@ -57,6 +90,7 @@ def main(): 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() + print('config ' + json.dumps(vars(a), ensure_ascii=False), flush=True) if a.dls == 'fd': clmod.dls_steps = functools.partial(clmod.dls_steps, analytic=False) model, ck = load_policy(a.ckpt, a.device) @@ -65,17 +99,35 @@ def main(): 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] + if not tasks: ap.error('no tasks selected') + meta = { + 'config': vars(a), 'batch_size': 256, 'batch_seed_rule': 'same seed per batch', + 'checkpoint_sha256': hashlib.sha256(Path(a.ckpt).read_bytes()).hexdigest(), + 'tasks_sha256': hashlib.sha256(json.dumps(tasks, sort_keys=True).encode()).hexdigest(), + 'kin_fp': kin.fp, 'act_scale': float(model.act_scale), + } 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, + r, pers = 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_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) + 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)) 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)) + result = {**meta, 'metrics': r, 'simulation_wall_s': wall} + print(json.dumps(json_finite(result), ensure_ascii=False, allow_nan=False), flush=True) + if a.out: + Path(a.out).write_text(json.dumps(json_finite(result), indent=2, ensure_ascii=False, allow_nan=False) + '\n', encoding='utf-8') + if a.dump: + rows = [] + keys = ['done', 'ok', 't_done', 'limit', 'col', 'brakes', 'err', 'first_hit', 'dls_it', 'goal_ratio_2s'] + flat = {k: torch.cat([p[k] for _, p, _ in pers]).tolist() for k in keys} + for i, t in enumerate(tasks): + rows.append({**t, 'i': i, **{k: flat[k][i] for k in keys}}) + Path(a.dump).write_text(json.dumps(json_finite(rows), allow_nan=False), encoding='utf-8') + print('dump', a.dump, flush=True) + if a.trace: + torch.save({**meta, 'tasks': tasks, 'batches': pers}, a.trace) + print('trace', a.trace, flush=True) if __name__ == '__main__': diff --git a/ml/sim/closed_loop.py b/ml/sim/closed_loop.py index 10a60f3..3110675 100644 --- a/ml/sim/closed_loop.py +++ b/ml/sim/closed_loop.py @@ -66,11 +66,10 @@ def dls_steps(kin, q, T, R, n=3, scale=0.3, lam=0.5, analytic=True): 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 + # 几何角速度 Jw 仅在姿态误差接近零时等于该 skew 残差的负导数。 Jv, Jw = kin.jac(x, xf) - J = torch.cat([Jv, 100 * Jw * pose[:, None, None]], 1) # [B,6(err),6(q)] + J = torch.cat([Jv, 100 * Jw * pose[:, None, None]], 1) 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)) @@ -153,7 +152,8 @@ 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_ki=0.0, fb_decay=0.9, fb_imax=0.5, fb_every=1, safety_try=0, sel_w=0.1): + 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): """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 条）。 @@ -174,6 +174,7 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None, 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) @@ -203,6 +204,14 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None, 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: @@ -210,6 +219,10 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None, 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 @@ -218,9 +231,14 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=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: - inc = inc + fb_gain * fb[:, :, None, :] + 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 cold: need = sub[:, 12] > 0.5 subs = q[:, None] + sub[:, :12].float().view(B, 2, 6) * 180 @@ -237,10 +255,16 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None, 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 == 'endpoint': + elif select in ('endpoint', 'adapt'): 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 == '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) @@ -272,6 +296,7 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None, 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 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() @@ -297,7 +322,7 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None, mskf = (maxlen == 0) & ok1 commit = torch.where(mskf[:, None, None], fbcom, commit) msk = msk | mskf[:, None] - brakes += (maxlen == 0).float() + 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) @@ -316,17 +341,24 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None, fb1 = dls_steps(kin, qe, T, R, n=1) * fb_fallback ok1 = kin.gaps(qe + fb1[:, 0], 0.5)['gm'].min(-1).values > 0 commit = torch.where((~ok_any & ok1)[:, None, None], fb1.expand(-1, commit_n, -1), commit) - brakes += (~ok_any).float() + brakes += (~ok_any).float(); braked = ~ok_any pick = chosen_k; committed = ok_any if near.any(): ref = dls_steps(kin, qe, T, R, n=commit_n) - commit = torch.where(near[:, None, None], ref, commit) - dls_it = torch.where(near & (dls_it < 0), torch.full_like(dls_it, it), dls_it) - committed = committed & ~near + take = near + if dls_safety and safety: # 2026-10-20：近目标 DLS 接管也要过安全层（部署端 _certCommit 本来就会验；不过则退回候选计划） + pts = qe[:, None] + torch.cumsum(ref, 1) + mids = torch.cat([qe[:, None], pts[:, :-1]], 1) + 0.5 * ref + allq = torch.cat([pts, mids], 1).reshape(-1, 6) + okd = (kin.gaps(allq, 0.5)['gm'].min(-1).values > 0).view(B, 2 * commit_n).all(-1) + take = near & okd + 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 = torch.cat([qe[:, None], fut[ar, pick]], 1); last_ok = committed; last_brake = braked queue = torch.cat([queue, commit], 1) cold = False d = queue[:, 0]; queue = queue[:, 1:] @@ -348,6 +380,8 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None, 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() diff --git a/tests/test_eval_cl.py b/tests/test_eval_cl.py new file mode 100644 index 0000000..6c97318 --- /dev/null +++ b/tests/test_eval_cl.py @@ -0,0 +1,162 @@ +import contextlib +import io +import json +import tempfile +import unittest +from pathlib import Path +from unittest.mock import patch + +import torch + +from ml.eval.eval_cl import json_finite, run +from ml.eval.diag_collision import replay_first_hits +from ml.sim.closed_loop import TeacherOracle, rollout + + +class EvalMetricsTest(unittest.TestCase): + def setUp(self): + self.tasks = [ + {'q0': [0] * 6, 'T': [0] * 3, 'exec_s': 1, 'travel': 5}, + {'q0': [0] * 6, 'T': [0] * 3, 'exec_s': None, 'travel': None}, + {'q0': [0] * 6, 'T': [0] * 3, 'exec_s': 2, 'travel': 6}, + ] + self.per = {k: torch.tensor(v) for k, v in { + 'ok': [True, False, True], 'done': [True, False, True], + 't_done': [0.5, float('nan'), 2.0], 'col': [1.0, 0.0, 0.0], + '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], + }.items()} + + def evaluate(self, tasks, bs=2, **kw): + offset = 0 + + def fake_rollout(model, kin, batch, device, **kwargs): + nonlocal offset + p = {k: v[offset:offset + len(batch)] for k, v in self.per.items()} + offset += len(batch) + return {'time_budget': 'mixed'}, p + + with patch('ml.eval.eval_cl.rollout', side_effect=fake_rollout), contextlib.redirect_stdout(io.StringIO()): + return run(None, None, tasks, 'cpu', bs=bs, **kw)[0] + + def test_global_denominators_and_collision_free_arrival(self): + result = self.evaluate(self.tasks) + self.assertEqual(result['n_ref'], 2) + self.assertAlmostEqual(result['success'], 2 / 3, places=6) + self.assertAlmostEqual(result['collision_free_success'], 1 / 3, places=6) + self.assertAlmostEqual(result['collision_rate'], 1 / 3, places=6) + self.assertEqual(result['time_ok_1p3'], 1.0) + self.assertEqual(result['time_mean_s'], 1.25) + self.assertAlmostEqual(result['travel_ratio_median'], 1.2, places=6) + 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) + + def test_empty_conditional_metrics_and_explicit_limit(self): + task = {**self.tasks[0], 'exec_s': None, 'travel': None} + result = self.evaluate([task], max_sec=0.04) + self.assertIsNone(result['err_fail_med']) + self.assertIsNone(result['time_ok_1p3']) + self.assertIsNone(result['travel_ratio_median']) + self.assertEqual(result['time_budget'], 'fixed 0.040 s') + self.assertEqual(result['collision_free_success'], 0.0) + + def test_json_replaces_nonfinite_without_changing_zero(self): + value = {'items': [float('nan'), float('inf'), -float('inf'), 0.0]} + text = json.dumps(json_finite(value), allow_nan=False) + self.assertEqual(json.loads(text), {'items': [None, None, None, 0.0]}) + + def test_first_hit_probe_detects_collision(self): + class PlaneKin: + def __init__(self, **kwargs): + self.cont = torch.zeros(6, dtype=torch.bool) + self.lo, self.hi = torch.full((6,), -10.0), torch.full((6,), 10.0) + + def gaps(self, q, cap): + return {'gm': q[:, :1].expand(-1, 49).clamp(max=cap)} + + traj = torch.zeros(1, 3, 6) + traj[0, :, 0] = torch.tensor([1.0, 1.0, -1.0]) + per = {'col': torch.tensor([1.0]), 'first_hit': torch.tensor([0.04]), + 'dls_it': torch.tensor([-1]), 'traj': traj} + with tempfile.TemporaryDirectory() as folder: + trace = Path(folder) / 'trace.pt' + torch.save({'tasks': [self.tasks[0]], 'batches': [({}, per, 1)]}, trace) + with patch('ml.eval.diag_collision.Kin', PlaneKin), contextlib.redirect_stdout(io.StringIO()): + result = replay_first_hits(trace) + self.assertEqual(result['reported_collision_tasks'], 1) + self.assertEqual(result['replayed_first_hits32'], 1) + self.assertEqual(result['replayed_first_hits64'], 1) + self.assertEqual(result['rows'][0]['gap64_mm'], [0.5, 0.5, 0.0, -1.0]) + self.assertEqual(result['rows'][0]['dense_gap64_min_mm'], -1.0) + + +class DlsSafetyTest(unittest.TestCase): + def test_rejected_refinement_retains_safe_candidate(self): + class ObstacleKin: + def clamp_box(self, q): + return q + + def gaps(self, q, cap): + return {'gm': torch.where(q[:, :1] > 0.3, -torch.ones_like(q[:, :1]), torch.ones_like(q[:, :1]))} + + task = {'q0': [0] * 6, 'T': [2, 0, 0]} + traj = torch.zeros(1, 2, 6) + traj[0, 1, 0] = 0.1 + ref = torch.zeros(1, 1, 6) + ref[0, 0, 0] = 1.0 + patches = { + 'features': lambda *args: torch.zeros(1, 83), + 'goal_err': lambda kin, q, T, R: (torch.full((q.shape[0],), 2.0), torch.zeros(q.shape[0])), + 'dls_steps': lambda *args, **kwargs: ref, + } + results = [] + with patch.multiple('ml.sim.closed_loop', **patches): + for gate in (0, 1): + _, per = rollout(None, ObstacleKin(), [task], 'cpu', K=1, safety=True, + max_sec=0.02, return_traj=True, use_sub=False, select='first', + oracle=TeacherOracle(traj), replan='empty', commit_n=1, + safety_partial=1, dls_safety=gate) + results.append(per) + self.assertEqual(results[0]['col'].item(), 1.0) + self.assertEqual(results[0]['dls_it'].item(), 0) + self.assertEqual(results[1]['col'].item(), 0.0) + self.assertEqual(results[1]['dls_it'].item(), -1) + self.assertAlmostEqual(results[1]['traj'][0, 1, 0].item(), 0.1) + + def test_feedback_is_reduced_only_after_full_rejection(self): + class ObstacleKin: + def clamp_box(self, q): + return q + + def gaps(self, q, cap): + return {'gm': torch.where(q[:, :1] > 0.3, -torch.ones_like(q[:, :1]), torch.ones_like(q[:, :1]))} + + task = {'q0': [0] * 6, 'T': [10, 0, 0]} + traj = torch.zeros(1, 17, 6) + traj[0, :, 0] = -0.2 * torch.arange(17) + ref = torch.zeros(1, 1, 6) + ref[0, 0, 0] = 1.0 + 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])), + 'dls_steps': lambda *args, **kwargs: ref, + } + results = [] + with patch.multiple('ml.sim.closed_loop', **patches): + for gain in (1.0, 0.5): + _, per = rollout(None, ObstacleKin(), [task], 'cpu', K=1, safety=True, + max_sec=0.04, return_traj=True, use_sub=False, select='first', + oracle=TeacherOracle(traj), replan='empty', commit_n=1, + safety_partial=1, fb_gain=0.6, fb_block_gain=gain) + results.append(per) + self.assertEqual(results[0]['brakes'].item(), 2.0) + self.assertEqual(results[1]['brakes'].item(), 1.0) + self.assertEqual(results[1]['traj'][0, 1, 0].item(), 0.0) + self.assertAlmostEqual(results[1]['traj'][0, 2, 0].item(), 0.1) + self.assertEqual(results[1]['col'].item(), 0.0) + + +if __name__ == '__main__': + unittest.main()