# Forward and Inverse Kinematics of PUMA 560

I am currently coding a Forward and Inverse Kinematics solver for a PUMA 560 robot. For the Inverse Kinematics part I am using the closed for solution given in this paper. But my issue is, my solution for IK for a given set of (x,y,z) does not return the same values returned by my FK values. The reason I am doing this is to verify my code accurately computes the FK and IK.

These are the DH parameters for my robot (These are Python code, since I was testing my algorithm on Spyder IDE before implementing on C++).

DH Parameters

a = [0, 650, 0, 0, 0, 0]

d = [0, 190, 0, 600, 0, 125]

alpha = [-pi/2, 0, pi/2, -pi/2, pi/2, 0]

So basically I finding the T transformation matrix for each link from the the base frame {B} to wrist frame {W}. This is my code;

Function to compute Forward Kinematics

def forwardK(q):

#T06 is the location of Wrist frame, {W}, relative to Base frame, {B}
T01 = genT(q[0],0,d[0],0)
T12 = genT(q[1],a[0],d[1],alpha[0])
T23 = genT(q[2],a[1],d[2],alpha[1])
T34 = genT(q[3],a[2],d[3],alpha[2])
T45 = genT(q[4],a[3],d[4],alpha[3])
T56 = genT(q[5],a[4],d[5],alpha[4])

#Tool frame {T}
#T67 = genT(0,0,d[5],0)

T03 = matmul(T01,T12,T23)
T36 = matmul(T34,T45,T56)
T06 = matmul(T03,T36)
#T07 = matmul(T06,T67)

x = T[0][3]
y = T[1][3]
z = T[2][3]

print("X: ",x)
print("Y: ",y)
print("Z: ",z,"\n")
print("T: ",T,"\n")

return T06


The function to compute T Matrix

def genT(theta, a, d, alpha):
T =  array([[cos(theta), (-sin(theta)), 0, a],
[sin(theta)*cos(alpha), (cos(theta)*cos(alpha)), -sin(alpha), (-   d*sin(alpha))],
[sin(theta)*sin(alpha), cos(theta)*sin(alpha), cos(alpha), cos(alpha)*d],
[0, 0, 0, 1]])

return T


from the T Matrix relating the {B} frame to the {W} frame position vector of the {w} [x y z] is extracted. R Matrix (orientation) of the {W} relative to the {B} is obtained by the following piece of code;

T = forwardK([30,-110,-30,0,0,0])
x = T[0][3]
y = T[1][3]
z = T[2][3]
R = T[0:3,0:3]


Where T is the transformation matrix relating {W} to {B}. Then this information is fed in to the invK(x,y,z,R,ARM,ELOBOW,WRIST) function to check if the algorithm returns the same set of angles fed to the forwardK(q1,q2,q3,q4,q5,q6) function.

In the invK(x,y,z,R,ARM,ELOBOW,WRIST) - ARM, ELBOW, WRIST are orientation specifiers to describe various possible configurations of the manipulator. Each of these parameters are either {+1,-1}. These values are then used in the closed form geometrical solution presented by the afore-mentioned paper.

I did not post the code for the invK(x,y,z,R,ARM,ELOBOW,WRIST) since it is a direct implementation of the closed form solution presented in the paper and also it significantly long hence making it highly unreadable.

What do you think I am doing wrong? I am quite sure the way I am computing the FK is correct but I could be wrong. The matrix multiplications of my Python code are correct since I double checked them with Matlab. Any advice is appreciated.

I already asked this question on robotics.stackexchange.com, but could not get a proper answer. That's why I am here. Thanks