Source code for pykin.kinematics.jacobian

import numpy as np

[docs]def calc_jacobian(frames: list, transformations: dict, jsize: int) -> np.array: """ Args: frames (list): frames to compute jacobian transformations (dict): transformations from forward kinematics thetas (int): size of joint space Returns: Jacobian (np.array(6, jsize)): return Jacobian """ target_position = list(transformations.values())[-1].pos J = np.zeros((6, jsize)) n = 0 for frame in frames: if frame.joint.dtype == "revolute": n += 1 w = np.dot(transformations[frame.link.name].h_mat[:3, :3], frame.joint.axis) v = np.cross(w, target_position - transformations[frame.link.name].pos) J[:, n - 1] = np.hstack((v, w)) elif frame.joint.dtype == "prismatic": n += 1 w = np.zeros(3) v = np.dot(transformations[frame.link.name].h_mat[:3, :3], frame.joint.axis) J[:, n - 1] = np.hstack((v, w)) return J