diff --git a/ml/eval/eval_cl.py b/ml/eval/eval_cl.py index 1a7bb26..2a4fd73 100644 --- a/ml/eval/eval_cl.py +++ b/ml/eval/eval_cl.py @@ -121,6 +121,7 @@ def main(): ap.add_argument('--len_w', type=float, default=0.0, help="ep_len=罚累计归一行程；ep_waste=罚(1−净/总)×当前误差；0 与 endpoint 逐位相同") ap.add_argument('--tie', type=int, default=0, help='1=等长安全前缀时按沿路最小间隙定夺（需 --cert_batch 1）') ap.add_argument('--pre_h', type=int, default=0, help="select=pre 的打分步长：0=commit_n（被执行前缀），>0=按绝对步数（#30 曲线）") + ap.add_argument('--pre_near', type=float, default=0.0, help="select=pre 的门控阈值(mm)：当前误差≤它时改用整条地平线打分（#31，0=关）") ap.add_argument('--backoff', type=int, default=0, help='全拒时把首选候选按 1/2、1/4…缩步重试的档数（需 --cert_batch 1）') ap.add_argument('--max_sec', type=float, default=None) ap.add_argument('--seed', type=int, default=0) @@ -148,7 +149,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, - inc_cap=a.inc_cap, gap_margin=a.gap_margin, cert_batch=a.cert_batch, backoff=a.backoff, len_w=a.len_w, tie=a.tie, pre_h=a.pre_h, return_traj=bool(a.trace)) + inc_cap=a.inc_cap, gap_margin=a.gap_margin, cert_batch=a.cert_batch, backoff=a.backoff, len_w=a.len_w, tie=a.tie, pre_h=a.pre_h, pre_near=a.pre_near, 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 7c37453..fd10afa 100644 --- a/ml/sim/closed_loop.py +++ b/ml/sim/closed_loop.py @@ -163,7 +163,7 @@ 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, - inc_cap=0, gap_margin=0.0, cert_batch=0, backoff=0, len_w=0.0, tie=0, pre_h=0, progress=None): + inc_cap=0, gap_margin=0.0, cert_batch=0, backoff=0, len_w=0.0, tie=0, pre_h=0, pre_near=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 条）。 @@ -297,6 +297,11 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None, qa_pre = fut[:, :, min(pre_h if pre_h > 0 else commit_n, fut.shape[2]) - 1].reshape(B * K, 6) e_pre, _ = goal_err(kin, qa_pre, T.repeat_interleave(K, 0), R.repeat_interleave(K, 0)) score = -e_pre.view(B, K) + if pre_near > 0: # #31：近终段换回整条地平线（终端代价）。轮次 48 的曲线说：时限收益来自短前瞻、 + # 精度代价却集中在 h≤4 ⇒ 两半可以分开要；代价是多一次 goal_err(B×K 点)，只在门控口径下发生。 + qa_full = fut[:, :, -1].reshape(B * K, 6) + e_full, _ = goal_err(kin, qa_full, T.repeat_interleave(K, 0), R.repeat_interleave(K, 0)) + score = torch.where((e <= pre_near)[:, None], -e_full.view(B, K), score) 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)) diff --git a/tests/test_eval_cl.py b/tests/test_eval_cl.py index 54b9e97..7760893 100644 --- a/tests/test_eval_cl.py +++ b/tests/test_eval_cl.py @@ -987,7 +987,7 @@ class PreHorizonTest(unittest.TestCase): def gaps(self, q, cap): return {'gm': torch.ones(q.shape[0], 49)} - def run1(self, select, commit_n=2, pre_h=0): + def run1(self, select, commit_n=2, pre_h=0, pre_near=0.0): # 候选0：前 2 步各 +0.2（前缀 0.4），随后 14 步 −0.4/14 ⇒ 地平线末端回到 0 # 候选1：16 步各 +0.05 ⇒ 前缀只有 0.1，但地平线末端 0.8 c0 = [0.2, 0.2] + [-(0.4 / 14.0)] * 14 @@ -1000,7 +1000,7 @@ class PreHorizonTest(unittest.TestCase): patch('ml.sim.closed_loop.goal_err', side_effect=err): _, per = rollout(self.StubModel(acts), self.NearKin(), [{'q0': [0] * 6, 'T': [1, 0, 0]}], 'cpu', K=2, safety=False, max_sec=0.04, return_traj=True, use_sub=False, - select=select, replan='empty', commit_n=commit_n, pre_h=pre_h) + select=select, replan='empty', commit_n=commit_n, pre_h=pre_h, pre_near=pre_near) return per def test_pre_and_endpoint_pick_opposite(self): @@ -1024,6 +1024,14 @@ class PreHorizonTest(unittest.TestCase): b = self.run1('pre', commit_n=2, pre_h=2) torch.testing.assert_close(a['traj'], b['traj'], rtol=0, atol=0) + def test_pre_near_gate_flips_only_inside_the_threshold(self): + """#31 的门控：夹具里"当前误差"= (0.9 − q0)·1000，起点 q0=0 ⇒ e=900。 + pre_near=950 ⇒ 第一次规划就在门内 ⇒ 必须退回 endpoint 的选择（0.06）； + pre_near=100 或 0 ⇒ 门外 ⇒ 保持 pre 的选择（0.24）。""" + self.assertAlmostEqual(float(self.run1('pre', pre_near=950.0)['traj'][0, 1, 0]), 0.05 * 1.2, places=5) + self.assertAlmostEqual(float(self.run1('pre', pre_near=100.0)['traj'][0, 1, 0]), 0.2 * 1.2, places=5) + self.assertAlmostEqual(float(self.run1('pre', pre_near=0.0)['traj'][0, 1, 0]), 0.2 * 1.2, places=5) + def test_pre_equals_endpoint_when_commit_is_the_whole_horizon(self): """不变式：commit_n=16 = 整条地平线 ⇒ pre 的索引落到末端，必须与 endpoint 选同一条。""" p = self.run1('pre', commit_n=16)