cbb/Robot_Arm
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

perf+exp(closed_loop): 解析雅可比 DLS（消除 float32 差分数值噪声）——commit3 到达 36.3→46.5%，commit2 49.2%（n=512），DLS n1 9.3→7.1ms

Browse Source 
- Kin.jac 几何雅可比，与 autograd 逐项一致（FD 自身 rel 误差 3.58%）
- dls_steps(analytic=True) 默认；每步少 6 次批量 FK
- 新增 ml/eval/eval_cl.py（部署口径小规模评估）与 check_dls.py（对拍+延迟）
- 积分反馈 fb_ki 脚手架（默认 0，未启用）
This commit is contained in:
cbb
2026-10-20 13:33:23 +08:00
parent 7e99b333ea
commit 46fe24365b
5 changed files with 197 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ml/sim/closed_loop.py
+27 -12
View File
@@ -48,26 +48,33 @@ def goal_err(kin, q, T, R):
return e, oe
def dls_steps(kin, q, T, R, n=3, scale=0.3, lam=0.5):
"""局部 DLS 精修：n 步增量 [B,n,6]"""
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)
B = q.shape[0]
pose = (R.abs().sum(-1) > 0).float()
Rt = R.view(-1, 3, 3)
def err(x, Tb, Rtb, poseb):
xf = kin.cum_xf(x); p = kin.fk(x, xf); Rq = kin.orient(x, xf)
def err(x, Tb, Rtb, poseb, xf=None):
xf = xf if xf is not None else kin.cum_xf(x)
p = kin.fk(x, xf); Rq = kin.orient(x, xf)
M = Rtb @ Rq.transpose(1, 2)
rv = 0.5 * torch.stack([M[:, 2, 1] - M[:, 1, 2], M[:, 0, 2] - M[:, 2, 0], M[:, 1, 0] - M[:, 0, 1]], -1) * 100 * poseb[:, None]
return torch.cat([Tb - p, rv], -1)
out = []; x = q.clone()
ar = torch.arange(6, device=q.device)
for _ in range(n):
e0 = err(x, T, Rt, pose)
# 2026-10-20：6 次串行有限差分合并为一次批量 FK（数值等价，单次规划 ~25ms→~7ms）
dx = torch.zeros(B, 6, 6, device=q.device); dx[:, ar, ar] = 1e-3
xp = (x[:, None, :] + dx).reshape(B * 6, 6)
ep = err(xp, T.repeat_interleave(6, 0), Rt.repeat_interleave(6, 0), pose.repeat_interleave(6, 0))
J = -((ep - e0.repeat_interleave(6, 0)) / 1e-3).view(B, 6, 6).transpose(1, 2) # [B,6(err),6(q)]
xf = kin.cum_xf(x)
e0 = err(x, T, Rt, pose, xf)
if analytic:
# J = −∂err/∂q：位置 err = T−p → +Jv；姿态 err = 100·rotvec(Rt·Rqᵀ) → +100·Jw
Jv, Jw = kin.jac(x, xf)
J = torch.cat([Jv, 100 * Jw * pose[:, None, None]], 1) # [B,6(err),6(q)]
else:
# 2026-10-20：6 次串行有限差分合并为一次批量 FK（数值等价，单次规划 ~25ms→~7ms）
dx = torch.zeros(B, 6, 6, device=q.device); dx[:, ar, ar] = 1e-3
xp = (x[:, None, :] + dx).reshape(B * 6, 6)
ep = err(xp, T.repeat_interleave(6, 0), Rt.repeat_interleave(6, 0), pose.repeat_interleave(6, 0))
J = -((ep - e0.repeat_interleave(6, 0)) / 1e-3).view(B, 6, 6).transpose(1, 2) # [B,6(err),6(q)]
A = J @ J.transpose(1, 2) + lam ** 2 * torch.eye(6, device=q.device)
y = torch.linalg.solve(A, e0.unsqueeze(-1))
dq = (J.transpose(1, 2) @ y).squeeze(-1)
@@ -146,7 +153,7 @@ class TeacherOracle:
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', oracle=None, replan='r3', sub_rule='cross',
fb_gain=0.0, fb_scale=0.0, fb_near=0.2, fb_fallback=0.0, safety_partial=0, commit_n=3, collect=None,
fb_exec=0.0, exec_safety=0):
fb_exec=0.0, exec_safety=0, fb_ki=0.0, fb_decay=0.9, fb_imax=0.5):
"""use_sub=False：不用子目标（sf=0）；sub_override=(subs [B,M,6] 绝对关节角, n [B] 个数 或 need [B] bool)：冷启动时用给定途经点代替子目标头；
qp0 [B,6]：起点的上一帧（默认 = q0，即静止起步）。steps：MeanFlow 采样段数；z_scale：噪声缩放；
select：consist（部署口径：冷启动价值前瞻、之后与上一条已提交计划的一致性）/ value（每次都价值前瞻）/ first（不选，取第 0 条）。
@@ -165,6 +172,7 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None,
q, qp = q0.clone(), (qp0.to(device) if qp0 is not None else q0.clone())
queue = torch.zeros(B, 0, 6, device=device)
last = None; last_ok = torch.zeros(B, dtype=torch.bool, device=device)
fb_i = torch.zeros(B, 6, device=device) # 积分项：累积持续同向的目标反馈（慢变偏差）
subs = torch.zeros(B, 2, 6, device=device); nsub = torch.zeros(B, dtype=torch.long, device=device)
sub_t = torch.zeros(B, device=device); sub_prev = q0.clone()
done = torch.zeros(B, dtype=torch.bool, device=device); t_done = torch.full((B,), float('nan'), device=device)
@@ -197,7 +205,12 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None,
inc = act.float().clamp(-1, 1) * astep
if fb_gain > 0: # 实验（2026-10-20）：目标反馈加在所有候选上（安全层也评估带反馈的候选）
fb = dls_steps(kin, qe, T, R, n=1)
if fb_scale > 0: # 距离衰减：远离目标全增益，贴近目标减弱（减少过冲/刹停）
if fb_ki > 0:
fb_i = fb_decay * fb_i + fb[:, 0] * (~done)[:, None]
lim = step * fb_imax
fb_i = fb_i.clamp(-lim, lim)
inc = inc + fb_gain * fb[:, :, None, :] + fb_ki * fb_i[:, None, None, :]
elif fb_scale > 0: # 距离衰减：远离目标全增益，贴近目标减弱（减少过冲/刹停）
dg, _ = goal_err(kin, qe, T, R)
gdec = fb_gain * torch.clamp(dg / fb_scale, fb_near, 1.0)
inc = inc + gdec[:, None, None, None] * fb[:, :, None, :]
@@ -289,6 +302,8 @@ def rollout(model, kin, tasks, device, K=8, safety=False, seed=0, max_sec=None,
dls_it = torch.where(near & (dls_it < 0), torch.full_like(dls_it, it), dls_it)
committed = committed & ~near
# 2026-10-19（F5）：last 记真正提交的那条候选（安全层换了候选时是换后的那条）；刹停 / DLS 接管的行无效
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
queue = torch.cat([queue, commit], 1)
cold = False