A Family of Iterative Gauss-Newton Shooting Methods for Nonlinear Optimal Control

This paper introduces a family of iterative algorithms for unconstrained nonlinear optimal control. We generalize the well-known iLQR algorithm to different multiple-shooting variants, combining advantages like straight-forward initialization and a closed-loop forward integration. All algorithms have similar computational complexity, i.e. linear complexity in the time horizon, and can be derived in the same computational framework. We compare the full-step variants of our algorithms and present several simulation examples, including a high-dimensional underactuated robot subject to contact switches. Simulation results show that our multiple-shooting algorithms can achieve faster convergence, better local contraction rates and much shorter runtimes than classical iLQR, which makes them a superior choice for nonlinear model predictive control applications.Comment: 8 page

Refraction-corrected ray-based inversion for three-dimensional ultrasound tomography of the breast

Ultrasound Tomography has seen a revival of interest in the past decade, especially for breast imaging, due to improvements in both ultrasound and computing hardware. In particular, three-dimensional ultrasound tomography, a fully tomographic method in which the medium to be imaged is surrounded by ultrasound transducers, has become feasible. In this paper, a comprehensive derivation and study of a robust framework for large-scale bent-ray ultrasound tomography in 3D for a hemispherical detector array is presented. Two ray-tracing approaches are derived and compared. More significantly, the problem of linking the rays between emitters and receivers, which is challenging in 3D due to the high number of degrees of freedom for the trajectory of rays, is analysed both as a minimisation and as a root-finding problem. The ray-linking problem is parameterised for a convex detection surface and three robust, accurate, and efficient ray-linking algorithms are formulated and demonstrated. To stabilise these methods, novel adaptive-smoothing approaches are proposed that control the conditioning of the update matrices to ensure accurate linking. The nonlinear UST problem of estimating the sound speed was recast as a series of linearised subproblems, each solved using the above algorithms and within a steepest descent scheme. The whole imaging algorithm was demonstrated to be robust and accurate on realistic data simulated using a full-wave acoustic model and an anatomical breast phantom, and incorporating the errors due to time-of-flight picking that would be present with measured data. This method can used to provide a low-artefact, quantitatively accurate, 3D sound speed maps. In addition to being useful in their own right, such 3D sound speed maps can be used to initialise full-wave inversion methods, or as an input to photoacoustic tomography reconstructions

The Control Toolbox - An Open-Source C++ Library for Robotics, Optimal and Model Predictive Control

We introduce the Control Toolbox (CT), an open-source C++ library for efficient modeling, control, estimation, trajectory optimization and Model Predictive Control. The CT is applicable to a broad class of dynamic systems but features interfaces to modeling tools specifically designed for robotic applications. This paper outlines the general concept of the toolbox, its main building blocks, and highlights selected application examples. The library contains several tools to design and evaluate controllers, model dynamical systems and solve optimal control problems. The CT was designed for intuitive modeling of systems governed by ordinary differential or difference equations. It supports rapid prototyping of cost functions and constraints and provides standard interfaces for different optimal control solvers. To date, we support Single Shooting, the iterative Linear-Quadratic Regulator, Gauss-Newton Multiple Shooting and classical Direct Multiple Shooting. We provide interfaces to general purpose NLP solvers and Riccati-based linear-quadratic optimal control solvers. The CT was designed to solve large-scale optimal control and estimation problems efficiently and allows for online control of dynamic systems. Some of the key features to enable fast run-time performance are full compatibility with Automatic Differentiation, derivative code generation, and multi-threading. Still, the CT is designed as a modular framework whose building blocks can also be used for other control and estimation applications such as inverse dynamics control, extended Kalman filters or kinematic planning

A Simple and Efficient Algorithm for Nonlinear Model Predictive Control

We present PANOC, a new algorithm for solving optimal control problems arising in nonlinear model predictive control (NMPC). A usual approach to this type of problems is sequential quadratic programming (SQP), which requires the solution of a quadratic program at every iteration and, consequently, inner iterative procedures. As a result, when the problem is ill-conditioned or the prediction horizon is large, each outer iteration becomes computationally very expensive. We propose a line-search algorithm that combines forward-backward iterations (FB) and Newton-type steps over the recently introduced forward-backward envelope (FBE), a continuous, real-valued, exact merit function for the original problem. The curvature information of Newton-type methods enables asymptotic superlinear rates under mild assumptions at the limit point, and the proposed algorithm is based on very simple operations: access to first-order information of the cost and dynamics and low-cost direct linear algebra. No inner iterative procedure nor Hessian evaluation is required, making our approach computationally simpler than SQP methods. The low-memory requirements and simple implementation make our method particularly suited for embedded NMPC applications

FATROP : A Fast Constrained Optimal Control Problem Solver for Robot Trajectory Optimization and Control

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 in an effective way 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 works 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 use 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

Crocoddyl: An Efficient and Versatile Framework for Multi-Contact Optimal Control

We introduce Crocoddyl (Contact RObot COntrol by Differential DYnamic Library), an open-source framework tailored for efficient multi-contact optimal control. Crocoddyl efficiently computes the state trajectory and the control policy for a given predefined sequence of contacts. Its efficiency is due to the use of sparse analytical derivatives, exploitation of the problem structure, and data sharing. It employs differential geometry to properly describe the state of any geometrical system, e.g. floating-base systems. We have unified dynamics, costs, and constraints into a single concept-action-for greater efficiency and easy prototyping. Additionally, we propose a novel multiple-shooting method called Feasibility-prone Differential Dynamic Programming (FDDP). Our novel method shows a greater globalization strategy compared to classical Differential Dynamic Programming (DDP) algorithms, and it has similar numerical behavior to state-of-the-art multiple-shooting methods. However, our method does not increase the computational complexity typically encountered by adding extra variables to describe the gaps in the dynamics. Concretely, we propose two modifications to the classical DDP algorithm. First, the backward pass accepts infeasible state-control trajectories. Second, the rollout keeps the gaps open during the early "exploratory" iterations (as expected in multiple-shooting methods). We showcase the performance of our framework using different tasks. With our method, we can compute highly-dynamic maneuvers for legged robots (e.g. jumping, front-flip) in the order of milliseconds

Contact-Implicit Trajectory Optimization Based on a Variable Smooth Contact Model and Successive Convexification

In this paper, we propose a contact-implicit trajectory optimization (CITO) method based on a variable smooth contact model (VSCM) and successive convexification (SCvx). The VSCM facilitates the convergence of gradient-based optimization without compromising physical fidelity. On the other hand, the proposed SCvx-based approach combines the advantages of direct and shooting methods for CITO. For evaluations, we consider non-prehensile manipulation tasks. The proposed method is compared to a version based on iterative linear quadratic regulator (iLQR) on a planar example. The results demonstrate that both methods can find physically-consistent motions that complete the tasks without a meaningful initial guess owing to the VSCM. The proposed SCvx-based method outperforms the iLQR-based method in terms of convergence, computation time, and the quality of motions found. Finally, the proposed SCvx-based method is tested on a standard robot platform and shown to perform efficiently for a real-world application.Comment: Accepted for publication in ICRA 201

Solution of Ordinary Differential Equations in Gradient-Based Multidisciplinary Design Optimization

A gradient-based approach to multidisciplinary design optimization enables efficient scalability to large numbers of design variables. However, the need for derivatives causes difficulties when integrating ordinary differential equations (ODEs) in models. To simplify this, we propose the use of the general linear methods framework, which unifies all Runge-Kutta and linear multistep methods. This approach enables rapid implementation of integration methods without the need to differentiate each one, even in a gradient-based optimization context. We also develop a new parallel time integration algorithm that enables vectorization across time steps. We present a set of benchmarking results using a stiff ODE, a non-stiff nonlinear ODE, and an orbital dynamics ODE, and compare integration methods. In a modular gradient-based multidisciplinary design optimization context, we find that the new parallel time integration algorithm with high-order implicit methods, especially Gauss-Legendre collocation, is the best choice for a broad range of problems

Improving Tatonnement Methods of Solving Heterogeneous Agent Models

This paper modifies standard block Gauss-Seidel iterations used by tatonnement methods for solving large scale deterministic heterogeneous agent models. The composite method between first- and second-order tatonnement methods is shown to considerably improve convergence both in terms of speed as well as robustness relative to conventional first-order tatonnement methods. In addition, the relative advantage of the modified algorithm increases in the size and complexity of the economic model. Therefore, the algorithm allows significant reductions in computational time when solving large models. The algorithm is particularly attractive since it is easy to implement – it only augments conventional and intuitive tatonnement iterations with standard numerical methods.