Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

Efficient Lexicographic Optimization for Prioritized Robot Control and Planning

Kai Pfeiffer, Abderrahmane Kheddar

TL;DR

It is shown how the high sparsity of the basis in the fully-actuated case can be preserved in the under-actuated case, and 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.

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.

Efficient Lexicographic Optimization for Prioritized Robot Control and Planning

TL;DR

It is shown how the high sparsity of the basis in the fully-actuated case can be preserved in the under-actuated case, and 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.

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 ADM, 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. ADM 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.
Paper Structure (33 sections, 2 theorems, 47 equations, 17 figures, 5 tables, 2 algorithms)

This paper contains 33 sections, 2 theorems, 47 equations, 17 figures, 5 tables, 2 algorithms.

Table of Contents

  1. Introduction
  2. Context and contribution
  3. Non-linear programming
  4. Prioritized robot control and planning
  5. Overview
  6. Problem definition and contributions
  7. Non-linear Hierarchical Least-Squares Programming
  8. Sequential Hierarchical Least-Squares Programming
  9. Prioritized trajectory optimization
  10. Hierarchical step-filter with adaptive threshold for second order information
  11. The hierarchical step-filter
  12. Adaptive SOI thresholding
  13. Alternating direction method of multipliers for HLSP
  14. Reduced Hessian based ADMM for HLSP
  15. Choice of the step-size parameters $\rho$
  16. ...and 18 more sections

Key Result

theorem 1

If $B_t$ and $E_t$ (or namely, $M_t$ and $S_t^T$) with $t=0,\dots,T$ are of full column rank $r_{B_t} = n_{\tau}$ and $r_{E_t} = n_{q}$, the basis of nullspace of $A\coloneqq \nabla_xf_{dyn}$eq:dxeid is of rank $r_{Z} = T(n_{\tau}+n_{\gamma})$. The linear independent sub-sets of $A$ are banded withi

Figures (17)

  • Figure 1: A symbolic overview of the sequential hierarchical least-squares programming (S-HLSP) with trust region and hierarchical step-filter (HSF) based on the SQP step-filter fletcher2002b to solve non-linear hierarchical least-squares programmings \ref{['eq:nlhlsp']} with $p$ levels. Our contributions, an adaptive threshold for second-order information and the HLSP sub-problem solver $\mathcal{N}$ ADM$_2$ in combination with an efficient turnback algorithm for Euler integrated dynamics, are marked in blue.
  • Figure 2: Iterates $(h_{\cup l-1}(x_k),\Vert f_l^+(x_k)\Vert_2$ of level $l$. A new filter front needs to lie within the shaded area.
  • Figure 3: Gradient and permuted gradients of the Euler integrated dynamics. The top matrix shows the un-permuted case. Matrices which only appear in the explicit case and in the implicit case are colored in orange and in yellow, respectively. The control matrices $B$ are colored in blue. Matrices of full column rank according to theorem \ref{['th:band']} are printed in bold. The middle and bottom matrices show the permuted subsets for $\mu=0$ in the explicit and for $\mu=1$ in the implicit case, respectively. These permuted column subsets are linearly independent to all other columns of $\nabla_x f_{dyn}$.
  • Figure 4: Computation time $t_{tb}$, number of non-zeros (nnz) and density ($\varphi$) of $Z^TZ$ of the turnback nullspace $Z$ for Euler integrated dynamics with $n_q = n_{\dot{q}} = 22$ and $n_{\gamma} = 24$ in dependence of control horizon $T$ and under-actuation $n_{ua}$.
  • Figure 5: Non-linear test functions, data for the different HLSP sub-solvers over S-HLSP outer iteration: computation times per HLSP solve, number of inner iterations, KKT residuals and overall number of non-zeros handled throughout the whole hierarchy.
  • ...and 12 more figures

Theorems & Definitions (6)

  • definition 1: Filter front
  • theorem 1
  • proof
  • definition 2
  • theorem 2
  • proof