Model-predictive control (MPC) is an increasingly popular method for controlling complex robotic systems in which optimal control problems are solved on board the robot at real-time rates. However, successful application of MPC depends critically on the performance of the algorithms used to solve the underlying optimization problems. An ideal solver should both leverage the structure of the MPC problem and support efficient "warm starting" so that information from previous solutions can be recycled to speed convergence. We present ALTRO-C, a high-performance solver with both of these properties that utilizes an augmented Lagrangian method to handle general convex conic constraints. We demonstrate the new solver’s superior performance against several existing state-of-the-art solvers on a variety of benchmark control problems formulated as both quadratic and second-order cone programs.

Trajectory optimization is a widely used tool for robot motion planning and control. Existing solvers for these problems either rely on off-the-shelf nonlinear programming solvers that are numerically robust and capable of handling arbitrary constraints, but tend to be slow because they are general purpose; or they use custom numerical methods that take advantage of the problem structure to be fast, but often lack robustness and have limited or no ability to reason about constraints. This paper presents ALTRO (Augmented Lagrangian TRajectory optimizer), a solver for constrained trajectory optimization problems that handles general nonlinear state and input constraints and offers fast convergence and numerical robustness thanks to careful exploitation of problem structure. We demonstrate its performance on a set of benchmark motion-planning problems and offer comparisons to the standard direct collocation method with large-scale sequential quadratic programming and interior-point solvers.