# help to find the bug for calculating inverse kinematic using jacobian

I am calculating inverse kinematic for 5dof robotic arm using Jacobian matrix (first time). i used following formulas to do my calculation and also implemented in my code.

I am not understanding what I am doing wrong. I am getting the wrong theta values and while loop is running maximum two times, it does not matter which values I give as x, y, z. - I have searched, how others have calculated their Jacobian matrices. but still could not find my mistake.

please help me to debug my code. I might be doing some silly mistake. This is a code for calculating forward kinematics

class Fkine:
def __init__(self, PT):
""" Initialize the DH parameters for forward kinematics
DH peramteres columes should be DH = [theta alpha  r  d]
"""
if not isinstance(PT, list):
print("[ERROR] dh parameter should be matrix {}".format(PT))
if not np.shape(PT)[1] == 4:
print("[ERROR] enter proper dh parameter {}".format(PT))

self.PT = np.array(PT)

def homog_trans_matrix(self, row_num):

cos_theta_n = np.cos(self.PT[row_num][0])
sin_theta_n = np.sin(self.PT[row_num][0])

cos_alpha_n = np.cos(self.PT[row_num][1])
sin_alpha_n = np.sin(self.PT[row_num][1])

r_n = self.PT[row_num][2]
d_n = self.PT[row_num][3]

homog_trans_matrix = [
[cos_theta_n, -sin_theta_n * cos_alpha_n, sin_theta_n * sin_alpha_n, r_n * cos_theta_n],
[sin_theta_n, cos_theta_n * cos_alpha_n, -cos_theta_n * sin_alpha_n, r_n * sin_theta_n],
[0, sin_alpha_n, cos_alpha_n, d_n],
[0, 0, 0, 1],
]
return np.matrix(homog_trans_matrix)

def fk(self):
""" function to generate forward kinematic from DH parameters
by multiplying homogeneous transformation matrix Hn = H_0 * H_1 * ....H_n-1
"""
Hn = []

# taking the number of rows from the DH parameters
for i in range(np.shape(self.PT)[0]):
H = self.homog_trans_matrix(i)

if i == 0:
Hn = H

else:
Hn = np.dot(Hn, H)

return Hn

def get_jacobian(fkine_obj, PT):
Hn = fkine_obj.fk()
# print(Hn)
R0_0 = [
[0.],
[0.],
[1.]
]
D0_n = Hn[:, 3][:-1]

upper_part = np.cross(R0_0, D0_n, axis=0)
# print(upper_part)

for i in range(np.shape(PT)[0] - 1):
H0_i = fkine_obj.homog_trans_matrix(i)

# accessing 3rd column of the matrix and removing last row
R0_i = H0_i[:, 2][:-1]

# accessing 4th column of the matrix and removing last row
D0_i = H0_i[:, 3][:-1]

next_col = np.cross(R0_i, np.subtract(D0_n, D0_i), axis=0)

upper_part = np.concatenate([upper_part, next_col], axis=1)

return upper_part
def main():

THETA_1 = 0.1
THETA_2 = 90.
THETA_3 = 0.1
THETA_4 = 0.1
THETA_5 = 0.1

L_1 = 33.  # mm 3.3cm
L_2 = 105.  # mm 10.5cm
L_3 = 98.  # mm 9.8cm
L_4 = 27.  # mm 2.7cm
L_5 = 65.  # mm 6.5cm

# DH parameter table for my robotic arm
PT = [
[math.radians(THETA_5), 0, 0, L_4 + L_5]
]

x_dst = 1.
y_dst = 1.
z_dst = 1.

while 180. >= math.degrees(PT[0][0]) >= 0. \
and 180. >= math.degrees(PT[1][0]) >= 0. \
and 180. >= math.degrees(PT[2][0]) >= 0. \
and 180. >= math.degrees(PT[3][0]) >= 0. \
and 180. >= math.degrees(PT[4][0]) >= 0.:

fk_5d = fk.Fkine(PT)
jacobian = get_jacobian(fk_5d, PT)

jacobian_inv = np.linalg.pinv(jacobian)
current_cord = fk_5d.fk()[:, 3][:-1]

theta_dots = jacobian_inv.dot([
[x_dst],
[y_dst],
[z_dst],
])

PT[0][0] += theta_dots.item(0) / 100
PT[1][0] += theta_dots.item(1) / 100
PT[2][0] += theta_dots.item(2) / 100
PT[3][0] += theta_dots.item(3) / 100
PT[4][0] += theta_dots.item(4) / 100


sorry for the bad indentation, I do now know how to do it.

thank you very much in advance