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

fix+exp: 轮次30——统一认证与执行限位，到达57.4%不变；加密回放仍检出段内碰撞

Browse Source
This commit is contained in:
cbb
2026-10-20 17:26:20 +08:00
parent 9eaff7341f
commit 95da49193f
12 changed files with 2516 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ml/sim/closed_loop.py
+69 -25
View File
@@ -48,6 +48,16 @@ def goal_err(kin, q, T, R):
return e, oe
def execution_points(kin, q, increments):
points, midpoints = [], []
for delta in increments.unbind(1):
nxt = kin.clamp_box(q + delta)
midpoints.append(q + 0.5 * (nxt - q))
points.append(nxt)
q = nxt
return torch.stack(points, 1), torch.stack(midpoints, 1)
def dls_steps(kin, q, T, R, n=3, scale=0.3, lam=0.5, analytic=True):
"""局部 DLS 精修：n 步增量 [B,n,6]（analytic：几何雅可比，1 次 FK；否则 7 次批量有限差分）"""
step = torch.tensor(STEP, device=q.device)
@@ -181,6 +191,7 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None,
col = torch.zeros(B, device=device); travel = torch.zeros(B, device=device); brakes = torch.zeros(B, device=device)
first_hit = torch.full((B,), float('nan'), device=device); dls_it = torch.full((B,), -1, dtype=torch.long, device=device)
traj = [q.clone()] if return_traj else None
plans = {k: [] for k in ('it', 'qe', 'commit', 'points', 'midpoints', 'gaps', 'certified')} if return_traj else None
n_start = int(round(0.5 / DT)); moved_start = None # R2 起步失败：0.5 s 内关节总行程 < 1°
k2s = int(round(2.0 / DT)); e2 = None
e0, _ = goal_err(kin, q0, T, R)
@@ -192,8 +203,12 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None,
tnow = it * DT
if (queue.shape[1] <= 3) if replan == 'r3' else (queue.shape[1] == 0):
r = queue.shape[1]
pre = torch.zeros(B, 3, 6, device=device); pre[:, :r] = queue[:, :r]
qe = q + queue.sum(1)
pre = torch.zeros(B, 3, 6, device=device)
qe = q
if r:
prefix_points, _ = execution_points(kin, q, queue)
pre[:, :r] = prefix_points - torch.cat([q[:, None], prefix_points[:, :-1]], 1)
qe = prefix_points[:, -1]
e, oe = goal_err(kin, qe, T, R)
near = (e < 5) & ((R.abs().sum(-1) == 0) | (oe < 5))
subs, nsub, sub_prev, sub_t = sub_advance(kin, subs, nsub, sub_prev, sub_t, qe, tnow, sub_rule)
@@ -297,31 +312,34 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=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 return_traj:
cert_pts = torch.full((B, commit_n, 6), float('nan'), device=device)
cert_mids = cert_pts.clone()
cert_gaps = torch.full((B, 2 * commit_n), float('nan'), device=device)
certified = torch.zeros(B, commit_n, 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()
ntry = min(K_eff, safety_try) if safety_try > 0 else K_eff
for kk in range(ntry):
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
pts, mids = execution_points(kin, qe, ck)
allq = torch.cat([pts, mids], 1).reshape(-1, 6)
ok6 = (kin.gaps(allq, 0.5)['gm'].min(-1).values > 0).view(B, 2 * commit_n)
okstep = ok6[:, :commit_n] & ok6[:, commit_n:] # 每步：步点与其中点都安全
plen = torch.cumprod(okstep.long(), 1).sum(1) # 安全前缀长度 0..3
gaps6 = kin.gaps(allq, 0.5)['gm'].min(-1).values.view(B, 2 * commit_n)
ok6 = gaps6 > 0
okstep = ok6[:, :commit_n] & ok6[:, commit_n:]
plen = torch.cumprod(okstep.long(), 1).sum(1)
take = plen > maxlen
chosen = torch.where(take[:, None, None], ck, chosen); chosen_k = torch.where(take, order[:, kk], chosen_k)
if return_traj:
cert_pts = torch.where(take[:, None, None], pts, cert_pts)
cert_mids = torch.where(take[:, None, None], mids, cert_mids)
cert_gaps = torch.where(take[:, None], gaps6, cert_gaps)
maxlen = torch.maximum(maxlen, plen)
if B == 1 and int(maxlen[0]) >= commit_n: break # 实时单题：首个全安全候选即停（安全层 16 点→2-4 点）
msk = (torch.arange(commit_n, device=device)[None] < maxlen[:, None])
commit = torch.where(msk[..., 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
fbcom = torch.zeros(B, commit_n, 6, device=device); fbcom[:, 0] = fb1[:, 0] # 只走一步（重复会被第二步未经认证的位移带进碰撞）
mskf = (maxlen == 0) & ok1
commit = torch.where(mskf[:, None, None], fbcom, commit)
msk = msk | mskf[:, None]
if return_traj: certified = msk.clone()
brakes += (maxlen == 0).float(); braked = maxlen == 0
pick = chosen_k; committed = maxlen > 0
elif safety:
@@ -330,28 +348,48 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None,
ntry = min(K_eff, safety_try) if safety_try > 0 else K_eff
for kk in range(ntry):
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
pts, mids = execution_points(kin, qe, 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)
gaps6 = kin.gaps(allq, 0.5)['gm'].min(-1).values.view(B, 2 * commit_n)
okk = (gaps6 > 0).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
if return_traj:
cert_pts = torch.where(take[:, None, None], pts, cert_pts)
cert_mids = torch.where(take[:, None, None], mids, cert_mids)
cert_gaps = torch.where(take[:, None], gaps6, cert_gaps)
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, commit_n, -1), commit)
if return_traj: certified = ok_any[:, None].expand(-1, commit_n).clone()
brakes += (~ok_any).float(); braked = ~ok_any
pick = chosen_k; committed = ok_any
if safety and fb_gain > 0 and fb_fallback > 0:
fbcom = torch.zeros(B, commit_n, 6, device=device)
fbcom[:, :1] = dls_steps(kin, qe, T, R, n=1) * fb_fallback
pts, mids = execution_points(kin, qe, fbcom)
allq = torch.cat([pts, mids], 1).reshape(-1, 6)
gaps6 = kin.gaps(allq, 0.5)['gm'].min(-1).values.view(B, 2 * commit_n)
takef = braked & (gaps6 > 0).all(-1)
commit = torch.where(takef[:, None, None], fbcom, commit)
if return_traj:
cert_pts = torch.where(takef[:, None, None], pts, cert_pts)
cert_mids = torch.where(takef[:, None, None], mids, cert_mids)
cert_gaps = torch.where(takef[:, None], gaps6, cert_gaps)
certified = certified | takef[:, None]
if near.any():
ref = dls_steps(kin, qe, T, R, n=commit_n)
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
pts, mids = execution_points(kin, qe, 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)
gaps6 = kin.gaps(allq, 0.5)['gm'].min(-1).values.view(B, 2 * commit_n)
okd = (gaps6 > 0).all(-1)
take = near & okd
if return_traj:
cert_pts = torch.where(take[:, None, None], pts, cert_pts)
cert_mids = torch.where(take[:, None, None], mids, cert_mids)
cert_gaps = torch.where(take[:, None], gaps6, cert_gaps)
if return_traj:
certified = torch.where(take[:, None], torch.full_like(certified, bool(dls_safety and safety)), certified)
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
@@ -359,6 +397,10 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None,
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_brake = braked
if return_traj:
plans['it'].append(it + r)
for key, value in (('qe', qe), ('commit', commit), ('points', cert_pts), ('midpoints', cert_mids), ('gaps', cert_gaps), ('certified', certified)):
plans[key].append(value)
queue = torch.cat([queue, commit], 1)
cold = False
d = queue[:, 0]; queue = queue[:, 1:]
@@ -403,7 +445,9 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None,
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(),
'goal_ratio_2s': ratio2.cpu()}
if return_traj: per['traj'] = torch.stack(traj, 1).cpu()
if return_traj:
per['traj'] = torch.stack(traj, 1).cpu()
per['plans'] = {k: torch.tensor(v) if k == 'it' else torch.stack(v, 1).cpu() for k, v in plans.items()}
return res, per