fix+exp: 轮次30——统一认证与执行限位，到达57.4%不变；加密回放仍检出段内碰撞
This commit is contained in:
@@ -7,7 +7,7 @@
|
||||
口径与 closed_loop.rollout 一致：每步同时检查当前状态 q 与相邻状态中点 mid 的间隙（cap=0.5 mm）；
|
||||
间隙组对索引 = gA*7+gB（gA<gB，g 为连杆组 0..5 → J1..J6）。
|
||||
"""
|
||||
import argparse, json, math, os, sys
|
||||
import argparse, json, math, os, sys, time
|
||||
from collections import Counter
|
||||
|
||||
import torch
|
||||
@@ -54,6 +54,25 @@ def replay_first_hits(path, out=None):
|
||||
'previous': prev.tolist(), 'endpoint': q.tolist(),
|
||||
'dense_nsegments': nseg, 'dense_gap64_min_mm': float(dense_gap.min()),
|
||||
}
|
||||
if 'plans' in per:
|
||||
plans = per['plans']
|
||||
pi = int(torch.searchsorted(plans['it'], it - 1, right=True)) - 1
|
||||
step = it - 1 - int(plans['it'][pi])
|
||||
ncommit = plans['commit'].shape[2]
|
||||
command = plans['commit'][b, pi, step]
|
||||
cp, cm = plans['points'][b, pi, step], plans['midpoints'][b, pi, step]
|
||||
checked = bool(plans['certified'][b, pi, step])
|
||||
predicted = k32.clamp_box((prev + command)[None])[0]
|
||||
row['certificate'] = {
|
||||
'plan_start_it': int(plans['it'][pi]), 'step': step, 'checked': checked,
|
||||
'endpoint': cp.tolist() if checked else None,
|
||||
'midpoint': cm.tolist() if checked else None,
|
||||
'recorded_gap32_mm': plans['gaps'][b, pi, [step, ncommit + step]].tolist() if checked else None,
|
||||
'replayed_gap64_mm': k64.gaps(torch.stack([cp, cm]).double(), 0.5)['gm'].min(-1).values.tolist() if checked else None,
|
||||
'command_delta': command.tolist(),
|
||||
'clamped_reconstruction_max_abs_deg': float((predicted - q).abs().max()),
|
||||
'certified_vs_executed_max_abs_deg': float((cp - q).abs().max()) if checked else None,
|
||||
}
|
||||
rows.append(row)
|
||||
print('task %d first_hit %.2fs gap32 %s gap64 %s boundary %s' %
|
||||
(row['i'], row['first_hit_s'], row['gap32_mm'], row['gap64_mm'], row['boundary_joints']), flush=True)
|
||||
@@ -73,10 +92,73 @@ def replay_first_hits(path, out=None):
|
||||
return result
|
||||
|
||||
|
||||
@torch.no_grad()
|
||||
def replay_dense(path, indices, max_step_deg=0.05, device='cpu', out=None):
|
||||
saved = torch.load(path, map_location='cpu', weights_only=True)
|
||||
kin = Kin(device=device, dtype=torch.float64)
|
||||
if saved.get('kin_fp') != kin.fp:
|
||||
raise ValueError('trace/kin fingerprint mismatch')
|
||||
if not indices or min(indices) < 0 or max(indices) >= len(saved['tasks']) or max_step_deg <= 0:
|
||||
raise ValueError('valid task indices and positive max_step_deg required')
|
||||
started = time.perf_counter()
|
||||
rows, offset = [], 0
|
||||
for _, per, count in saved['batches']:
|
||||
for index in indices:
|
||||
if not offset <= index < offset + count:
|
||||
continue
|
||||
b = index - offset
|
||||
nstep = min(per['traj'].shape[1] - 1, math.floor((float(per['limit'][b]) + 1e-6) / DT))
|
||||
if torch.isfinite(per['t_done'][b]):
|
||||
nstep = min(nstep, round(float(per['t_done'][b]) / DT))
|
||||
trajectory = per['traj'][b, :nstep + 1].double()
|
||||
points, segment_ids, fractions = [trajectory[:1]], [torch.tensor([-1])], [torch.zeros(1)]
|
||||
for it in range(nstep):
|
||||
prev, end = trajectory[it:it + 2]
|
||||
nseg = max(2, 2 * math.ceil(float((end - prev).abs().max()) / (2 * max_step_deg)))
|
||||
alpha = torch.arange(1, nseg + 1, dtype=torch.float64) / nseg
|
||||
points.append(prev + alpha[:, None] * (end - prev))
|
||||
segment_ids.append(torch.full((nseg,), it))
|
||||
fractions.append(alpha)
|
||||
points = torch.cat(points)
|
||||
segments, alpha = torch.cat(segment_ids), torch.cat(fractions)
|
||||
print('dense task %d steps %d points %d start' % (index, nstep, len(points)), flush=True)
|
||||
gaps = []
|
||||
for start in range(0, len(points), 64):
|
||||
gaps.append(kin.gaps(points[start:start + 64].to(device), 0.5)['gm'].min(-1).values.cpu())
|
||||
if (start // 64 + 1) % 64 == 0:
|
||||
print('dense task %d points %d/%d wall %.1fs' %
|
||||
(index, min(start + 64, len(points)), len(points), time.perf_counter() - started), flush=True)
|
||||
gaps = torch.cat(gaps)
|
||||
hit = gaps <= 0
|
||||
first = int(hit.nonzero()[0]) if hit.any() else None
|
||||
rows.append({
|
||||
'i': index, 'task': saved['tasks'][index], 'steps': nstep, 'sampled_points': len(points),
|
||||
'initial_gap64_mm': float(gaps[0]), 'min_gap64_mm': float(gaps.min()),
|
||||
'collision_segments': int(segments[hit & (segments >= 0)].unique().numel()),
|
||||
'first_collision_s': max(0.0, float(segments[first] + alpha[first]) * DT) if first is not None else None,
|
||||
})
|
||||
print('dense task %d min_gap %.9f mm collision_segments %d' %
|
||||
(index, rows[-1]['min_gap64_mm'], rows[-1]['collision_segments']), flush=True)
|
||||
offset += count
|
||||
result = {
|
||||
'trace': str(path), 'tasks_sha256': saved.get('tasks_sha256'), 'kin_fp': kin.fp,
|
||||
'indices': indices, 'max_step_deg': max_step_deg, 'dtype': 'float64', 'device': device,
|
||||
'scope': 'Executed trajectory through each task deadline or arrival, including initial state; finite sampling, not continuous certification.',
|
||||
'collision_tasks': sum(r['min_gap64_mm'] <= 0 for r in rows), 'rows': rows,
|
||||
'audit_wall_s': time.perf_counter() - started,
|
||||
}
|
||||
if out:
|
||||
with open(out, 'w', encoding='utf-8') as f:
|
||||
json.dump(result, f, indent=2, allow_nan=False)
|
||||
return result
|
||||
|
||||
|
||||
def main():
|
||||
ap = argparse.ArgumentParser()
|
||||
ap.add_argument('--ckpt')
|
||||
ap.add_argument('--trace')
|
||||
ap.add_argument('--dense_indices', help='逗号分隔的任务下标；回放实际轨迹，不依赖原 col 标记')
|
||||
ap.add_argument('--max_step_deg', type=float, default=0.05)
|
||||
ap.add_argument('--samples', default='ml/data/samples')
|
||||
ap.add_argument('--split', default='val')
|
||||
ap.add_argument('--n', type=int, default=512)
|
||||
@@ -88,8 +170,13 @@ def main():
|
||||
ap.add_argument('--out', default=None)
|
||||
a = ap.parse_args()
|
||||
if a.trace:
|
||||
replay_first_hits(a.trace, a.out)
|
||||
if a.dense_indices:
|
||||
replay_dense(a.trace, [int(i) for i in a.dense_indices.split(',')], a.max_step_deg, a.device, a.out)
|
||||
else:
|
||||
replay_first_hits(a.trace, a.out)
|
||||
return
|
||||
if a.dense_indices:
|
||||
ap.error('--dense_indices requires --trace')
|
||||
if not a.ckpt:
|
||||
ap.error('--ckpt or --trace is required')
|
||||
|
||||
|
||||
+1
-1
@@ -20,7 +20,7 @@ def run(model, kin, tasks, device, bs=256, **kw):
|
||||
r, p = rollout(model, kin, tasks[i:i + bs], device, progress=progress, **kw)
|
||||
per.append((r, p, len(tasks[i:i + bs])))
|
||||
print('batch %d done wall %.1fs' % (i // bs + 1, time.perf_counter() - t0), flush=True)
|
||||
flat = {k: torch.cat([p[k] for _, p, _ in per]) for k in per[0][1] if k != 'traj'}
|
||||
flat = {k: torch.cat([p[k] for _, p, _ in per]) for k in per[0][1] if k not in ('traj', 'plans')}
|
||||
_, _, _, ex, tr = tasks_to_tensors(tasks, 'cpu')
|
||||
ok, col = flat['ok'], flat['col'] > 0
|
||||
has_ref = torch.isfinite(ex)
|
||||
|
||||
+69
-25
@@ -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
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user