Feature/jeking multi goal planning

Added PlanToConfigurations function that allows passing a list of goals to cbirrt.