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)
|
||||
|
||||
Reference in New Issue
Block a user