# why is PyKDL not converging?

The code below properly (as far as I can tell) calculates the inverse kinematics for a 2D linkage. However, when I add a segment that is normal to the rest of the segments (segment3 in the code), it has a tough time doing much with it. It still solves for an appropriate x-y position, but doesn't seem to be able to converge on a solution for the Z value.

I am certain there is a solution as I've modeled it in CAD.

So here is the code (output included)

from PyKDL import *
import numpy as np
print "Creating Robotic Chain"
print " "
#create chain
chain=Chain()

#create first segment and add it to chain
joint0=Joint(Joint.RotZ)
frame0=Frame(Vector(1.0,0.0,0.))
segment0=Segment(joint0,frame0)

#create second segment and add it to chain
joint1=Joint(Joint.RotZ) #Iqual joint
frame1=Frame(Vector(1.5,0.,0.))
segment1=Segment(joint1,frame1)

#create third segment and add it to chain
joint2=Joint(Joint.RotZ) #Iqual joint
frame2=Frame(Vector(0.5,0.0,0.))
segment2=Segment(joint2,frame2)

#create fourth segment and add it to chain
joint3=Joint(Joint.RotX) #Iqual joint
frame3=Frame(Vector(0.,0.,1.5))
segment3=Segment(joint3,frame3)

#create some initial angles for the joints
q0=5.9037
q1=17.203
q2=36.416
q3=-30

#put the initial angles in a list
qList=[q0,q1,q2,q3]

#start looking at the forward kinematics
print "Forward kinematics"

# create a Joint Array containing all of the initial angles
jointAngles=JntArray(len(qList))
for i,q in enumerate(qList):

#calculate the forward kinematics of the chain
fk=ChainFkSolverPos_recursive(chain)

#calculate the position of the end effector
finalFrame=Frame()
fk.JntToCart(jointAngles,finalFrame)

print "End-effector position: ",finalFrame.p
print ' '

#now that i've done the forward kinematics, i'll do the inverse kinematics

#set a desired position, note that this is the almost exactly position of the end effector that was calculated
#in the forward kinematics. The only difference is that Z has been dropped by 0.2 (to 1.3). I am certain the end effector
#can reach this point, as I modeled it in CAD.

x=1.98
y=1.50
z=1.30
desiredFrame=Frame(Vector(x,y,z))

print "Inverse Kinematics"

#calculate the velocity IK of the chain
vik=ChainIkSolverVel_pinv(chain)

#calculate the position IK of the chain
ik=ChainIkSolverPos_NR(chain,fk,vik,100,1e-3)

#print out the desired position
print "Desired Position: ", desiredFrame.p

#calculate the output angles
q_out=JntArray(len(qList))
ik.CartToJnt(jointAngles,desiredFrame,q_out)

#put the angles into the fk solver to get the end effector position
checkFrame=Frame()
fk.JntToCart(q_out,checkFrame)
print 'Actual Position: ',checkFrame.p


And here is the output:

chris@thinkpad:~/code/igr\$ python ik.py
Creating Robotic Chain

Forward kinematics
End-effector position:  [     1.98159,     1.50284,     1.29904]

Inverse Kinematics
Desired Position:  [        1.98 ...
edit retag close merge delete

Sort by » oldest newest most voted

ChainIkSolverPos_NR is very easy to be trapped in local minimums. Use ChainIkSolverPos_LMA instead. However at this time ChainIkSolverPos_LMA hasn't been ported to PyKDL, so you have to use C++ or port it by yourself following the other classes of Orocos KDL.

The following is the C++ implementation of your codes using ChainIkSolverPos_LMA instead of ChainIkSolverPos_NR:

#include <iostream>
#include <frames_io.hpp>
#include <chainiksolverpos_lma.hpp>
#include <chainfksolverpos_recursive.hpp>

using namespace KDL;
using namespace std;

int main(int argc,char* argv[]) {
Chain chain;

Frame pos_goal = Frame(Vector(1.98, 1.50, 1.30));
Frame pos_reached;

JntArray q_init(chain.getNrOfJoints());
JntArray q_out(chain.getNrOfJoints());
//q_init.data.setRandom();
//q_init.data *= M_PI;

Eigen::Matrix<double,6,1> L;
L(0)=1;L(1)=1;L(2)=1;
L(3)=0.01;L(4)=0.01;L(5)=0.01;
ChainFkSolverPos_recursive fwdkin(chain);
ChainIkSolverPos_LMA solver(chain,L);

int retval = solver.CartToJnt(q_init,pos_goal,q_out);

fwdkin.JntToCart(q_out, pos_reached);
cout &lt;&lt; "Goal:   " &lt;&lt; pos_goal.p &lt;&lt; endl;
cout &lt;&lt; "Reached:" &lt;&lt; pos_reached.p &lt;&lt; endl;

return 0;
}


And the output:

Goal:   [        1.98,         1.5,         1.3]
Reached:[        1.98,         1.5,     1.30007]


</double,6,1></chainfksolverpos_recursive.hpp></chainiksolverpos_lma.hpp></frames_io.hpp></iostream>

more