def __compute_specific_solution(self, theta_1, theta_3, theta_5, theta_6, tm_dict, debug=False):
    specific_solution = InverseKinematicsSpecificSolution()
    P14 = tm_dict['P14']
    phi_1 = math.atan2((- P14[2]), (- P14[0]))
    phi_2 = math.asin((((- self.joint4_dh.a) * math.sin(theta_3)) / math.sqrt(((P14[0] ** 2) + (P14[2] ** 2)))))
    theta_2 = (phi_1 - phi_2)
    if debug:
        print(f'Theta 2: {theta_2:.3f}')
    T01 = tm_dict['T01']
    T12 = self.compute_transformation_matrix(theta_2, self.joint2_dh)
    T23 = self.compute_transformation_matrix(theta_3, self.joint3_dh)
    T45 = tm_dict['T45']
    T56 = tm_dict['T56']
    T06 = tm_dict['T06']
    T03 = np.matmul(np.matmul(T01, T12), T23)
    T30 = np.linalg.inv(T03)
    T64 = tm_dict['T64']
    T34 = np.matmul(np.matmul(T30, T06), T64)
    X34 = T34[:, 0]
    theta_4 = math.atan2(X34[1], X34[0])
    if debug:
        print(f'Theta 4: {theta_4:.3f}')
    specific_solution.thetas = [theta_1, theta_2, theta_3, theta_4, theta_5, theta_6]
    return specific_solution
def __compute_specific_solution(self, theta_1, theta_3, theta_5, theta_6, tm_dict, debug=False):
    specific_solution = InverseKinematicsSpecificSolution()
    P14 = tm_dict['P14']
    phi_1 = math.atan2((- P14[2]), (- P14[0]))
    phi_2 = math.asin((((- self.joint4_dh.a) * math.sin(theta_3)) / math.sqrt(((P14[0] ** 2) + (P14[2] ** 2)))))
    theta_2 = (phi_1 - phi_2)
    if debug:
        print(f'Theta 2: {theta_2:.3f}')
    T01 = tm_dict['T01']
    T12 = self.compute_transformation_matrix(theta_2, self.joint2_dh)
    T23 = self.compute_transformation_matrix(theta_3, self.joint3_dh)
    T45 = tm_dict['T45']
    T56 = tm_dict['T56']
    T06 = tm_dict['T06']
    T03 = np.matmul(np.matmul(T01, T12), T23)
    T30 = np.linalg.inv(T03)
    T64 = tm_dict['T64']
    T34 = np.matmul(np.matmul(T30, T06), T64)
    X34 = T34[:, 0]
    theta_4 = math.atan2(X34[1], X34[0])
    if debug:
        print(f'Theta 4: {theta_4:.3f}')
    specific_solution.thetas = [theta_1, theta_2, theta_3, theta_4, theta_5, theta_6]
    return specific_solution