exp: 轮次29——全拒后减半反馈到达55.1%，补齐无碰撞指标与首撞回放

512题无碰撞到达54.7%、碰撞1.8%；9次首撞均在J5限位且双精度复现。保留小样本消融，撤回未经实际调度验证的50Hz达标结论，附六项回归测试及完整配置结果。

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()