why is PyKDL not converging?

Question

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)
chain.addSegment(segment0) 

#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)
chain.addSegment(segment1)

#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)
chain.addSegment(segment2)

#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)
chain.addSegment(segment3)


#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):
    jointAngles[i]=np.deg2rad(q)

#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,         1.5,         1.3]
Actual Position:  [        1.98,         1.5,         1.5]

You can see that the actual position converges for X and Y, but not Z. This is true for every set of starting points I have tried.

Any thoughts? I'm really stuck here.

---

Originally posted by loughnane on ROS Answers with karma: 1 on 2013-07-24

Post score: 0

Answer 1

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;

    chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(1.0,0.0,0.0))));
    chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(1.5,0.0,0.0))));
    chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.5,0.0,0.0))));
    chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,1.5))));

    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 << "Goal:   " << pos_goal.p << endl;
    cout << "Reached:" << pos_reached.p << endl;

    return 0;
}

And the output:

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

---

Originally posted by jzhan with karma: 16 on 2014-01-29

This answer was ACCEPTED on the original site

Post score: 0