feat+test: 换向率 reverse_rate 进闭环指标(按轴方向翻转/活动步,站住不重置),带正对照单测
This commit is contained in:
@@ -202,6 +202,7 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None,
|
||||
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)
|
||||
rev = torch.zeros(B, device=device); nact = torch.zeros(B, device=device); prev_sgn = torch.zeros(B, 6, device=device)
|
||||
traj = [q.clone()] if return_traj else None
|
||||
plans = {k: [] for k in ('it', 'r', '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°
|
||||
@@ -478,6 +479,10 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None,
|
||||
vratio = torch.maximum(vratio, (exec_d / step).max(-1).values * act_) # 实际速度 / VMAX
|
||||
aratio = torch.maximum(aratio, ((exec_d - prev_exec) / step).max(-1).values * act_) # 每控制步的速度变化 / VMAX
|
||||
prev_exec = exec_d
|
||||
sgn = torch.sign(q - qp) # 换向率:轮次 39 的分解证明"行程超出 2.4 倍"主要是同一轴来回抖,不是绕远
|
||||
rev += ((sgn != 0) & (prev_sgn != 0) & (sgn == -prev_sgn)).float().sum(-1) * act_
|
||||
prev_sgn = torch.where(sgn != 0, sgn, prev_sgn) # 零步不重置方向(站住再走同向不算换向)
|
||||
nact += act_
|
||||
e, oe = goal_err(kin, q, T, R)
|
||||
if it + 1 == k2s: e2 = e.clone()
|
||||
newly = (~done) & (e < 1) & (oe < 0.5)
|
||||
@@ -506,7 +511,8 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None,
|
||||
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(), '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()}
|
||||
'goal_ratio_2s': ratio2.cpu(), 'speed_ratio': vratio.cpu(), 'accel_ratio': aratio.cpu(),
|
||||
'reverse_rate': (rev / nact.clamp_min(1.0)).cpu()}
|
||||
if return_traj:
|
||||
per['traj'] = torch.stack(traj, 1).cpu()
|
||||
per['plans'] = {k: torch.tensor(v) if k in ('it', 'r') else torch.stack(v, 1).cpu() for k, v in plans.items()}
|
||||
|
||||
Reference in New Issue
Block a user