feat(abs-target): 方案 B 初版——Y 改 16 步绝对目标角(/180),训练/评估/rollout/ckpt 适配;修复前缀噪声增广对绝对目标的标签污染(eps 直接加到绝对位置=18°);实测一步生成误差 31°、闭环 0%,此配方不成立
This commit is contained in:
+26
-10
@@ -153,7 +153,10 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None,
|
||||
B = q0.shape[0]
|
||||
step = torch.tensor(STEP, device=device)
|
||||
act_scale = oracle.act_scale if oracle is not None else float(getattr(model, 'act_scale', 1.0))
|
||||
astep = step * act_scale # 动作反归一化:每单位 act 对应的度数
|
||||
# 方案 B(2026-10-19):abs_target 模型输出 16 步绝对目标角(/180 归一化),执行时直接朝目标走;
|
||||
# oracle / 旧 ckpt 仍是增量口径
|
||||
abs_t = oracle is None and bool(getattr(model, 'abs_target', False))
|
||||
astep = torch.full((6,), 180.0, device=device) if abs_t else step * act_scale
|
||||
has_ref = torch.isfinite(ex)
|
||||
limit = time_limit(ex, max_sec)
|
||||
nmax = int(math.ceil(limit.max().item() / DT))
|
||||
@@ -177,8 +180,13 @@ 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)
|
||||
if abs_t and r > 0: # queue 存绝对目标:前缀特征仍用增量(度)
|
||||
prevq = torch.cat([q[:, None], queue[:, :r - 1]], 1)
|
||||
pre[:, :r] = queue[:, :r] - prevq
|
||||
else:
|
||||
pre[:, :r] = queue[:, :r]
|
||||
qe = (queue[:, -1] if r > 0 else q) if abs_t else q + queue.sum(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)
|
||||
@@ -189,7 +197,7 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None,
|
||||
else:
|
||||
z = torch.randn(B * K, H, 6, device=device, generator=g) * z_scale
|
||||
act, val, sub = model.sample_k(feat, K, z, steps=steps)
|
||||
inc = act.float().clamp(-1, 1) * astep
|
||||
inc = act.float().clamp(-1, 1) * astep # 增量口径=步长增量;B 口径=绝对目标角
|
||||
if cold:
|
||||
need = sub[:, 12] > 0.5
|
||||
subs = q[:, None] + sub[:, :12].float().view(B, 2, 6) * 180
|
||||
@@ -203,7 +211,7 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None,
|
||||
nsub = torch.where(need, torch.full_like(nsub, 2), torch.zeros_like(nsub))
|
||||
if not use_sub: nsub = torch.zeros_like(nsub)
|
||||
sub_t.fill_(tnow); sub_prev = q.clone()
|
||||
fut = qe[:, None, None] + torch.cumsum(inc[:, :, r:], 2) # [B,K,H-r,6]
|
||||
fut = inc[:, :, r:] if abs_t else qe[:, None, None] + torch.cumsum(inc[:, :, r:], 2) # [B,K,H-r,6] 绝对位置
|
||||
if select == 'first':
|
||||
score = torch.zeros(B, K, device=device); score[:, 0] = 1
|
||||
elif cold or select == 'value':
|
||||
@@ -231,8 +239,12 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None,
|
||||
chosen = torch.zeros(B, 3, 6, device=device); chosen_k = pick.clone()
|
||||
for kk in range(K):
|
||||
ck = inc[ar, order[:, kk], r:r + 3]
|
||||
pts = qe[:, None] + torch.cumsum(ck, 1)
|
||||
mids = torch.cat([qe[:, None], pts[:, :-1]], 1) + 0.5 * ck
|
||||
if abs_t:
|
||||
pts = ck
|
||||
mids = 0.5 * (torch.cat([qe[:, None], ck[:, :-1]], 1) + ck)
|
||||
else:
|
||||
pts = qe[:, None] + torch.cumsum(ck, 1)
|
||||
mids = torch.cat([qe[:, None], pts[:, :-1]], 1) + 0.5 * ck
|
||||
allq = torch.cat([pts, mids], 1).reshape(-1, 6)
|
||||
okk = (kin.gaps(allq, 0.5)['gm'].min(-1).values > 0).view(B, 6).all(-1)
|
||||
take = okk & ~ok_any
|
||||
@@ -242,6 +254,7 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None,
|
||||
pick = chosen_k; committed = ok_any
|
||||
if near.any():
|
||||
ref = dls_steps(kin, qe, T, R)
|
||||
if abs_t: ref = qe[:, None] + torch.cumsum(ref, 1) # 增量 → 绝对目标
|
||||
commit = torch.where(near[:, None, None], ref, commit)
|
||||
dls_it = torch.where(near & (dls_it < 0), torch.full_like(dls_it, it), dls_it)
|
||||
committed = committed & ~near
|
||||
@@ -250,9 +263,12 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None,
|
||||
queue = torch.cat([queue, commit], 1)
|
||||
cold = False
|
||||
d = queue[:, 0]; queue = queue[:, 1:]
|
||||
d = torch.where(done[:, None], torch.zeros_like(d), d)
|
||||
qp = q; q = kin.clamp_box(q + d)
|
||||
travel += d.abs().sum(-1)
|
||||
if abs_t: # 绝对目标:直接走到目标;到位的题原地冻结
|
||||
qp = q; q = kin.clamp_box(torch.where(done[:, None], q, d)); travel += (q - qp).abs().sum(-1)
|
||||
else:
|
||||
d = torch.where(done[:, None], torch.zeros_like(d), d)
|
||||
qp = q; q = kin.clamp_box(q + d)
|
||||
travel += d.abs().sum(-1)
|
||||
j1_acc = j1_acc + d[:, 0]; j1_max = torch.maximum(j1_max, j1_acc); j1_min = torch.minimum(j1_min, j1_acc)
|
||||
j1_first = torch.where((j1_first == 0) & (j1_acc.abs() >= FIRST_DEG), torch.sign(j1_acc), j1_first)
|
||||
if it + 1 == n_start: moved_start = travel.clone()
|
||||
|
||||
Reference in New Issue
Block a user