Towards reliable real-time trajectory optimization