Борьба с якобианской обратной кинематикой

Question

Я действительно изо всех сил пытаюсь найти подходящий якобиан для моей руки робота 4DOF в 3D-пространстве. Первый шарнир вращается вокруг оси y, второй вокруг оси z, третий вокруг оси z и последний вокруг оси y. Все ссылки имеют длину 1 единицу. Моя проблема, вероятно, вектор ориентации zi потому что я знаю, что моя прямая кинематика верна. j1_pos-j4_pos позиции суставов. ee_pos является конечным эффектором. Все позиции верны. Я думал просто умножить соответствующие векторы осей на матрицы преобразования, чтобы получить векторы ориентации. zi,

Любые советы будут высоко ценится.

Я использую формулу

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