  Trajectory optimization is a powerful tool for robot motion planning and
control. State-of-the-art general-purpose nonlinear programming solvers are
versatile, handle constraints effectively and provide a high numerical
robustness, but they are slow because they do not fully exploit the optimal
control problem structure at hand. Existing structure-exploiting solvers are
fast, but they often lack techniques to deal with nonlinearity or rely on
penalty methods to enforce (equality or inequality) path constraints. This work
presents Fatrop: a trajectory optimization solver that is fast and benefits
from the salient features of general-purpose nonlinear optimization solvers.
The speed-up is mainly achieved through the integration of a specialized linear
solver, based on a Riccati recursion that is generalized to also support
stagewise equality constraints. To demonstrate the algorithm's potential, it is
benchmarked on a set of robot problems that are challenging from a numerical
perspective, including problems with a minimum-time objective and no-collision
constraints. The solver is shown to solve problems for trajectory generation of
a quadrotor, a robot manipulator and a truck-trailer problem in a few tens of
milliseconds. The algorithm's C++-code implementation accompanies this work as
open source software, released under the GNU Lesser General Public License
(LGPL). This software framework may encourage and enable the robotics community
to use trajectory optimization in more challenging applications.
