def compute_joint_angles(self, T06, debug=False):
    solution = InverseKinematicsSolution()
    P05 = np.dot(T06, [0, 0, (- self.joint6_dh.d), 1])
    phi_1 = math.atan2(P05[1], P05[0])
    phi_2_cos = (self.joint4_dh.d / math.sqrt(((P05[0] ** 2) + (P05[1] ** 2))))
    phi_2 = math.acos(phi_2_cos)
    theta_1_1 = ((phi_1 + phi_2) + (np.pi / 2))
    theta_1_2 = ((phi_1 - phi_2) + (np.pi / 2))
    if debug:
        print(f'Theta 1: {theta_1_1:.3f}, {theta_1_2:.3f}')
    if (not math.isnan(theta_1_1)):
        solution.solution_shoulder_left = self.__compute_solution_for_theta_1(T06, theta_1_1, debug)
    else:
        solution.solution_shoulder_left = InverseKinematicsWristSolution().is_valid_solution = False
    if (not math.isnan(theta_1_2)):
        solution.solution_shoulder_right = self.__compute_solution_for_theta_1(T06, theta_1_2, debug)
    else:
        solution.solution_shoulder_right = InverseKinematicsWristSolution().is_valid_solution = False
    if debug:
        self.__print_all_solutions(solution)
    return solution
def compute_joint_angles(self, T06, debug=False):
    solution = InverseKinematicsSolution()
    P05 = np.dot(T06, [0, 0, (- self.joint6_dh.d), 1])
    phi_1 = math.atan2(P05[1], P05[0])
    phi_2_cos = (self.joint4_dh.d / math.sqrt(((P05[0] ** 2) + (P05[1] ** 2))))
    phi_2 = math.acos(phi_2_cos)
    theta_1_1 = ((phi_1 + phi_2) + (np.pi / 2))
    theta_1_2 = ((phi_1 - phi_2) + (np.pi / 2))
    if debug:
        print(f'Theta 1: {theta_1_1:.3f}, {theta_1_2:.3f}')
    if (not math.isnan(theta_1_1)):
        solution.solution_shoulder_left = self.__compute_solution_for_theta_1(T06, theta_1_1, debug)
    else:
        solution.solution_shoulder_left = InverseKinematicsWristSolution().is_valid_solution = False
    if (not math.isnan(theta_1_2)):
        solution.solution_shoulder_right = self.__compute_solution_for_theta_1(T06, theta_1_2, debug)
    else:
        solution.solution_shoulder_right = InverseKinematicsWristSolution().is_valid_solution = False
    if debug:
        self.__print_all_solutions(solution)
    return solution