robodk.py 文件源码

def Pose_2_KUKA(H):
    """Converts a pose (4x4 matrix) to a Kuka target

    :param H: pose
    :type H: :class:`.Mat`"""
    x = H[0,3]
    y = H[1,3]
    z = H[2,3]
    if (H[2,0]) > (1.0 - 1e-6):
        p = -pi/2
        r = 0
        w = atan2(-H[1,2],H[1,1])
    elif (H[2,0]) < (-1.0 + 1e-6):
        p = pi/2
        r = 0
        w = atan2(H[1,2],H[1,1])
    else:
        p = atan2(-H[2,0],sqrt(H[0,0]*H[0,0]+H[1,0]*H[1,0]))
        w = atan2(H[1,0],H[0,0])
        r = atan2(H[2,1],H[2,2])
    return [x, y, z, w*180/pi, p*180/pi, r*180/pi]