Abstract:In this work, we present several tools for efficient sequential hierarchical least-squares programming (S-HLSP) for lexicographical optimization tailored to robot control and planning. As its main step, S-HLSP relies on approximations of the original non-linear hierarchical least-squares programming (NL-HLSP) to a hierarchical least-squares programming (HLSP) by the hierarchical Newton's method or the hierarchical Gauss-Newton algorithm. We present a threshold adaptation strategy for appropriate switches between the two. This ensures optimality of infeasible constraints, promotes numerical stability when solving the HLSP's and enhances optimality of lower priority levels by avoiding regularized local minima. We introduce the solver $\mathcal{N}$ADM$_2$, an alternating direction method of multipliers for HLSP based on nullspace projections of active constraints. The required basis of nullspace of the active constraints is provided by a computationally efficient turnback algorithm for system dynamics discretized by the Euler method. It is based on an upper bound on the bandwidth of linearly independent column subsets within the linearized constraint matrices. Importantly, an expensive initial rank-revealing matrix factorization is unnecessary. We show how the high sparsity of the basis in the fully-actuated case can be preserved in the under-actuated case. $\mathcal{N}$ADM$_2$ consistently shows faster computations times than competing off-the-shelf solvers on NL-HLSP composed of test-functions and whole-body trajectory optimization for fully-actuated and under-actuated robotic systems. We demonstrate how the inherently lower accuracy solutions of the alternating direction method of multipliers can be used to warm-start the non-linear solver for efficient computation of high accuracy solutions to non-linear hierarchical least-squares programs.

An Almost Feasible Sequential Linear Programming Algorithm

A Feasible Sequential Linear Programming Algorithm with Application to Time-Optimal Path Planning Problems

A GLOBALLY AND SUPERLIEARLY CONVERGENT FEASIBLE SQP ALGORITHM FOR DEGENERATE CONSTRAINED OPTIMIZATION

Particle Swarm Optimization Algorithm for Constrained Problems

A feasible adaptive refinement algorithm for linear semi-infinite optimization

Sequential Feasible Optimal Power Flow: Theoretical Basis and Numerical Implementation

A Method of Sequential Log-Convex Programming for Engineering Design

An Efficient Algorithm for Linear Semi-Infinite Programming over Positive Polynomials

A nonlinear programming algorithm for finite element limit analysis using feasible arc searching technique

An iterative algorithm to solve a linear fractional programming problem

An Effective Shuffled Frog-Leaping Algorithm for the Flexible Job-Shop Scheduling Problem

Improved SQP and SLSQP Algorithms for Feasible Path-based Process Optimisation

Sequential conservative integer programming method for multi-constrained discrete variable structure topology optimization

A global optimization algorithm for linear fractional programming

A Sequential Reduction Algorithm for the Large-Scale Fixed-Charge Network Flow Problems

Efficient Lexicographic Optimization for Prioritized Robot Control and Planning

Sequential Global Linear Programming Algorithm for Continuous Piecewise Linear Programming

Distributed Asynchronous Mixed-Integer Linear Programming with Feasibility Guarantees

A Sequential Follower Refinement Algorithm for Robust Surgery Scheduling

A Global Solution Algorithm for AC Optimal Power Flow through Linear Constrained Quadratic Programming