Optimization Based Motion Planning With Obstacles And Priorities