# Optimizing Trajectories with Closed-Loop Dynamic SQP

arXiv: https://arxiv.org/abs/2109.07081 · HTML: https://mlsanity.com/paper/2109.07081/

## Key facts

- Cited by 5 papers in the map (data through 14 Oct 2026).
- First posted on arXiv 15 Sep 2021.

## Built on by (most cited first, 5 of 5)

- [Robots That Ask For Help: Uncertainty Alignment for Large Language Model Planners](https://mlsanity.com/paper/2307.01928/) (2023)
- [Robotic Table Wiping via Reinforcement Learning and Whole-body Trajectory Optimization](https://mlsanity.com/paper/2210.10865/) (2022)
- [Mnemosyne: Learning to Train Transformers with Transformers](https://mlsanity.com/paper/2302.01128/) (2023)
- [Differentiable Model Predictive Control on the GPU](https://mlsanity.com/paper/2510.06179/) (2025)
- [IR-STP: Enhancing Autonomous Driving with Interaction Reasoning in Spatio-Temporal Planning](https://mlsanity.com/paper/2311.02850/) (2023)

## Abstract (arXiv)

Indirect trajectory optimization methods such as Differential Dynamic Programming (DDP) have found considerable success when only planning under dynamic feasibility constraints. Meanwhile, nonlinear programming (NLP) has been the state-of-the-art approach when faced with additional constraints (e.g., control bounds, obstacle avoidance). However, a na"ive implementation of NLP algorithms, e.g., shooting-based sequential quadratic programming (SQP), may suffer from slow convergence -- caused from natural instabilities of the underlying system manifesting as poor numerical stability within the optimization. Re-interpreting the DDP closed-loop rollout policy as a sensitivity-based correction to a second-order search direction, we demonstrate how to compute analogous closed-loop policies (i.e., feedback gains) for constrained problems. Our key theoretical result introduces a novel dynamic programming-based constraint-set recursion that augments the canonical "cost-to-go" backward pass. On the algorithmic front, we develop a hybrid-SQP algorithm incorporating DDP-style closed-loop rollouts, enabled via efficient parallelized computation of the feedback gains. Finally, we validate our theoretical and algorithmic contributions on a set of increasingly challenging benchmarks, demonstrating significant improvements in convergence speed over standard open-loop SQP.