Wrong FK for UR5

[lidonghui-ai]

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)

ur5.zip

[LemonPi]

Hi, this might've been an issue with serial chains before, but on 52881009a13c98e737e5a6ef7e3d97f35391b00e 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)