Constrained Multi-Level Algorithm for Trajectory Optimization