1
$\begingroup$

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.

enter image description here enter image description here enter image description here

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

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.