def __compute_solution_for_theta_1(self, T06, theta_1, debug=False):
    wrist_solution = InverseKinematicsWristSolution()
    P06 = T06[:, 3]
    theta_5_1 = None
    theta_5_2 = None
    theta_5_cos = ((((P06[0] * math.sin(theta_1)) - (P06[1] * np.cos(theta_1))) - self.joint4_dh.d) / self.joint6_dh.d)
    theta_5_cos = self.__clamp_cos_sin_within_threshold(theta_5_cos)
    if ((- 1) <= theta_5_cos <= 1):
        theta_5_1 = math.acos(theta_5_cos)
        theta_5_2 = (- math.acos(theta_5_cos))
    sigma = 1e-05
    if ((theta_5_1 is not None) and (not ((- sigma) <= math.sin(theta_5_1) <= sigma))):
        wrist_solution.solution_wrist_up = self.__compute_solution_for_wrist(theta_1, theta_5_1, T06)
    else:
        wrist_solution.solution_wrist_up.is_valid_solution = False
    if ((theta_5_2 is not None) and (not ((- sigma) <= math.sin(theta_5_2) <= sigma))):
        wrist_solution.solution_wrist_down = self.__compute_solution_for_wrist(theta_1, theta_5_2, T06)
    else:
        wrist_solution.solution_wrist_down.is_valid_solution = False
    if ((not wrist_solution.solution_wrist_up.is_valid_solution) and (not wrist_solution.solution_wrist_down.is_valid_solution)):
        wrist_solution.is_valid_solution = False
    if debug:
        print(f'Theta 5: {theta_5_1:.3f}, {theta_5_2:.3f}')
    return wrist_solution
def __compute_solution_for_theta_1(self, T06, theta_1, debug=False):
    wrist_solution = InverseKinematicsWristSolution()
    P06 = T06[:, 3]
    theta_5_1 = None
    theta_5_2 = None
    theta_5_cos = ((((P06[0] * math.sin(theta_1)) - (P06[1] * np.cos(theta_1))) - self.joint4_dh.d) / self.joint6_dh.d)
    theta_5_cos = self.__clamp_cos_sin_within_threshold(theta_5_cos)
    if ((- 1) <= theta_5_cos <= 1):
        theta_5_1 = math.acos(theta_5_cos)
        theta_5_2 = (- math.acos(theta_5_cos))
    sigma = 1e-05
    if ((theta_5_1 is not None) and (not ((- sigma) <= math.sin(theta_5_1) <= sigma))):
        wrist_solution.solution_wrist_up = self.__compute_solution_for_wrist(theta_1, theta_5_1, T06)
    else:
        wrist_solution.solution_wrist_up.is_valid_solution = False
    if ((theta_5_2 is not None) and (not ((- sigma) <= math.sin(theta_5_2) <= sigma))):
        wrist_solution.solution_wrist_down = self.__compute_solution_for_wrist(theta_1, theta_5_2, T06)
    else:
        wrist_solution.solution_wrist_down.is_valid_solution = False
    if ((not wrist_solution.solution_wrist_up.is_valid_solution) and (not wrist_solution.solution_wrist_down.is_valid_solution)):
        wrist_solution.is_valid_solution = False
    if debug:
        print(f'Theta 5: {theta_5_1:.3f}, {theta_5_2:.3f}')
    return wrist_solution