# Inverse Kinematics of Puma 566 giving low angle results with DH convention

I am new at the robotics area, and i need to deliver an activity until this week in my college based to solve the inverse kinematics problem from an article that doesn't have any code done, and after, purpose improvements on it to create one new article, for it i am trying to reach these existing results based in this article with a Puma 566 manipulator with 4 DOF. Inverse kinematics of redundant robots using genetic algorithms (Parker Et Al.)

+--------+------------------------+-------------------------------+-----------------------+--------------------------+----------+
| Joint  | Initial Value(Degrees) | Final Value(Run 1) (Degrees)  | Final Value(Run 2) (Degrees) | Final Value(Run 2) (Degrees) |
+========+========================+===============================+==============================+===================+==========+
|    1   |         +30.0          |            +8.75              |            +8.44             |           +8.44              |
+--------+------------------------+-------------------------------+------------------------------+------------------------------+
|    2   |         -60.0          |            -45.44             |           -47.81             |         -48.08               |
+--------+------------------------+-------------------------------+------------------------------+------------------------------+
|    3   |         +30.0          |            +14.59             |          +21.97              |         +23.29               |
+--------+------------------------+-------------------------------+------------------------------+------------------------------+
|    4   |         +60.0          |            +47.02             |           +39.64             |          +39.64              |
+--------+------------------------+-------------------------------+------------------------------+------------------------------+
|         Minimum Error (p)       |          7.1mm (0.279in)      |          7.1mm (0.279in)     |         7.1mm (0.279in)      |
----------------------------------+-------------------------------+------------------------------+------------------------------+
|         Displacement (Max)      |             21.3 deg.         |             21.6 deg.        |             21.6 deg.        |
----------------------------------+-------------------------------+------------------------------+------------------------------+


So for the Puma 566 i considered these as being the initial parameters:

+----------------+-------+---------+-------+------=-+
|  Link/Joint    |  a_i  |   α_i   |  d_i  |   θ_i  |
+----------------+-------+---------+-------+------=-+
|        1       |   0   |   +π/2  |  a_i  | θ^*_1  |
+----------------+-------+---------+-------+------=-+
|        2       |  a_2  |     0   | -d_2  | θ^*_2  |
+----------------+-------+---------+-------+------=-+
|        3       |  a_3  |     0   |   0   | θ^*_3  |
+----------------+-------+---------+-------+--------+
|        4       |  a_4  |     0   |   0   | θ^*_4  |
+----------------+-------+---------+-------+--------+

and at the article they considered the distances being:
a_1 = 660.4mm(26.00in)
a_2 = 431.8mm(17.00in)
d_2 = 149.1mm(5.87in)
a_3 = 254.0mm(10.00in)
a_4 = 196.85mm(7.75in)



Below i did the python code by myself following the classic equations to solve the forward and the inverse kinematics:

import numpy as np
#import sympy as sp

'''Puma 566 Redundant Robot'''

'''input parameters'''
'''
sin = symbols('sin')
cos = symbols('cos')
alpha = symbols('\u03B1')
theta = symbols('\u03B8')
a_i = symbols('a_i')
d_i = symbols('d_i')
'''

#a_1 = 660.4mm
a_1 = 0
#d_1 = symbols('a_1')
d_1 = 660.4/1000 #d_1 == a_1
theta_1 = 30
#a_2 = symbols('a_2')
a_2 = 431.8/1000
alpha_2 = 0
#d_2 = symbols('-d_2')
d_2 = -149.1/1000
theta_2 = -60
#a_3 = symbols('a_3')
a_3 = 254.0/1000
alpha_3 = 0
d_3 = 0
theta_3 = 30
#a_4 = symbols('a_4')
a_4 = 196.85/1000
alpha_4 = 0
d_4 = 0
theta_4 = 60

'A_1 values'
A_1r1_1 = np.cos(theta_1)
A_1r1_2 = -np.sin(theta_1)*np.cos(alpha_1)
A_1r1_3 = np.sin(theta_1)*np.sin(alpha_1)
A_1d_x = a_1*np.cos(theta_1)
A_1r2_1 = np.sin(theta_1)
A_1r2_2 = np.cos(theta_1)*np.cos(alpha_1)
A_1r2_3 = -np.cos(theta_1)*np.sin(alpha_1)
A_1d_y = a_1*np.sin(theta_1)
A_1r3_1 = 0
A_1r3_2 = np.sin(alpha_1)
A_1r3_3 = np.cos(alpha_1)
A_1d_z = d_1

'A_2 values'
A_2r1_1 = np.cos(theta_2)
A_2r1_2 = -np.sin(theta_2)*np.cos(alpha_2)
A_2r1_3 = np.sin(theta_2)*np.sin(alpha_2)
A_2d_x = a_2*np.cos(theta_2)
A_2r2_1 = np.sin(theta_2)
A_2r2_2 = np.cos(theta_2)*np.cos(alpha_2)
A_2r2_3 = -np.cos(theta_2)*np.sin(alpha_2)
A_2d_y = a_2*np.sin(theta_2)
A_2r3_1 = 0
A_2r3_2 = np.sin(alpha_2)
A_2r3_3 = np.cos(alpha_2)
A_2d_z = d_2

'A_3 values'
A_3r1_1 = np.cos(theta_3)
A_3r1_2 = -np.sin(theta_3)*np.cos(alpha_3)
A_3r1_3 = np.sin(theta_3)*np.sin(alpha_3)
A_3d_x = a_3*np.cos(theta_3)
A_3r2_1 = np.sin(theta_3)
A_3r2_2 = np.cos(theta_3)*np.cos(alpha_3)
A_3r2_3 = -np.cos(theta_3)*np.sin(alpha_3)
A_3d_y = a_3*np.sin(theta_3)
A_3r3_1 = 0
A_3r3_2 = np.sin(alpha_3)
A_3r3_3 = np.cos(alpha_3)
A_3d_z = d_3

'A_4 values'
A_4r1_1 = np.cos(theta_4)
A_4r1_2 = -np.sin(theta_4)*np.cos(alpha_4)
A_4r1_3 = np.sin(theta_4)*np.sin(alpha_4)
A_4d_x = a_4*np.cos(theta_4)
A_4r2_1 = np.sin(theta_4)
A_4r2_2 = np.cos(theta_4)*np.cos(alpha_4)
A_4r2_3 = -np.cos(theta_4)*np.sin(alpha_4)
A_4d_y = a_4*np.sin(theta_4)
A_4r3_1 = 0
A_4r3_2 = np.sin(alpha_4)
A_4r3_3 = np.cos(alpha_4)
A_4d_z = d_4

'transformation matrices'
A_1 = np.matrix([[A_1r1_1,A_1r1_2,A_1r1_3,A_1d_x],[A_1r2_1,A_1r2_2,A_1r2_3,A_1d_y],[A_1r3_1,A_1r3_2,A_1r3_3,A_1d_z],[0,0,0,1]])
A_2 = np.matrix([[A_2r1_1,A_2r1_2,A_2r1_3,A_2d_x],[A_2r2_1,A_2r2_2,A_2r2_3,A_2d_y],[A_2r3_1,A_2r3_2,A_2r3_3,A_2d_z],[0,0,0,1]])
A_3 = np.matrix([[A_3r1_1,A_3r1_2,A_3r1_3,A_3d_x],[A_3r2_1,A_3r2_2,A_3r2_3,A_3d_y],[A_3r3_1,A_3r3_2,A_3r3_3,A_3d_z],[0,0,0,1]])
A_4 = np.matrix([[A_4r1_1,A_4r1_2,A_4r1_3,A_4d_x],[A_4r2_1,A_4r2_2,A_4r2_3,A_4d_y],[A_4r3_1,A_4r3_2,A_4r3_3,A_4d_z],[0,0,0,1]])

'transformation matrix Hi-1_i = A_i+1 = qi+1'
H0_1 = A_1
H1_2 = A_2
H2_3 = A_3
H3_4 = A_4

'orientation matrix'
R0_1 = np.matrix([[A_1r1_1,A_1r1_2,A_1r1_3],[A_1r2_1,A_1r2_2,A_1r2_3],[A_1r3_1,A_1r3_2,A_1r3_3]])
R1_2 = np.matrix([[A_2r1_1,A_2r1_2,A_2r1_3],[A_2r2_1,A_2r2_2,A_2r2_3],[A_2r3_1,A_2r3_2,A_2r3_3]])
R2_3 = np.matrix([[A_3r1_1,A_3r1_2,A_3r1_3],[A_3r2_1,A_3r2_2,A_3r2_3],[A_3r3_1,A_3r3_2,A_3r3_3]])
R3_4 = np.matrix([[A_4r1_1,A_4r1_2,A_4r1_3],[A_4r2_1,A_4r2_2,A_4r2_3],[A_4r3_1,A_4r3_2,A_4r3_3]])

'product of orientation matrix'
R0_2 = np.dot(R0_1,R1_2)
R0_3 = np.dot(R0_2,R2_3)
R2_4 = np.dot(R0_3,R3_4)

'''transformation matrix Hi-1_i, Ti-1_i:'''
H0_2 = np.dot(H0_1,H1_2)
H0_3 = np.dot(H0_2,H2_3)
H0_4 = np.dot(H0_3,H3_4)

'individual matrices'
print('H0_1:',H0_1)
print("\n")
print('H1_2:',H1_2)
print("\n")
print('H2_3:',H2_3)
print("\n")
print('H3_4:',H3_4)
print("\n")

''
print("Matrix H0_4/T0_4:",H0_4)


in the first interaction i got these results:

H0_1: [[ 0.15425145 -0.4427109  -0.88329698  0.        ]
[-0.98803162 -0.069116   -0.13790028 -0.        ]
[ 0.          0.89399666 -0.44807362  0.6604    ]
[ 0.          0.          0.          1.        ]]

H1_2: [[-0.95241298 -0.30481062  0.         -0.41125192]
[ 0.30481062 -0.95241298  0.          0.13161723]
[ 0.          0.          1.         -0.1491    ]
[ 0.          0.          0.          1.        ]]

H2_3: [[ 0.15425145  0.98803162 -0.          0.03917987]
[-0.98803162  0.15425145 -0.         -0.25096003]
[ 0.          0.          1.          0.        ]
[ 0.          0.          0.          1.        ]]

H3_4: [[-0.95241298  0.30481062 -0.         -0.1874825 ]
[-0.30481062 -0.95241298  0.         -0.06000197]
[ 0.          0.          1.          0.        ]
[ 0.          0.          0.          1.        ]]

Matrix H0_4/T0_4: [[ 0.46120588  0.08411651 -0.88329698 -0.00427582]
[-0.08411651 -0.98686773 -0.13790028  0.34517934]
[-0.88329698  0.13790028 -0.44807362  0.89535356]
[ 0.          0.          0.          1.        ]]


and when i did the inverse kinematics the angles weren't compatible with the first interaction of that table:

'''theta_1 value'''
x0_4 = H0_4[0,3]
y0_4 = H0_4[1,3]
z0_4 = H0_4[2,3]
r2 = (np.power(x0_4,2)+np.power(y0_4,2)-np.power(d_2,2))
s = z0_4-d_1

invtheta1 = np.arctan2(x0_4,y0_4)

'''theta_3 value'''

num_beta = r2+np.power(s,2)-np.power(a_2,2)-np.power(a_3,2)
den_beta = 2*(a_2*a_3)
beta=num_beta/den_beta
invtheta3 = np.arctan2(beta,np.sqrt(1-(np.power(-beta,2))))
invtheta3

'''theta_2 value'''
num2=(a_3*np.sin(theta_3))
den2=(a_2+(a_3*np.cos(theta_3)))
invtheta2 = np.arctan2(np.sqrt(r2),s)-np.arctan2(den2,num2)
invtheta2

print(invtheta1,invtheta2,invtheta3)


and the result:

-0.01238661073906638 -1.1360414233492173 -0.4673695117711878


At the moment i didn't do any functions or conditions on it to understand the algorithm and a test to check if it's correctly being performed, the code is working well but i can't able to reach the desired results(the inverse kinematics of the angles are giving a very low value... i would like to know if there are any incorrect values and which do you consider the best way to reach close the results? They did the genetic algorithm implementations also after but i don't know if is necessary now. Thanks