Added PlanToConfigurations to CBiRRT

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