starting an optimal planner (currently simple copy of carrot_planner)