Wrong FK for UR5
lidonghui-ai opened this issue · 1 comments
lidonghui-ai commented
The FK for UR5 using pytorch_kinematics seems to be wrong.
I use both pytorch_kinematics and ikpy to solve the FK of UR5, but the results are not same.
Using the ikpy can get the right result but using pytorch_kinematics get the wrong answer.
Hope you can help me!
Thanks!
import pytorch_kinematics as pk
import ikpy
pk_chain = pk.build_serial_chain_from_urdf(open('ur5.urdf').read(), 'ee_link', 'base_link')
ik_chain = ikpy.chain.Chain.from_urdf_file('ur5.urdf', active_links_mask=[False, True, True, True, True, True, True, False])
th = [0.0, -math.pi / 4.0, 0.0, math.pi / 2.0, 0.0, math.pi / 4.0]
ret = pk_chain.forward_kinematics(th, end_only=True)
print(ret.get_matrix())
ik_ret = ik_chain.forward_kinematics([0, *th, 0])
print(ik_ret)
LemonPi commented
Hi, this might've been an issue with serial chains before, but on 5288100 I'm getting that they produce the same results.
tensor([[[ 0.0000e+00, -5.9605e-08, -1.0000e+00, 5.1096e-01],
[ 1.0000e+00, 0.0000e+00, 0.0000e+00, 1.9145e-01],
[ 0.0000e+00, -1.0000e+00, 5.9605e-08, 6.0011e-01],
[ 0.0000e+00, 0.0000e+00, 0.0000e+00, 1.0000e+00]]])
[[-6.44330720e-18 3.58979314e-09 -1.00000000e+00 5.10955359e-01]
[ 1.00000000e+00 1.79489651e-09 0.00000000e+00 1.91450000e-01]
[ 1.79489651e-09 -1.00000000e+00 -3.58979312e-09 6.00114361e-01]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
Which when tested for similarity is true:
assert torch.allclose(ik_ret, ret.get_matrix(), atol=1e-6)