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.
for more information please follow this link --> https://www.youtube.com/watch?v=SefTCXrpL8U&t=872s
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_1), math.radians(90.0), 0, L_1],
[math.radians(THETA_2), 0, L_2, 0],
[math.radians(THETA_3), 0, L_3, 0],
[math.radians(THETA_4) + math.radians(90.0), math.radians(90.0), 0, 0],
[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
