fkine and ik_lm work wrong for fetch robot?
Opened this issue · 0 comments
tractormaster4596 commented
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
Obviously,the result is wrong.
I dont find the reason. Can anyone help me?