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

exp+feat: 轮次36(续)——时间账证明限幅后的损失是"站住"不是巡航；全拒缩步顶上(backoff，默认关)

Browse Source
This commit is contained in:
cbb
2026-10-20 21:25:47 +08:00
parent f23f6fc93f
commit 349814db7e
5 changed files with 224 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ml/eval/eval_cl.py
+2 -1
View File
@@ -93,6 +93,7 @@ def main():
ap.add_argument('--inc_cap', type=int, default=0, help='1=反馈叠加后再按每轴 VMAX·DT 限幅')
ap.add_argument('--gap_margin', type=float, default=0.0, help='安全层要求的间隙余量 mm（>0 时宽相位 cap 自动抬到 margin+0.5）')
ap.add_argument('--cert_batch', type=int, default=0, help='1=一次 gaps 调用覆盖全部候选（压最坏情况，典型路径略贵）')
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)
ap.add_argument('--dls', default='analytic', choices=['analytic', 'fd'])
@@ -119,7 +120,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, return_traj=bool(a.trace))
inc_cap=a.inc_cap, gap_margin=a.gap_margin, cert_batch=a.cert_batch, backoff=a.backoff, 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)
ml/sim/closed_loop.py
+32 -11
View File
@@ -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, progress=None):
inc_cap=0, gap_margin=0.0, cert_batch=0, backoff=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 条）。
@@ -172,7 +172,10 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None,
sub_rule：子目标推进规则，见 sub_advance。除默认值外均为诊断用。
inc_cap=1：把「策略动作 + 目标反馈」的合计增量再按每轴 VMAX·DT 限一次幅（默认关；关时反馈是在 ±1 限幅之后叠加的，第30轮实测最大 1.66×VMAX）。
gap_margin>0：安全层要求间隙大于该毫米余量而不是 >0；宽相位 cap 自动抬到 margin+0.5，否则 0.5 的截顶值会让余量门限恒假。
backoff>0：全拒（连第 1 步都不安全）时不再站住，把首选候选按 1/2、1/4…（共 backoff 档）缩步一起送进同一次认证，
取能通过的第 1 档执行；缩步候选不参与"前缀更长者优先"的比较，只在原候选全被拒时顶上。需 cert_batch=1。
两者默认值下行为与第32轮逐位相同。"""
if backoff and not cert_batch: raise ValueError('backoff 目前只实现在 cert_batch=1 口径')
g = torch.Generator(device=device); g.manual_seed(seed)
q0, T, R, ex, tr = tasks_to_tensors(tasks, device)
B = q0.shape[0]
@@ -193,6 +196,7 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None,
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)
col = torch.zeros(B, device=device); travel = torch.zeros(B, device=device); brakes = torch.zeros(B, device=device)
bo = torch.zeros(B, device=device) # 缩步顶上的次数（backoff>0 才会计）
first_hit = torch.full((B,), float('nan'), device=device); dls_it = torch.full((B,), -1, dtype=torch.long, device=device)
# 2026-10-20（轮次33）：速度/加速度按「实际执行位移」算，不看指令——第30轮证明反馈是在限幅之后叠加的
vratio = torch.zeros(B, device=device); aratio = torch.zeros(B, device=device); prev_exec = torch.zeros(B, 6, device=device)
@@ -330,18 +334,34 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None,
chosen = torch.zeros(B, commit_n, 6, device=device); chosen_k = pick.clone()
ntry = min(K_eff, safety_try) if safety_try > 0 else K_eff
if cert_batch: # 一次 gaps 覆盖全部候选：典型路径略贵，但把「前面候选被拒→串行调用 K 次」的最坏情况封成 1 次
cks = inc[ar[:, None], order[:, :ntry], r:r + commit_n].reshape(B * ntry, commit_n, 6)
pts, mids = execution_points(kin, qe.repeat_interleave(ntry, 0), cks)
gaps6 = kin.gaps(torch.cat([pts, mids], 1).reshape(-1, 6), gcap)['gm'].min(-1).values.view(B, ntry, 2 * commit_n)
plen = torch.cumprod(((gaps6 > gap_margin)[:, :, :commit_n] &
(gaps6 > gap_margin)[:, :, commit_n:]).long(), -1).sum(-1)
cks = inc[ar[:, None], order[:, :ntry], r:r + commit_n] # [B,ntry,cn,6]
if backoff: # 缩步档与正常候选并进同一次认证，不额外调用几何
sc = (0.5 ** torch.arange(1, backoff + 1, device=device, dtype=inc.dtype))[None, :, None, None]
cands = torch.cat([cks, inc[ar, order[:, 0], r:r + commit_n][:, None] * sc], 1)
else:
cands = cks
nc = cands.shape[1]
pts, mids = execution_points(kin, qe.repeat_interleave(nc, 0), cands.reshape(B * nc, commit_n, 6))
gaps6 = kin.gaps(torch.cat([pts, mids], 1).reshape(-1, 6), gcap)['gm'].min(-1).values.view(B, nc, 2 * commit_n)
stepok = (gaps6 > gap_margin)[:, :, :commit_n] & (gaps6 > gap_margin)[:, :, commit_n:]
pl_all = torch.cumprod(stepok.long(), -1).sum(-1) # [B,nc] 每条候选的安全前缀长度
plen = pl_all[:, :ntry]
maxlen = plen.max(-1).values
best = plen.argmax(-1) # 与串行同口径：严格更大者胜 ⇒ 首个最大值
chosen = cks.view(B, ntry, commit_n, 6)[ar, best]
chosen_k = order[ar, best]
if backoff: # 缩步候选只在全拒时顶上：不参与「前缀更长者优先」的比较，所以正常路径逐位不变
full0 = maxlen == 0
bb = pl_all[:, ntry:].argmax(-1) # 平手取缩得最少的（档 0 = 1/2）
mv = pl_all[ar, ntry + bb]
bo += (full0 & (mv > 0))
best = torch.where(full0, ntry + bb, best)
maxlen = torch.where(full0, mv, maxlen)
chosen_k = torch.where(full0, order[:, 0], order[ar, best.clamp(max=ntry - 1)])
else:
chosen_k = order[ar, best]
chosen = cands[ar, best]
if return_traj:
cert_pts = pts.view(B, ntry, commit_n, 6)[ar, best]
cert_mids = mids.view(B, ntry, commit_n, 6)[ar, best]
cert_pts = pts.view(B, nc, commit_n, 6)[ar, best]
cert_mids = mids.view(B, nc, commit_n, 6)[ar, best]
cert_gaps = gaps6[ar, best]
else:
for kk in range(ntry):
@@ -466,6 +486,7 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None,
'collision_rate': (col > 0).float().mean().item(), 'collision_steps_mean': col.mean().item(),
'travel_ratio_median': (travel[okr] / tr[okr].clamp(min=1)).median().item() if okr.any() else None,
'time_mean_s': t_done[ok].mean().item() if ok.any() else None, 'brakes_mean': brakes.mean().item(),
'backoff_mean': bo.mean().item(),
'start_fail': start_fail.float().mean().item(), 'goal_ratio_2s': ratio2.median().item(),
'speed_ratio_mean': vratio.mean().item(), 'over_vmax_rate': (vratio > 1.001).float().mean().item(),
'time_budget': 'teacher×2(≥3 s)' if bool(has_ref.all()) else ('fixed %.0f s' % (max_sec or NOREF_SEC) if not has_ref.any() else 'mixed')}
@@ -473,7 +494,7 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None,
# 首次方向；全程没偏离到 FIRST_DEG 的（基本不绕 J1）退回净方向
side = torch.where(j1_first != 0, j1_first, torch.where(j1_max >= -j1_min, torch.ones_like(j1_max), -torch.ones_like(j1_max)))
per = {'done': done.cpu(), 'ok': ok.cpu(), 't_done': t_done.cpu(), 'limit': limit.cpu(), 'col': col.cpu(), 'travel': travel.cpu(), 'err': e_end.cpu(),
'side': side.cpu(), 'start_fail': start_fail.cpu(), 'brakes': brakes.cpu(), 'first_hit': first_hit.cpu(), 'dls_it': dls_it.cpu(),
'side': side.cpu(), 'start_fail': start_fail.cpu(), 'brakes': brakes.cpu(), 'backoff': bo.cpu(), 'first_hit': first_hit.cpu(), 'dls_it': dls_it.cpu(),
'goal_ratio_2s': ratio2.cpu(), 'speed_ratio': vratio.cpu(), 'accel_ratio': aratio.cpu()}
if return_traj:
per['traj'] = torch.stack(traj, 1).cpu()