def __compute_solution_for_wrist(self, theta_1, theta_5, T06, debug=False):
    shoulder_solution = InverseKinematicsShoulderSolution()
    T60 = np.linalg.inv(T06)
    X60 = T60[:, 0]
    Y60 = T60[:, 1]
    theta_6_cos = (((X60[0] * math.sin(theta_1)) - (Y60[0] * math.cos(theta_1))) / math.sin(theta_5))
    theta_6_sin = ((((- X60[1]) * math.sin(theta_1)) + (Y60[1] * math.cos(theta_1))) / math.sin(theta_5))
    theta_6 = math.atan2(theta_6_sin, theta_6_cos)
    if debug:
        print(f'Theta 6: {theta_6:.3f}')
    tm_dict = {}
    T01 = self.compute_transformation_matrix(theta_1, self.joint1_dh)
    T45 = self.compute_transformation_matrix(theta_5, self.joint5_dh)
    T56 = self.compute_transformation_matrix(theta_6, self.joint6_dh)
    T46 = np.matmul(T45, T56)
    T64 = np.linalg.inv(T46)
    T10 = np.linalg.inv(T01)
    T14 = np.matmul(np.matmul(T10, T06), T64)
    P14 = T14[:, 3]
    tm_dict['T06'] = T06
    tm_dict['T01'] = T01
    tm_dict['T45'] = T45
    tm_dict['T56'] = T56
    tm_dict['T64'] = T64
    tm_dict['T10'] = T10
    tm_dict['T14'] = T14
    tm_dict['P14'] = P14
    theta_3_cos = ((((math.sqrt(((P14[0] ** 2) + (P14[2] ** 2))) ** 2) - (self.joint3_dh.a ** 2)) - (self.joint4_dh.a ** 2)) / ((2 * (- self.joint3_dh.a)) * (- self.joint4_dh.a)))
    if debug:
        print('theta3_cos: ', theta_3_cos)
    theta_3_cos = self.__clamp_cos_sin_within_threshold(theta_3_cos)
    if (not ((- 1) <= theta_3_cos <= 1)):
        shoulder_solution.is_valid_solution = False
        return shoulder_solution
    theta_3_up = math.acos(theta_3_cos)
    theta_3_down = (- math.acos(theta_3_cos))
    if debug:
        print(f'Theta 3: Up: {theta_3_up:.3f} Down: {theta_3_down:.3f}')
    shoulder_solution.solution_elbow_up = self.__compute_specific_solution(theta_1, theta_3_up, theta_5, theta_6, tm_dict)
    shoulder_solution.solution_elbow_down = self.__compute_specific_solution(theta_1, theta_3_down, theta_5, theta_6, tm_dict)
    return shoulder_solution
def __compute_solution_for_wrist(self, theta_1, theta_5, T06, debug=False):
    shoulder_solution = InverseKinematicsShoulderSolution()
    T60 = np.linalg.inv(T06)
    X60 = T60[:, 0]
    Y60 = T60[:, 1]
    theta_6_cos = (((X60[0] * math.sin(theta_1)) - (Y60[0] * math.cos(theta_1))) / math.sin(theta_5))
    theta_6_sin = ((((- X60[1]) * math.sin(theta_1)) + (Y60[1] * math.cos(theta_1))) / math.sin(theta_5))
    theta_6 = math.atan2(theta_6_sin, theta_6_cos)
    if debug:
        print(f'Theta 6: {theta_6:.3f}')
    tm_dict = {}
    T01 = self.compute_transformation_matrix(theta_1, self.joint1_dh)
    T45 = self.compute_transformation_matrix(theta_5, self.joint5_dh)
    T56 = self.compute_transformation_matrix(theta_6, self.joint6_dh)
    T46 = np.matmul(T45, T56)
    T64 = np.linalg.inv(T46)
    T10 = np.linalg.inv(T01)
    T14 = np.matmul(np.matmul(T10, T06), T64)
    P14 = T14[:, 3]
    tm_dict['T06'] = T06
    tm_dict['T01'] = T01
    tm_dict['T45'] = T45
    tm_dict['T56'] = T56
    tm_dict['T64'] = T64
    tm_dict['T10'] = T10
    tm_dict['T14'] = T14
    tm_dict['P14'] = P14
    theta_3_cos = ((((math.sqrt(((P14[0] ** 2) + (P14[2] ** 2))) ** 2) - (self.joint3_dh.a ** 2)) - (self.joint4_dh.a ** 2)) / ((2 * (- self.joint3_dh.a)) * (- self.joint4_dh.a)))
    if debug:
        print('theta3_cos: ', theta_3_cos)
    theta_3_cos = self.__clamp_cos_sin_within_threshold(theta_3_cos)
    if (not ((- 1) <= theta_3_cos <= 1)):
        shoulder_solution.is_valid_solution = False
        return shoulder_solution
    theta_3_up = math.acos(theta_3_cos)
    theta_3_down = (- math.acos(theta_3_cos))
    if debug:
        print(f'Theta 3: Up: {theta_3_up:.3f} Down: {theta_3_down:.3f}')
    shoulder_solution.solution_elbow_up = self.__compute_specific_solution(theta_1, theta_3_up, theta_5, theta_6, tm_dict)
    shoulder_solution.solution_elbow_down = self.__compute_specific_solution(theta_1, theta_3_down, theta_5, theta_6, tm_dict)
    return shoulder_solution