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

tests/test_eval_cl.py

@@ -781,5 +781,69 @@ class BackoffTest(unittest.TestCase):
             self.run1(acts, 1, cert_batch=0)
 
 
+class TravelPenaltySelectTest(unittest.TestCase):
+    """#17b：候选打分加"归一化行程"惩罚（§9.2 要行程≤规划器×1.10，现在 2.4 倍没人管）。
+    两条候选末态相同（goal_err 打成常数）、累计位移相同，但一条来回抖、一条匀速 ⇒ 只可能靠行程分开。"""
+    class StubModel:
+        act_scale = 1.0
+
+        def __init__(self, act):
+            self.act = act
+
+        def sample_k(self, feat, K, z, steps=1):
+            B = feat.shape[0]
+            return (self.act.expand(B, K, 16, 6).clone(), torch.zeros(B, device=feat.device),
+                    torch.zeros(B, 13, device=feat.device))
+
+    def run1(self, len_w):
+        class SafeKin:
+            def clamp_box(self, q):
+                return q
+
+            def gaps(self, q, cap):
+                return {'gm': torch.full((len(q), 49), 1.0, device=q.device)}
+
+        zig = ([[0.9, 0, 0, 0, 0, 0], [-0.45, 0, 0, 0, 0, 0]] * 8)          # 16 步，合计 3.6、行程 10.8
+        smooth = ([[0.225, 0, 0, 0, 0, 0]] * 16)                            # 合计 3.6、行程 3.6
+        acts = torch.tensor([zig, smooth], dtype=torch.float32)             # [K=2,16,6]
+        tasks = [{'q0': [0] * 6, 'T': [10, 0, 0]}]
+        with patch('ml.sim.closed_loop.features', return_value=torch.zeros(1, 83)), \
+             patch('ml.sim.closed_loop.goal_err',
+                   side_effect=lambda kin, q, T, R: (torch.full((q.shape[0],), 10.0), torch.zeros(q.shape[0]))):
+            _, per = rollout(self.StubModel(acts), SafeKin(), tasks, 'cpu', K=2, safety=True, max_sec=0.06,
+                             return_traj=True, use_sub=False, select='ep_len', replan='empty', commit_n=2,
+                             safety_partial=1, len_w=len_w)
+        return per
+
+    def test_penalty_prefers_the_shorter_path(self):
+        p0 = self.run1(0.0)      # 不加惩罚：同分 ⇒ 稳定排序取第 0 条（来回抖那条）
+        self.assertAlmostEqual(p0['traj'][0, 1, 0].item(), 0.9 * 1.2, places=5)
+        p1 = self.run1(1.0)      # 加惩罚：行程 10.8 vs 3.6 ⇒ 选匀速那条
+        self.assertAlmostEqual(p1['traj'][0, 1, 0].item(), 0.225 * 1.2, places=5)
+
+    def test_zero_weight_is_the_old_endpoint_path(self):
+        """len_w=0 且 select='ep_len' 必须与 select='endpoint' 逐位相同（不许悄悄改默认行为）"""
+        class SafeKin:
+            def clamp_box(self, q):
+                return q
+
+            def gaps(self, q, cap):
+                return {'gm': torch.full((len(q), 49), 1.0, device=q.device)}
+
+        zig = ([[0.9, 0, 0, 0, 0, 0], [-0.45, 0, 0, 0, 0, 0]] * 8)
+        acts = torch.tensor([zig, zig], dtype=torch.float32)
+        tasks = [{'q0': [0] * 6, 'T': [10, 0, 0]}]
+        out = []
+        for sel in ('endpoint', 'ep_len'):
+            with patch('ml.sim.closed_loop.features', return_value=torch.zeros(1, 83)), \
+                 patch('ml.sim.closed_loop.goal_err',
+                       side_effect=lambda kin, q, T, R: (torch.full((q.shape[0],), 10.0), torch.zeros(q.shape[0]))):
+                _, per = rollout(self.StubModel(acts), SafeKin(), tasks, 'cpu', K=2, safety=True, max_sec=0.06,
+                                 return_traj=True, use_sub=False, select=sel, replan='empty', commit_n=2,
+                                 safety_partial=1, len_w=0.0)
+            out.append(per)
+        torch.testing.assert_close(out[0]['traj'], out[1]['traj'], rtol=0, atol=0)
+
+
 if __name__ == '__main__':
     unittest.main()