search

UM-ARM-Lab / pytorch_kinematics

Robot kinematics implemented in pytorch
MIT License
394 stars 34 forks source link

Wrong FK for UR5 #30

Open lidonghui-ai opened 9 months ago

lidonghui-ai commented 9 months ago

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