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.