# 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)
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 ...
```