Time Optimal Trajectory Planning