与雅可比逆运动学作斗争

Question

我真的很努力为3D空间中的4DOF机器人手臂找到合适的Jacobian。第一个关节绕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