petercorke/robotics-toolbox-python

fkine and ik_lm work wrong for fetch robot?

Opened this issue · 0 comments

I want to calculate param of fetch robot using rt party.
My test code is below,it is very simple and the same method work right on panda robot:

import roboticstoolbox as rtb
import numpy as np

robot = rtb.models.Fetch()

init_q=np.array([0,0,0,0, -0.3, 0, -2.2, 0, 2, 0.7854])
start_pos = robot.fkine(init_q)
print("start_pos:\n",start_pos)

q_ik=robot.ik_LM(start_pos,q0=init_q)

print("q_ik:\n",q_ik)

end_pos = robot.fkine(q_ik[0])

print("end_pos:\n",end_pos)``

The picture of result like this
Screenshot from 2024-11-09 15-20-41

Obviously,the result is wrong.
I dont find the reason. Can anyone help me?