feat+exp: 轮次39——候选档位开关 --z_scale 与行程惩罚 select=ep_len/--len_w（零权重与 endpoint 逐位相同，2 条机制单测）

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, progress=None):
+            inc_cap=0, gap_margin=0.0, cert_batch=0, backoff=0, len_w=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 条）。
@@ -174,6 +174,8 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None,
     gap_margin>0：安全层要求间隙大于该毫米余量而不是 >0；宽相位 cap 自动抬到 margin+0.5，否则 0.5 的截顶值会让余量门限恒假。
     backoff>0：全拒（连第 1 步都不安全）时不再站住，把首选候选按 1/2、1/4…（共 backoff 档）缩步一起送进同一次认证，
                取能通过的第 1 档执行；缩步候选不参与"前缀更长者优先"的比较，只在原候选全被拒时顶上。需 cert_batch=1。
+    select='ep_len' + len_w>0：在末态误差打分上再减 len_w·Σ|Δ|/STEP（每轴归一的候选累计行程）。§9.2 要行程≤规划器×1.10，
+               而实测行程比 2.4~2.6 没有任何机制在管；len_w=0 与 select='endpoint' 逐位相同（有回归断言）。
     两者默认值下行为与第32轮逐位相同。"""
     if backoff and not cert_batch: raise ValueError('backoff 目前只实现在 cert_batch=1 口径')
     g = torch.Generator(device=device); g.manual_seed(seed)
@@ -282,10 +284,13 @@ 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 in ('endpoint', 'adapt'):
+            elif select in ('endpoint', 'adapt', 'ep_len'):
                 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 == 'ep_len' and len_w > 0:   # 行程惩罚：每轴按 STEP 归一的累计路程。§9.2 要"行程≤规划器×1.10"，
+                    ln = ((fut[:, :, 1:] - fut[:, :, :-1]).abs() / step).sum(-1).sum(-1)   # 而现在 2.4 倍没人管
+                    score = score - len_w * ln
                 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)