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

- 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
+13
View File
@@ -110,6 +110,19 @@ class Kin:
Rs, _ = xf if xf is not None else self.cum_xf(q)
return Rs[:, 6]
def jac(self, q, xf=None):
"""几何雅可比(对度):返回 (Jv [B,3,6] mm/deg, Jw [B,3,6] 单位轴/deg)"""
Rs, Ts = xf if xf is not None else self.cum_xf(q)
p = self.fk(q, (Rs, Ts))
Jv, Jw = [], []
for k in range(6):
d = torch.tensor(self.D[k], dtype=q.dtype, device=q.device)
a = Rs[:, k] @ d
o = torch.einsum('bij,j->bi', Rs[:, k], self.P[k].to(q.dtype)) + Ts[:, k]
Jv.append(torch.linalg.cross(a, p - o, dim=-1) * D2R)
Jw.append(a * D2R)
return torch.stack(Jv, -1), torch.stack(Jw, -1)
def caps_world(self, q, xf=None):
Rs, Ts = xf if xf is not None else self.cum_xf(q)
Rg, Tg = Rs[:, self.cap_g], Ts[:, self.cap_g] # [B,C,3,3], [B,C,3]