Odtnhj

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_trans*j3_trans*j4_trans)[0:3, 3]
 j4_pos = (j1_trans*j2_trans*j3_trans)[0:3, 3]
 j3_pos = (j1_trans*j2_trans)[0:3, 3]
 j2_pos = (j1_trans)[0:3, 3]
 j1_pos = np.zeros((3,1))

 pos3D = np.zeros(3)

 pos3D = (ee_pos-j1_pos).T
 z0_vector = [0, 1, 0]
 jacobian[0:3, 0] = np.cross(z0_vector, pos3D)
 pos3D[0:3] = (ee_pos-j2_pos).T

 z1_vector = (j1_trans*np.array([0, 0, 1, 0]).reshape(4,1))[0:3].T

 jacobian[0:3, 1] = np.cross(z1_vector, pos3D)
 pos3D[0:3] = (ee_pos-j3_pos).T

 z2_vector = (j1_trans*j2_trans*np.array([0, 0, 1, 0]).reshape(4,1))[0:3].T

 jacobian[0:3, 2] = np.cross(z2_vector, pos3D)
 pos3D[0:3] = (ee_pos-j4_pos).T

 z3_vector = (j1_trans*j2_trans*j3_trans*np.array([0, 1, 0, 0]).reshape(4,1))[0:3].T

 jacobian[0:3, 3] = np.cross(z3_vector, pos3D)

 jacobian[3:6, 0] = z0_vector
 jacobian[3:6, 1] = z1_vector
 jacobian[3:6, 2] = z2_vector
 jacobian[3:6, 3] = z3_vector


 return jacobian