The Jacobian Matrix in Robotics & Python - Denavit Hartenberg & Universal Robots

This Video derives and reviews the Jacobian Matrix used in the field of Robotics and it shows how to program / implement the Jacobian in Python with example code. It explains the Denavit Hartenberg Parameters and shows an Universal Robot Simulation. # Creators: „Bass Paranoya“ & „Red Rubin“ from the „University Munich“ # Sources: internal documents for the master's degree in electrical engineering: - „Ausarbeitung Robotik - Jacobi Matrix“ from N. Z. (Red Rubin) & N. B. (Bass Paranoya) - lecture slices robotics module from Prof. Dr. G. S. external (free) documents: - John Craig: http://www.mech.sharif.ir/c/document_library/get_file?uuid=5a4bb247-1430-4e46-942c-d692dead831f&groupId=14040 - Universal Robots: https://www.universal-robots.com/ # Music by Bass Paranoya # Python Code: - „RoboLib“: def jacobian_matrix(q: np.ndarray, dh_para: np.ndarray, runden = False) -"""insert spitze klammer here""" np.array: # code in the video decription Jacobian = np.zeros((6, 6)) T_0_6 = fk_ur(q, dh_para) # transformation matrix of the system (forward kinematics) point_end = T_0_6[0:3, 3] # calculate the TCP origin coordinates T_0_i = np.array([[1, 0, 0, 0], # create T_0_0; needed for for-loop [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) for i in range(6): if i == 0: # kinematic chain T_0_i = T_0_i # adds velocity of previous joint to current joint else: # using the DH parameters T = dh(dh_para[i-1, 0], dh_para[i-1, 1], dh_para[i-1, 2], q[i-1]) T_0_i = np.dot(T_0_i, T) z_i = T_0_i[0:3, 2] # gets the vectors p_i and z_i for the Jacobian from the last two coloums of the transformation matrices p_i = T_0_i[0:3, 3] r = point_end - p_i Jacobian[0:3, i] = np.cross(z_i, r) # linear portion Jacobian[3:6, i] = z_i # angular portion ## each time the loop is passed, another column of the Jacobi matrix is filled if runden: Jacobian[0:3, i] = np.round(np.cross(z_i, r), 3) # round if True Jacobian[3:6, i] = np.round(z_i, 3) return Jacobian def fk_ur(q: np.ndarray, dh_para: np.ndarray) - np.ndarray: """ Forward Kinematics for UR type robots :param: dh_para: DH-Transformation, table of dh parameters (alpha, a, d, theta) :param: q: Gelenkwinkel """ T_0_6 = np.zeros((4, 4)) dh_params_count = dh_para.shape[1] number_dh_trafos = dh_para.shape[0] if dh_params_count != 4: print("Wrong number of dh parameters!") return None trafo_matrizes = [] for i in range(number_dh_trafos): trafo_matrizes.append(dh(dh_para[i, 0], dh_para[i, 1], dh_para[i, 2], q[i])) if len(trafo_matrizes) != 0: for i in range(len(trafo_matrizes) - 1): if i == 0: T_0_6 = trafo_matrizes[i] @ trafo_matrizes[i+1] else: T_0_6 = T_0_6 @ trafo_matrizes [i+1] return T_0_6 - „Testing“: print("STARTING: ", datetime.now()) # Denavit Hartenberg Parameter: a = [0.00000, -0.24365, -0.21325, 0.00000, 0.00000, 0.0000] d = [0.1519, 0.00000, 0.00000, 0.11235, 0.08535, 0.0819] alpha = [1.570796327, 0, 0, 1.570796327, -1.570796327, 0] q_home_offset = [0, -1.570796327, 0, -1.570796327, 0, 0] joint_direction = [1, 1, -1, 1, 1, 1] dh_paras = np.array([alpha, a, d, q_home_offset]).T q = np.array([0, 0, 0, 0, 0, 0]) / 180 * np.pi # Singularitätsrechnung # q = np.array([10, -90, 90, 0, 90, 0]) J = robolib.jacobian_matrix(q, dh_paras, runden=True) # d = np.linalg.det(J) print("\n\n\nJacobian Matrix:\n",J) tcp = np.dot(J, np.array([0.67, 0, 0, 0, 0, 0])) print("\n\n\nTCP velocities:\n",tcp)