Optimal Trajectory Generation