Efficient And Robust Time Optimal