fix(closed_loop): 一致性打分错位一步(对齐 last[3]→last[4],原来每次重规划奖励比上一计划慢一步的候选;Python 闭环与 ik-policy.js 同步修正);rollout 加 select;diag_cl 删 F 段、加 G 段(1/4 段采样 × consist/value/first、4 段+安全层);本机执行清单更新
This commit is contained in:
+10
-5
@@ -65,8 +65,8 @@ def dls_steps(kin, q, T, R, n=3, scale=0.3, lam=0.5):
|
||||
|
||||
|
||||
@torch.no_grad()
|
||||
def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None, return_traj=False, use_sub=True, sub_override=None, qp0=None, steps=1, z_scale=1.0):
|
||||
"""use_sub=False:不用子目标(sf=0);sub_override=(subs [B,2,6] 绝对关节角, need [B] bool):冷启动时用给定途经点代替子目标头;qp0 [B,6]:起点的上一帧(默认 = q0,即静止起步)。steps:MeanFlow 采样段数;z_scale:噪声缩放(0 = 只取「均值」样本)。均为诊断用"""
|
||||
def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None, return_traj=False, use_sub=True, sub_override=None, qp0=None, steps=1, z_scale=1.0, select='consist'):
|
||||
"""use_sub=False:不用子目标(sf=0);sub_override=(subs [B,2,6] 绝对关节角, need [B] bool):冷启动时用给定途经点代替子目标头;qp0 [B,6]:起点的上一帧(默认 = q0,即静止起步)。steps:MeanFlow 采样段数;z_scale:噪声缩放(0 = 只取「均值」样本);select:consist(部署口径:冷启动价值前瞻、之后一致性)/ value(每次都价值前瞻)/ first(不选,取第 0 条)。均为诊断用"""
|
||||
g = torch.Generator(device=device); g.manual_seed(seed)
|
||||
q0, T, R, ex, tr = tasks_to_tensors(tasks, device)
|
||||
B = q0.shape[0]
|
||||
@@ -113,7 +113,9 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None,
|
||||
if not use_sub: need = torch.zeros_like(need)
|
||||
nsub = torch.where(need, torch.full_like(nsub, 2), torch.zeros_like(nsub)); sub_t.fill_(tnow)
|
||||
fut = qe[:, None, None] + torch.cumsum(inc[:, :, r:], 2) # [B,K,H-r,6]
|
||||
if last is None:
|
||||
if select == 'first':
|
||||
score = torch.zeros(B, K, device=device); score[:, 0] = 1
|
||||
elif last is None or select == 'value':
|
||||
# 短前瞻:候选末态的价值
|
||||
qa = fut[:, :, -1].reshape(B * K, 6); qb = fut[:, :, -2].reshape(B * K, 6)
|
||||
f2 = features(kin, qa, qb, T.repeat_interleave(K, 0), R.repeat_interleave(K, 0), torch.zeros(B * K, 3, 6, device=device),
|
||||
@@ -121,9 +123,12 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None,
|
||||
v2, _ = model.heads(model.encode(f2))
|
||||
score = -v2.view(B, K)
|
||||
else:
|
||||
n = min(fut.shape[2], last.shape[1] - 3)
|
||||
# 2026-10-19 修错位:last[i] = 上一计划从 qe_old 走 i 步后的位置;提交 3 步后 qe_new = last[3],
|
||||
# 新候选 fut[0] 是 qe_new 再走 1 步 = 对应 last[4]。原来对齐 last[3],等于奖励「比上一计划慢一步」的候选,
|
||||
# 每次重规划都挑最拖的那条,闭环越走越慢。JS 部署(ik-policy.js)同一处同样修正。
|
||||
n = min(fut.shape[2], last.shape[1] - 4)
|
||||
w = 0.85 ** torch.arange(n, device=device, dtype=torch.float32)
|
||||
d2 = (((fut[:, :, :n] - last[:, None, 3:3 + n]) / step) ** 2).sum(-1)
|
||||
d2 = (((fut[:, :, :n] - last[:, None, 4:4 + n]) / step) ** 2).sum(-1)
|
||||
score = -(d2 * w).sum(-1)
|
||||
order = score.argsort(-1, descending=True)
|
||||
pick = order[:, 0]
|
||||
|
||||
Reference in New Issue
Block a user