def get_current_configuration_id(self, joint_angles):
    T06 = self.forward_kinematics.compute_0_to_6_matrix(joint_angles)
    solution = self.compute_joint_angles(T06)
    differences = np.full(8, 1000)
    if solution.solution_shoulder_left.is_valid_solution:
        if solution.solution_shoulder_left.solution_wrist_up.is_valid_solution:
            if solution.solution_shoulder_left.solution_wrist_up.solution_elbow_up.is_valid_solution:
                differences[0] = 0
            if solution.solution_shoulder_left.solution_wrist_up.solution_elbow_down.is_valid_solution:
                differences[1] = 0
        if solution.solution_shoulder_left.solution_wrist_down.is_valid_solution:
            if solution.solution_shoulder_left.solution_wrist_down.solution_elbow_up.is_valid_solution:
                differences[2] = 0
            if solution.solution_shoulder_left.solution_wrist_down.solution_elbow_down:
                differences[3] = 0
    if solution.solution_shoulder_right.is_valid_solution:
        if solution.solution_shoulder_right.solution_wrist_up.is_valid_solution:
            if solution.solution_shoulder_right.solution_wrist_up.solution_elbow_up.is_valid_solution:
                differences[4] = 0
            if solution.solution_shoulder_right.solution_wrist_up.solution_elbow_down.is_valid_solution:
                differences[5] = 0
        if solution.solution_shoulder_right.solution_wrist_down.is_valid_solution:
            if solution.solution_shoulder_right.solution_wrist_down.solution_elbow_up.is_valid_solution:
                differences[6] = 0
            if solution.solution_shoulder_right.solution_wrist_down.solution_elbow_down.is_valid_solution:
                differences[7] = 0
    for i in range(6):
        if solution.solution_shoulder_left.is_valid_solution:
            if solution.solution_shoulder_left.solution_wrist_up.is_valid_solution:
                if solution.solution_shoulder_left.solution_wrist_up.solution_elbow_up.is_valid_solution:
                    differences[0] += abs((joint_angles[i] - solution.solution_shoulder_left.solution_wrist_up.solution_elbow_up.thetas[i]))
                if solution.solution_shoulder_left.solution_wrist_up.solution_elbow_down.is_valid_solution:
                    differences[1] += abs((joint_angles[i] - solution.solution_shoulder_left.solution_wrist_up.solution_elbow_down.thetas[i]))
            if solution.solution_shoulder_left.solution_wrist_down.is_valid_solution:
                if solution.solution_shoulder_left.solution_wrist_down.solution_elbow_up.is_valid_solution:
                    differences[2] += abs((joint_angles[i] - solution.solution_shoulder_left.solution_wrist_down.solution_elbow_up.thetas[i]))
                if solution.solution_shoulder_left.solution_wrist_down.solution_elbow_down:
                    differences[3] += abs((joint_angles[i] - solution.solution_shoulder_left.solution_wrist_down.solution_elbow_down.thetas[i]))
        if solution.solution_shoulder_right.is_valid_solution:
            if solution.solution_shoulder_right.solution_wrist_up.is_valid_solution:
                if solution.solution_shoulder_right.solution_wrist_up.solution_elbow_up.is_valid_solution:
                    differences[4] += abs((joint_angles[i] - solution.solution_shoulder_right.solution_wrist_up.solution_elbow_up.thetas[i]))
                if solution.solution_shoulder_right.solution_wrist_up.solution_elbow_down.is_valid_solution:
                    differences[5] += abs((joint_angles[i] - solution.solution_shoulder_right.solution_wrist_up.solution_elbow_down.thetas[i]))
            if solution.solution_shoulder_right.solution_wrist_down.is_valid_solution:
                if solution.solution_shoulder_right.solution_wrist_down.solution_elbow_up.is_valid_solution:
                    differences[6] += abs((joint_angles[i] - solution.solution_shoulder_right.solution_wrist_down.solution_elbow_up.thetas[i]))
                if solution.solution_shoulder_right.solution_wrist_down.solution_elbow_down.is_valid_solution:
                    differences[7] += abs((joint_angles[i] - solution.solution_shoulder_right.solution_wrist_down.solution_elbow_down.thetas[i]))
    print(differences)
    return np.argmin(differences)
def get_current_configuration_id(self, joint_angles):
    T06 = self.forward_kinematics.compute_0_to_6_matrix(joint_angles)
    solution = self.compute_joint_angles(T06)
    differences = np.full(8, 1000)
    if solution.solution_shoulder_left.is_valid_solution:
        if solution.solution_shoulder_left.solution_wrist_up.is_valid_solution:
            if solution.solution_shoulder_left.solution_wrist_up.solution_elbow_up.is_valid_solution:
                differences[0] = 0
            if solution.solution_shoulder_left.solution_wrist_up.solution_elbow_down.is_valid_solution:
                differences[1] = 0
        if solution.solution_shoulder_left.solution_wrist_down.is_valid_solution:
            if solution.solution_shoulder_left.solution_wrist_down.solution_elbow_up.is_valid_solution:
                differences[2] = 0
            if solution.solution_shoulder_left.solution_wrist_down.solution_elbow_down:
                differences[3] = 0
    if solution.solution_shoulder_right.is_valid_solution:
        if solution.solution_shoulder_right.solution_wrist_up.is_valid_solution:
            if solution.solution_shoulder_right.solution_wrist_up.solution_elbow_up.is_valid_solution:
                differences[4] = 0
            if solution.solution_shoulder_right.solution_wrist_up.solution_elbow_down.is_valid_solution:
                differences[5] = 0
        if solution.solution_shoulder_right.solution_wrist_down.is_valid_solution:
            if solution.solution_shoulder_right.solution_wrist_down.solution_elbow_up.is_valid_solution:
                differences[6] = 0
            if solution.solution_shoulder_right.solution_wrist_down.solution_elbow_down.is_valid_solution:
                differences[7] = 0
    for i in range(6):
        if solution.solution_shoulder_left.is_valid_solution:
            if solution.solution_shoulder_left.solution_wrist_up.is_valid_solution:
                if solution.solution_shoulder_left.solution_wrist_up.solution_elbow_up.is_valid_solution:
                    differences[0] += abs((joint_angles[i] - solution.solution_shoulder_left.solution_wrist_up.solution_elbow_up.thetas[i]))
                if solution.solution_shoulder_left.solution_wrist_up.solution_elbow_down.is_valid_solution:
                    differences[1] += abs((joint_angles[i] - solution.solution_shoulder_left.solution_wrist_up.solution_elbow_down.thetas[i]))
            if solution.solution_shoulder_left.solution_wrist_down.is_valid_solution:
                if solution.solution_shoulder_left.solution_wrist_down.solution_elbow_up.is_valid_solution:
                    differences[2] += abs((joint_angles[i] - solution.solution_shoulder_left.solution_wrist_down.solution_elbow_up.thetas[i]))
                if solution.solution_shoulder_left.solution_wrist_down.solution_elbow_down:
                    differences[3] += abs((joint_angles[i] - solution.solution_shoulder_left.solution_wrist_down.solution_elbow_down.thetas[i]))
        if solution.solution_shoulder_right.is_valid_solution:
            if solution.solution_shoulder_right.solution_wrist_up.is_valid_solution:
                if solution.solution_shoulder_right.solution_wrist_up.solution_elbow_up.is_valid_solution:
                    differences[4] += abs((joint_angles[i] - solution.solution_shoulder_right.solution_wrist_up.solution_elbow_up.thetas[i]))
                if solution.solution_shoulder_right.solution_wrist_up.solution_elbow_down.is_valid_solution:
                    differences[5] += abs((joint_angles[i] - solution.solution_shoulder_right.solution_wrist_up.solution_elbow_down.thetas[i]))
            if solution.solution_shoulder_right.solution_wrist_down.is_valid_solution:
                if solution.solution_shoulder_right.solution_wrist_down.solution_elbow_up.is_valid_solution:
                    differences[6] += abs((joint_angles[i] - solution.solution_shoulder_right.solution_wrist_down.solution_elbow_up.thetas[i]))
                if solution.solution_shoulder_right.solution_wrist_down.solution_elbow_down.is_valid_solution:
                    differences[7] += abs((joint_angles[i] - solution.solution_shoulder_right.solution_wrist_down.solution_elbow_down.thetas[i]))
    print(differences)
    return np.argmin(differences)