Ndtyjky

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