cbb
46fe24365b
- Kin.jac 几何雅可比，与 autograd 逐项一致（FD 自身 rel 误差 3.58%） - dls_steps(analytic=True) 默认；每步少 6 次批量 FK - 新增 ml/eval/eval_cl.py（部署口径小规模评估）与 check_dls.py（对拍+延迟） - 积分反馈 fb_ki 脚手架（默认 0，未启用）
55 lines
2.3 KiB
Python
55 lines
2.3 KiB
Python
"""DLS 解析雅可比对拍 + 延迟（python -m ml.eval.check_dls）"""
|
||
import time
|
||
import torch
|
||
|
||
from ml.common.kin_torch import Kin
|
||
from ml.sim.closed_loop import dls_steps
|
||
|
||
|
||
def main():
|
||
dev = 'cuda' if torch.cuda.is_available() else 'cpu'
|
||
kin = Kin(device=dev)
|
||
g = torch.Generator(device=dev); g.manual_seed(0)
|
||
B = 64
|
||
lo, hi = kin.lo + 20, kin.hi - 20
|
||
q = lo + (hi - lo) * torch.rand(B, 6, device=dev, generator=g)
|
||
xf = kin.cum_xf(q)
|
||
p = kin.fk(q, xf)
|
||
T = p + 6 * torch.randn(B, 3, device=dev, generator=g)
|
||
R = torch.zeros(B, 9, device=dev)
|
||
a = dls_steps(kin, q, T, R, n=3, analytic=True)
|
||
b = dls_steps(kin, q, T, R, n=3, analytic=False)
|
||
d = (a - b).abs()
|
||
print('pos-only max|Δdq| %.6f° mean %.6f° rel %.4f' % (d.max().item(), d.mean().item(), (d.norm() / b.norm()).item()))
|
||
# 带姿态目标：绕随机轴 ±8°
|
||
axis = torch.randn(B, 3, device=dev, generator=g); axis = axis / axis.norm(dim=-1, keepdim=True)
|
||
ang = (torch.rand(B, 1, device=dev, generator=g) * 16 - 8) * torch.pi / 180
|
||
Kx = torch.zeros(B, 3, 3, device=dev)
|
||
Kx[:, 0, 1] = -axis[:, 2]; Kx[:, 0, 2] = axis[:, 1]; Kx[:, 1, 0] = axis[:, 2]
|
||
Kx[:, 1, 2] = -axis[:, 0]; Kx[:, 2, 0] = -axis[:, 1]; Kx[:, 2, 1] = axis[:, 0]
|
||
I = torch.eye(3, device=dev)
|
||
Rd = I + torch.sin(ang)[..., None] * Kx + (1 - torch.cos(ang))[..., None] * (Kx @ Kx)
|
||
Rt = kin.orient(q, xf) @ Rd
|
||
R9 = Rt.reshape(B, 9)
|
||
a = dls_steps(kin, q, T, R9, n=3, analytic=True)
|
||
b = dls_steps(kin, q, T, R9, n=3, analytic=False)
|
||
d = (a - b).abs()
|
||
print('pos+rot max|Δdq| %.6f° mean %.6f° rel %.4f' % (d.max().item(), d.mean().item(), (d.norm() / b.norm()).item()))
|
||
|
||
for n_ in (1, 3):
|
||
for Bn in (1, 256):
|
||
qq = q[:Bn]; TT = T[:Bn]; RR = R9[:Bn]
|
||
for an in (False, True):
|
||
for _ in range(3):
|
||
dls_steps(kin, qq, TT, RR, n=n_, analytic=an)
|
||
torch.cuda.synchronize() if dev == 'cuda' else None
|
||
t0 = time.perf_counter()
|
||
for _ in range(20):
|
||
dls_steps(kin, qq, TT, RR, n=n_, analytic=an)
|
||
torch.cuda.synchronize() if dev == 'cuda' else None
|
||
print('n=%d B=%3d analytic=%d %.2f ms' % (n_, Bn, an, (time.perf_counter() - t0) / 20 * 1e3))
|
||
|
||
|
||
if __name__ == '__main__':
|
||
main()
|