Robust And Efficient Trajectory Planning For