Kinematic model for a 5axis mill with universal (nutating) head

# returns the A/B joint positions for a given tool-orientation vector
# NOTE: These formulas are specific to the machine
def kins_calc_jnt_angles(self, tool_z_req):
    epsilon = 0.000001
    print("Vector tool-z-requested",tool_z_req)
    # using xyzabc-trsrn kinematics (ALL ANGLES ARE IN RADIANS)
    (Kwx, Kwy, Kwz) = (tool_z_req[0], tool_z_req[1], tool_z_req[2])

    # on this particular machine the tool cannot be oriented if the requested tool-z vector
    # has a negative y-component (ie Kwy)
    if Kwz > 1 - epsilon:
        return (0 ,0)

    elif Kwy < 0:
        print("Error -> Tool-z-requested is outside of this kinematic, aborting 'kins_calc_jnt_angles()'")
        return(None, None)

    # in this particular kinematic there are infinite solutions for tool_z vector =[0,1,0]
    # so we need to check if the requested tool_z vector is sufficiently close to it
    elif Kwx > 1 - epsilon:
        A_j = 0
        B_j = pi/2
        return(A_j, B_j)

    elif Kwx < -1 + epsilon:
        A_j = 0
        B_j = -pi/2
        return(A_j, B_j)

    elif Kwy > 1 - epsilon:
        A_j = pi  # note: we can choose the rotation direction of the joint with the sign (+/-pi)
        B_j = 0
        return(A_j, B_j)

    elif Kwz < -1 + epsilon:
        A_j = 0
        B_j = pi  # note: we can choose the rotation direction of the joint with the sign (+/-pi)
        return(A_j, B_j)

    # note: handling of individual quadrants is required
    if Kwx >= 0:
        A_j = pi - acos(2*Kwy - 1)
        Sa_j  = sin(A_j)
        Ca_j  = cos(A_j)
        if Kwz >= 0:
            if  -(sqrt(2)*Kwz*Sa_j) <= Sa_j*Sa_j:
                B_j = (- asin(2*(sqrt(2)*Kwz*Sa_j - (Ca_j + 1)*Kwx)/(Ca_j*Ca_j
                       + 2*Sa_j*Sa_j + 2*Ca_j + 1)))
                quad = 111
            else: # Kwz <  0 AND Kwz < 0
                B_j = (pi + asin(2*(sqrt(2)*Kwz*Sa_j - (Ca_j + 1)*Kwx)/(Ca_j*Ca_j
                       + 2*Sa_j*Sa_j + 2*Ca_j + 1)))
                quad = 112
        else: # Kwz < 0
            A_j =  pi + acos(2*Kwy - 1);
            Sa_j  = sin(A_j);
            Ca_j  = cos(A_j);
            if  -(sqrt(2)*Kwz*Sa_j) <= Sa_j*Sa_j:
                B_j = (pi + asin(2*(sqrt(2)*Kwz*Sa_j - (Ca_j + 1)*Kwx)/(Ca_j*Ca_j
                       + 2*Sa_j*Sa_j + 2*Ca_j + 1)))
                quad = 121
            else: # Kwz <  0 AND Kwz < 0
                B_j = (pi - asin(2*(sqrt(2)*Kwz*Sa_j - (Ca_j + 1)*Kwx)/(Ca_j*Ca_j
                       + 2*Sa_j*Sa_j + 2*Ca_j + 1)))
                quad = 122

    else: # Kwx < 0
        A_j = - pi + acos(2*Kwy - 1);
        Sa_j  = sin(A_j);
        Ca_j  = cos(A_j);
        if Kwz >= 0:
            if sqrt(2)*Kwz*Sa_j <= Sa_j*Sa_j:
                B_j = (- asin(2*(sqrt(2)*Kwz*Sa_j - (Ca_j + 1)*Kwx)/(Ca_j*Ca_j
                       + 2*Sa_j*Sa_j + 2*Ca_j + 1)))
                quad = 211
            else:
                B_j = (- pi + asin(2*(sqrt(2)*Kwz*Sa_j - (Ca_j + 1)*Kwx)/(Ca_j*Ca_j
                       + 2*Sa_j*Sa_j + 2*Ca_j + 1)))
                quad = 212
        else:
            A_j = - pi - acos(2*Kwy - 1);
            Sa_j  = sin(A_j);
            Ca_j  = cos(A_j);
            if sqrt(2)*Kwz*Sa_j <= Sa_j*Sa_j:
                B_j = (-pi + asin(2*(sqrt(2)*Kwz*Sa_j - (Ca_j + 1)*Kwx)/(Ca_j*Ca_j
                       + 2*Sa_j*Sa_j + 2*Ca_j + 1)))
                quad = 221
            else:
                B_j = (- pi + asin(2*(sqrt(2)*Kwz*Sa_j - (Ca_j + 1)*Kwx)/(Ca_j*Ca_j
                       + 2*Sa_j*Sa_j + 2*Ca_j + 1)))
                quad = 222
    #if Kwz < 0:
    #    B_j = -B_j
    #    return(A_j, B_j)


    print("QUADRANT: ",quad)
    # return angles in radians
    return(A_j, B_j)