up vote 0 down vote favorite 2 I am really struggling to find the right Jacobian for my 4DOF robot arm in 3D-space. First joint rotates around the y-axis, second around z-axis, third around z-axis and last one around the y-axis. All links are 1 unit long. My problem is probably the orientation vector zi because I know that my Forward Kinematics are correct. j1_pos - j4_pos are the positions of the joints. ee_pos is the end-effector. All positions are correct. My thought was just to multiply the respective axis vectors with the transformation matrices to get the orientation vectors zi . Any advice would be much appreciated. I am using the formula J = [Jpi] = [zi x (pe - pi)] [Joi] [ zi ] def Jacobian(self,joint_angles): jacobian = np.zeros((6,4)) j1_trans = self.link_transform_y(joint_angles[0]) j2_trans = self.link_transform_z(joint_angles[1]) j3_trans = self.link_transform_z(joint_angles[2]) j4_trans = self.link_transform_y(joint_angles[3]) ee_pos = (j1_trans*j2_...