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:
@@ -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]
|
||||
|
||||
Reference in New Issue
Block a user