cbb/Robot_Arm
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

exp(dagger-deploy): 部分安全前缀（执行到首个不安全步）——刹停 27.4→8.6、到达 38.3%、碰撞 1.6%；新增 commit_n 参数（每步规划实验）；更正 replan=empty 实为 r=0 规划非每步

Browse Source
This commit is contained in:
cbb
2026-10-20 06:26:31 +08:00
parent de009bd516
commit 76ac563d21
2 changed files with 75 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ml/sim/closed_loop.py
+28 -10
View File
@@ -142,7 +142,7 @@ class TeacherOracle:
@torch.no_grad()
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):
fb_gain=0.0, fb_scale=0.0, fb_near=0.2, fb_fallback=0.0, safety_partial=0, commit_n=3):
"""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 条）。
@@ -227,34 +227,52 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None,
# 新候选 fut[0] 是 qe_new 再走 1 步 = 对应 last[4]。JS 部署（ik-policy.js）同一处同样修正。
n = min(fut.shape[2], last.shape[1] - 4)
w = 0.85 ** torch.arange(n, device=device, dtype=torch.float32)
d2 = (((fut[:, :, :n] - last[:, None, 4:4 + n]) / step) ** 2).sum(-1)
d2 = (((fut[:, :, :n] - last[:, None, commit_n + 1:commit_n + 1 + n]) / step) ** 2).sum(-1)
# 2026-10-19（审查 review_closedloop F5）：上一次是刹停或 DLS 接管的行没有「已提交计划」，
# 与 JS（lastPlan = null → 一致性项缺席、各候选同分 → 取第 0 条）一致：同分，稳定排序取第 0 条
score = torch.where(last_ok[:, None], -(d2 * w).sum(-1), torch.zeros_like(d2[..., 0]))
order = torch.sort(score, dim=-1, descending=True, stable=True).indices
pick = order[:, 0]
commit = inc[ar, pick, r:r + 3]
commit = inc[ar, pick, r:r + commit_n]
committed = torch.ones(B, dtype=torch.bool, device=device)
if safety:
ok_any = torch.zeros(B, dtype=torch.bool, device=device)
chosen = torch.zeros(B, 3, 6, device=device); chosen_k = pick.clone()
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()
for kk in range(K):
ck = inc[ar, order[:, kk], r:r + 3]
ck = inc[ar, order[:, kk], r:r + commit_n]
pts = qe[:, None] + torch.cumsum(ck, 1)
mids = torch.cat([qe[:, None], pts[:, :-1]], 1) + 0.5 * ck
allq = torch.cat([pts, mids], 1).reshape(-1, 6)
okk = (kin.gaps(allq, 0.5)['gm'].min(-1).values > 0).view(B, 6).all(-1)
ok6 = (kin.gaps(allq, 0.5)['gm'].min(-1).values > 0).view(B, 6)
okstep = ok6[:, :commit_n] & ok6[:, commit_n:] # 每步：步点与其中点都安全
plen = torch.cumprod(okstep.long(), 1).sum(1) # 安全前缀长度 0..3
take = plen > maxlen
chosen = torch.where(take[:, None, None], ck, chosen); chosen_k = torch.where(take, order[:, kk], chosen_k)
maxlen = torch.maximum(maxlen, plen)
msk = (torch.arange(commit_n, device=device)[None] < maxlen[:, None])
commit = torch.where(msk[..., None], chosen, torch.zeros_like(chosen))
brakes += (maxlen == 0).float()
pick = chosen_k; committed = maxlen > 0
elif safety:
ok_any = torch.zeros(B, dtype=torch.bool, device=device)
chosen = torch.zeros(B, commit_n, 6, device=device); chosen_k = pick.clone()
for kk in range(K):
ck = inc[ar, order[:, kk], r:r + commit_n]
pts = qe[:, None] + torch.cumsum(ck, 1)
mids = torch.cat([qe[:, None], pts[:, :-1]], 1) + 0.5 * ck
allq = torch.cat([pts, mids], 1).reshape(-1, 6)
okk = (kin.gaps(allq, 0.5)['gm'].min(-1).values > 0).view(B, 2 * commit_n).all(-1)
take = okk & ~ok_any
chosen = torch.where(take[:, None, None], ck, chosen); chosen_k = torch.where(take, order[:, kk], chosen_k); ok_any |= okk
commit = torch.where(ok_any[:, None, None], chosen, torch.zeros_like(chosen))
if fb_gain > 0 and fb_fallback > 0: # 全拒时的兜底：半速朝目标走一步（须通过安全认证），避免原地停滞
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, 3, -1), commit)
commit = torch.where((~ok_any & ok1)[:, None, None], fb1.expand(-1, commit_n, -1), commit)
brakes += (~ok_any).float()
pick = chosen_k; committed = ok_any
if near.any():
ref = dls_steps(kin, qe, T, R)
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