How do I program forward and inverse kinematics of PUMA 560?

See more:

robot

Python2.7

C++11

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](https://deepblue.lib.umich.edu/bitstream/handle/2027.42/6192/bac6709.0001.001.pdf). 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
>Link lengths

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

>Link offsets

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

>Link twist angle

`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}`.

What I have tried:

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  

Function to compute T;

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.<br>


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.