betaFeedback

WorksComputer Science

Large-Scale Hierarchical Decision-Making in Robot Control via ADMM

Abstract

Sparse hierarchical nonlinear programming (SH-NLP) based on the ℓ0\ell_0-norm provides a powerful framework for integrated, efficient hierarchical decision-making in robot kinematic control, for example in the presence of a large set of possible end-effector candidate locations. While Sequential Sparse Hierarchical Quadratic Programming (S-SHQP) with interior-point method based SHQP sub-problem solvers exploit the hierarchical, sparse structure of such problems, they are less amenable to effective warm-starting between subsequent, coherent control steps. In this work, we formulate an Alternating Direction Method of Multipliers (ADMM) to efficiently solve related SHQP sub-problems. We introduce efficient substitutions for auxiliary variables representing ℓ0\ell_0-norm surrogates. The solver efficiently handles separable decision functions by evaluating linear algebra expressions in the appropriate order. Results in large-scale kinematic and dynamic control show reduced computation times of up to 23.5% compared with a structure-exploiting interior-point method in low- to moderate-accuracy regimes. Furthermore, hierarchical decision-making is extended to whole-body dynamic control for humanoid walking, where the need for high-accuracy solutions is highlighted.

Introduction

Hierarchical decision-making in robot control frequently requires selecting among a large number of candidate actions or locations while simultaneously satisfying whole-body kinematic and physical constraints of different priorities. Examples include selecting footholds [1], or activating only a subset of joints in highly redundant robotic systems [2], all while respecting higher or lower priority constraints [3]. Sparse hierarchical nonlinear programming (SH-NLP) [4] provides an attractive formulation for such problems, as discrete decisions can be embedded directly into continuous programs without explicitly introducing combinatorial decision variables.

SH-NLPs can be addressed by iteratively solving sparse hierarchical quadratic programs (SHQP), for example by the interior-point method (IPM) [4]. By exploiting the sparse hierarchical structure, a high degree of efficiency with linear dependency in the number of sparse candidate constraints can be achieved, unlike standard QP solvers with cubic dependency.

However, exploitable structure exists also between individual SHQP subproblems. Robot control typically consists of high-frequency updates with gradual changes between subsequent instances, for example similar sets of active and

inactive inequality constraints. The IPM is less amenable to exploiting this additional sequential coherence through warm-starting.

The Alternating Direction Method of Multipliers (ADMM) [5] is a suitable candidate for such parametric programs [6], as its primal and dual can be effectively initialized from previous optimization instances. However, specialized solver formulations for SHQPs do not exist. This work aims to close this gap with the following main contributions:

  • We derive a structure-exploiting ADMM solver for SHQPs which eliminates sparsity-related auxiliary variables for linear dependence on the number of decision candidates, while enabling warm-starting, unlike existing IPM based methods.

  • For separable decision functions, we exploit their structure and implement computationally favorable linear algebra evaluations, which improves efficiency for large candidate sets.

  • We evaluate the proposed solver on large-scale sparse programs and robot-control problems, isolating the effects of sparse structure, warm-starting, and separable decision-function evaluation. The results demonstrate reduced computation times over the structure-exploiting IPM [4] in low- to moderate-accuracy regimes.

  • We investigate the extension of hierarchical decision-making from kinematic to dynamic robot control. The experiments demonstrate efficient drone control, while a humanoid walking experiment exposes the need for high-accuracy solutions, which is less suitable for first-order methods like ADMM.

The remainder of this paper is organized as follows. Sec. II reviews related work on optimization methods for hierarchical decision-making in robot control. Sec. III recalls the SHQP formulation arising within S-SHQP. Sec. IV derives the proposed efficient ADMM solver, and compares it with other SHQP solvers. Sec. V discusses equilibration and warm-starting, while Sec. VI outlines how structure in separable decision functions can be exploited. Sec. VII evaluates the computational performance and scalability of the proposed solver on numerical and robot control problems.

Hierarchical Decision-Making in Robotics

Mixed-integer formulations provide a powerful tool for globally optimal decision-making in robot planning [7], but their combinatorial complexity can limit their applicability to large candidate sets and real-time control. Instead, sparsity-promoting continuous ℓ1\ell_1-norm formulations have been efficiently applied to parsimonious kinematic control [8] and footstep selection [1]. However, these methods either rely on specialized algorithms like the simplex method or additional brute-force layers to achieve accurate sparsity.

The approach in [9] enhances the sparsity-promoting qualities of the ℓ1\ell_1-norm by sequentially reweighting the linear objective according to a logarithm-based ℓ0\ell_0-norm surrogate. The ℓ0\ell_0-norm directly measures cardinality and therefore provides a natural representation of sparse decision-making, but needs to be smoothly approximated for continuous optimization. While [9] is limited to linear unconstrained problems, the approach has been extended to hierarchical nonlinear decision-making by incorporation within a SQP-type framework (Sequential Quadratic Programming [10]; from hereon referred to as Sequential Sparse Hierarchical Quadratic Programming (S-SHQP) [4]). The corresponding dedicated IPM exploits the sparse hierarchical structure of the SHQP sub-problems, resulting in linear computational complexity with the number of candidate constraints. However, warm-starting is not exploited, and the solver has only been applied to purely kinematic control scenarios.

This work addresses this gap and proposes an ADMM solver which can be effectively warm-started for the efficient repeated solution of subsequent SHQP subproblems. Furthermore, hierarchical decision-making is integrated within a humanoid whole-body controller under consideration of instantaneous robot dynamics.

Parametric Quadratic Programming

Repeatedly solving closely related parametric optimization problems is fundamental to real-time robot control. Active-set methods like qpOASES [11] are well suited to exploit this similarity to accelerate subsequent optimizations, because the active set of a preceding QP often provides a good estimate for that of the subsequent problem in high-frequency control. However, active-set methods can suffer from issues like repeated constraint activation and deactivation (cycling) when constraints are linearly dependent, leading to long computation times.

Interior-point methods are numerically robust alternatives to the active-set method, for example by integrating proximal regularization which enables stable convergence even in the case of linearly dependent constraints [12]. An extension to SHQPs, which are a member of the broad class of QPs, has been proposed in [4]. Linear dependence on the number of decision candidates as in the unconstrained case [13] is maintained by efficient exploitation of the underlying sparse hierarchical structure introduced by ℓ0\ell_0-norm surrogates.

Warm-starting interior-point methods is less direct than for active-set methods. Some techniques modify previous solutions to meet central feasibility conditions before restarting the method [14], which limits the direct use of a preceding solution.

Alternating Direction Method of Multipliers (ADMM)

First-order methods such as ADMM update primal and dual variables through gradient and projection operations with factorization reuse at the cost of reduced accuracy, rather than repeatedly forming and factorizing KKT systems as required by second-order methods such as the IPM. Their computational efficiency can be further improved through effective warm-starting and by caching factorizations and other quantities which remain invariant across inner iterations [15] and consecutive control steps [6]. ADMM has been successfully deployed as an SQP subproblem solver for complex whole-body trajectory optimization, while efficiently exploiting temporal sparsity of optimal control problems [16].

In this work, we adapt the ADMM to the specific sparse and hierarchical structure of SHQPs in order to maintain linear dependency of the solver in the number of candidates in hierarchical decision-making.

Background

Sparse Hierarchical Nonlinear Programming for Hierarchical Decision-Making

This work formulates a continuous optimization problem for hierarchical decision-making with a more aggressive sparsity promoting approximation of the ℓ0\ell_0-norm with strong gradient decay [17] (see Figure 2). This can be advantageous in the presence of a large number of candidates, when perpetual influence as seen for the logarithmic penalty ∑log⁡(τd2+ξ)\sum\log(\tau_d^2+\xi) [4, 9] is undesired. For completeness, and unlike the work in [4], we also consider ℓ2\ell_2-norm objectives:

Comparison between continuous log and fraction function surrogates of the $\ell_0$-norm on squared decision functions

Figure 2. Comparison between continuous log [9] and fraction function surrogates [17] of the ℓ0\ell_0-norm on squared decision functions d2d^2 (see (1)).

min⁡χ,(νd)d∈Dl,(τd)d∈Dl,(νi)i∈Cl∑d∈Dlτdτd+ξ+∥(νi)∥22(1)\min_{\chi,(\nu_d)_{d\in\mathcal{D}_l},(\tau_d)_{d\in\mathcal{D}_l},(\nu_i)_{i\in\mathcal{C}_l}} \sum_{d\in\mathcal{D}_l}\frac{\tau_d}{\tau_d+\xi}+\lVert(\nu_i)\rVert_2^2 \tag*{(1)}

s.t.  −τd−≤νd≤τd+,d±∈Dl±,l=1,…,p\ -\tau_{d-}\leq\nu_{d}\leq\tau_{d+},\quad d^{\pm}\in\mathcal{D}_l^{\pm},\quad l=1,\ldots,p

fd/i/j/k(χ)≥νd/i/k(∗),d/i/j/k∈Dl/Cl/I∪l−1/A∪l−1f_{d/i/j/k}(\chi)\geq\nu_{d/i/k}^{(*)},\quad d/i/j/k\in\mathcal{D}_l/\mathcal{C}_l/\mathcal{I}_{\cup l-1}/\mathcal{A}_{\cup l-1}

χ∈Rn\chi\in\mathbb{R}^n is the optimization variable, for example representing the robot joint state. The functions fd/i/j/k(χ)f_{d/i/j/k}(\chi) denote nonlinear robot constraints associated with the index sets d∈Dld\in\mathcal{D}_l, i∈Cli\in\mathcal{C}_l, j∈I∪l−1j\in\mathcal{I}_{\cup l-1}, or k∈A∪l−1k\in\mathcal{A}_{\cup l-1}. Each scalar entry dd in Dl\mathcal{D}_l represents a decision candidate, for example a squared expression of kinematic translational reaching g(χ)g(\chi) with a constant desired value gdg_d (see Sec. VI on how this separable structure can be exploited computationally)

fd(χ)=12∥g(χ)−gd∥22,d∈Dl(2)f_d(\chi)=\frac{1}{2}\lVert g(\chi)-g_d\rVert_2^2,\qquad d\in\mathcal{D}_l \tag*{(2)}

The set Cl\mathcal{C}_l contains least-squares tasks, while inactive or active tasks from higher priority levels are represented in I∪l−1/A∪l−1:=I1/A1∪⋯∪Il−1/Al−1\mathcal{I}_{\cup l-1}/\mathcal{A}_{\cup l-1}:=\mathcal{I}_1/\mathcal{A}_1\cup\cdots\cup\mathcal{I}_{l-1}/\mathcal{A}_{l-1}, respectively. These sets may include both equality and inequality constraints, collectively denoted by the relation ≥\geq.

The constraint sets Dl\mathcal{D}_l and Cl\mathcal{C}_l are associated with a slack variable νd/i\nu_{d/i}, which permits relaxation when exact satisfaction is not possible, for example when a task specifies an unreachable target. On each priority level ll, the slack is optimized until the optimal one νd/i∗\nu_{d/i}^{*} is found, in which case the associated constraints enter the active set of the next level A∪l\mathcal{A}_{\cup l} with fixed optimal slack νk∗\nu_k^{*}. The auxiliary variable τd\tau_d, together with the appropriate bound constraint set D±\mathcal{D}^{\pm} on νd\nu_d, provides a smooth representation of the elementwise magnitude ∣νd∣\lvert\nu_d\rvert. For compactness, parentheses denote the stacked vector of quantities indexed by a given set, for example (νd)d∈Dl=[νd1νd2⋯ ]T∈R∣Dl∣(\nu_d)_{d\in\mathcal{D}_l}=[\nu_{d_1}\quad\nu_{d_2}\quad\cdots]^T\in\mathbb{R}^{|\mathcal{D}_l|}.

In the following, the explicit set memberships d∈Dld\in\mathcal{D}_l, i∈Cli\in\mathcal{C}_l, j∈I∪l−1j\in\mathcal{I}_{\cup l-1}, and k∈A∪l−1k\in\mathcal{A}_{\cup l-1} are omitted whenever they are clear from the context, for example (fd):=(fd)d∈Dl(f_d):=(f_d)_{d\in\mathcal{D}_l}.

Sequential Sparse Hierarchical Quadratic Programming

S-HNLP is solved by iteratively solving SHQP subproblems [4] for the approximate step variables x(l)x^{(l)}, ν(l)\nu^{(l)}, and t(l)t^{(l)}. The step x(l)x^{(l)} is subject to a trust region of radius ϕ\phi for globalization. In this work focusing on instantaneous robot control, the trust region is kept constant while a filter framework as in [18, 19] is omitted. SHQP is a constrained version of the reweighted majorization-minimization ℓ1\ell_1-optimization problem of [9]:

min⁡zl,(vd),(td),(ti)∑d∈Dlωdtd+∥(vi)∥22+0.5(xl−1∗+Nl−1zl)THl(xl−1∗+Nl−1zl)s.t.− td−≤vd±≤td+,d±∈Dl±a~dTzl−b~d≥vd,d∈Dl,a~iTzl−b~i≥vi,i∈Cla~jTzl−b~j≥0,j∈I∪l−1∣xl∣≤ϕ\begin{aligned} \min_{z_l,(v_d),(t_d),(t_i)}\quad&\sum_{d\in\mathcal{D}_l}\omega_dt_d+\lVert(v_i)\rVert_2^2\\ &+0.5(x_{l-1}^{*}+N_{l-1}z_l)^TH_l(x_{l-1}^{*}+N_{l-1}z_l)\\ \text{s.t.}\quad&-\ t_{d-}\leq v_{d\pm}\leq t_{d+},\quad d^{\pm}\in\mathcal{D}_l^{\pm}\\ &\tilde{a}_d^Tz_l-\tilde{b}_d\geq v_d,\quad d\in\mathcal{D}_l,\\ &\tilde{a}_i^Tz_l-\tilde{b}_i\geq v_i,\quad i\in\mathcal{C}_l\\ &\tilde{a}_j^Tz_l-\tilde{b}_j\geq0,\quad j\in\mathcal{I}_{\cup l-1}\\ &\lvert x_l\rvert\leq\phi \end{aligned}
∣xl∣≤ϕ(3)\lvert x_l\rvert\leq\phi \tag*{(3)}

The weights are defined as follows, with a small constant ξ≈1⋅10−4\xi\approx1\cdot10^{-4} which improves numerical robustness:

ωd=ξ(τd+ξ)2>0(4)\omega_d=\frac{\xi}{(\tau_d+\xi)^2}>0 \tag*{(4)}

The positive-definite hierarchical Lagrangian Hessian Hl≻0H_l\succ0 [20] captures second-order terms of the constraints and objectives, while ad:=∇fd(χ(l))a_d:=\nabla f_d(\chi^{(l)}) and bd:=fd(χ(l))b_d:=f_d(\chi^{(l)}) represent linearizations of S-HNLP at the current iterate ll. Following the approaches in [21, 4], we introduced the change of variables: xl=xl−1∗+Nl−1zlx_l=x_{l-1}^{*}+N_{l-1}z_l with the overall primal solution: x∗=xpx^{*}=x_p. Here, xl−1∗x_{l-1}^{*} is the optimal primal of levels 11 to l−1l-1, and Nl−1∈Rn×nrN_{l-1}\in\mathbb{R}^{n\times n_r} is a nullspace basis of the vectors ak∈Rna_k\in\mathbb{R}^n (k∈I∪l−1k\in\mathcal{I}_{\cup l-1}; N0=IN_0=I). A vector ad/i/ja_{d/i/j} or matrix Ad/i/jA_{d/i/j} marked by A~d/i/j\widetilde{A}_{d/i/j} represents the projected value A~d/i/j:=Ad/i/jNl−1\widetilde{A}_{d/i/j}:=A_{d/i/j}N_{l-1} (or H~l:=Nl−1THlNl−1\widetilde{H}_l:=N_{l-1}^TH_lN_{l-1}). The value bdb_d denotes the nullspace projection offset from previous priority levels: b~d:=bd−adTxl−1∗\widetilde{b}_d:=b_d-a_d^Tx_{l-1}^{*}. zl∈Rnrz_l\in\mathbb{R}^{n_r} is the projected primal of level ll, with nr<nn_r<n for efficiency.

Below are the primal and dual residuals KλK_{\lambda} and KqlK_{q_l} as the derivative Kl:=∇LlK_l:=\nabla\mathcal{L}_l of the Lagrangian Ll\mathcal{L}_l of a level ll of SHQP [4]. For convenience, we define the placeholder Λ\Lambda

Λ∈{d±,d,i,j}(5)\Lambda\in\{d^{\pm},d,i,j\} \tag*{(5)}

such that λΛ\lambda_{\Lambda} denotes the Lagrange multiplier associated with the corresponding constraint class. Specifically, λΛ\lambda_{\Lambda} represents λd±\lambda_{d\pm}, λd\lambda_d, λi\lambda_i, or λj\lambda_j for Λ=d±,d,i,j\Lambda=d^{\pm},d,i,j, respectively. Additionally, the constraint vectors a~d\widetilde{a}_d, a~i\widetilde{a}_i, and a~j\widetilde{a}_j are stacked row-wise into the matrices A~d\widetilde{A}_d, A~i\widetilde{A}_i, and A~j\widetilde{A}_j, according to their respective index sets Dl\mathcal{D}_l, Cl\mathcal{C}_l, and I∪l−1\mathcal{I}_{\cup l-1}. The ℓ0\ell_0-norm auxiliary constraints Dl±\mathcal{D}_l^{\pm}, the inequality constraints in Dl\mathcal{D}_l and Cl\mathcal{C}_l, and I∪l−1\mathcal{I}_{\cup l-1} are relaxed by positive slack variables wΛ≥0w_{\Lambda}\geq0 (marked by checkered boxes):

Kλ=[(td+)−(vd+)−(wd+)(td−)+(vd−)−(wd−)−A~dzl+(b~d)+(vd)+(wd)−A~izl+(b~i)+(vi)+(wi)−A~jzl+(b~j)+(wj)](6)K_{\lambda}= \begin{bmatrix} (t_{d+})-(v_{d+})-(w_{d+})\\ (t_{d-})+(v_{d-})-(w_{d-})\\ -\widetilde{A}_dz_l+(\widetilde{b}_d)+(v_d)+(w_d)\\ -\widetilde{A}_iz_l+(\widetilde{b}_i)+(v_i)+(w_i)\\ -\widetilde{A}_jz_l+(\widetilde{b}_j)+(w_j) \end{bmatrix} \tag*{(6)}
Kql=[H~lzl+Nl−1THlxl−1∗−A~dT(λd)−A~iT(λi)−A~jT(λj)(λd+)−(λd−)+(λd)(ωd)−(λd+)−(λd−)(vi)−(λi)](7)K_{q_l}= \begin{bmatrix} \widetilde{H}_lz_l+N_{l-1}^TH_lx_{l-1}^{*}-\widetilde{A}_d^T(\lambda_d)-\widetilde{A}_i^T(\lambda_i)-\widetilde{A}_j^T(\lambda_j)\\ (\lambda_{d+})-(\lambda_{d-})+(\lambda_d)\\ (\omega_d)-(\lambda_{d+})-(\lambda_{d-})\\ (v_i)-(\lambda_i) \end{bmatrix} \tag*{(7)}

An ADMM for SHQP

ADMM solves problems of the following form [15]:

min⁡ql,sl,z~l12qlTIlql+glTql+I+(sl)\min_{q_l,s_l,\widetilde{z}_l}\frac{1}{2}q_l^TI_lq_l+g_l^Tq_l+I_+(s_l)

s.t. Blql+Clsl=cl,zl=z~lB_lq_l+C_ls_l=c_l,\quad z_l=\widetilde{z}_l

{#eq:6}

Applying it to SHQP, we get the following variable and split variable vectors qlq_l and sls_l (excluding z~l\widetilde{z}_l):

ql:=[zlT(vd)T(td)T(vi)T]Tq_l:=\left[z_l^T\quad(v_d)^T\quad(t_d)^T\quad(v_i)^T\right]^T

sl:=[(wd+)T(wd)T(wi)T(wj)T]Ts_l := \left[(w_{d+})^T\quad(w_d)^T\quad(w_i)^T\quad(w_j)^T\right]^T

The slack variables relax inequality constraints, while the operator I+\mathbb{I}_+ projects them onto the set of positive numbers C+C_+. The matrices and vectors are composed as:

Jl:=[H~l00000000000000I],gl:=[00(wd)0]J_l := \begin{bmatrix} \widetilde{H}_l & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & I \end{bmatrix}, \qquad g_l := \begin{bmatrix} 0 \\ 0 \\ (w_d) \\ 0 \end{bmatrix}
Bl:=[Bd+Bd−BdBiBj]:=[0−II00II0A~d−I00A~i00−IA~j000],cl:=[(cd+)(cd−)(cd)(ci)(cj)]:=[(b~d)0(b~i)(b~j)]B_l := \begin{bmatrix} B_{d+} \\ B_{d-} \\ B_d \\ B_i \\ B_j \end{bmatrix} := \begin{bmatrix} 0 & -I & I & 0 \\ 0 & I & I & 0 \\ \widetilde{A}_d & -I & 0 & 0 \\ \widetilde{A}_i & 0 & 0 & -I \\ \widetilde{A}_j & 0 & 0 & 0 \end{bmatrix}, \qquad c_l := \begin{bmatrix} (c_{d+}) \\ (c_{d-}) \\ (c_d) \\ (c_i) \\ (c_j) \end{bmatrix} := \begin{bmatrix} (\widetilde{b}_d) \\ 0 \\ (\widetilde{b}_i) \\ (\widetilde{b}_j) \end{bmatrix}
Cl:=[Cd+Cd−CdCiCj]:=[−I00000−I00000I00000I00000I]C_l := \begin{bmatrix} C_{d+} \\ C_{d-} \\ C_d \\ C_i \\ C_j \end{bmatrix} := \begin{bmatrix} -I & 0 & 0 & 0 & 0 \\ 0 & -I & 0 & 0 & 0 \\ 0 & 0 & I & 0 & 0 \\ 0 & 0 & 0 & I & 0 \\ 0 & 0 & 0 & 0 & I \end{bmatrix}

The augmented Lagrangian Llρ\mathcal{L}_l^\rho of level ll of (6) is:

Llρ(ql,sl,z^l,(λΛ))=12qlTJlql+glTql+I+(sl)+σ2∥zl−z^l∥22+∑Λ∈{d±,d,i,j}ρΛ2∥BΛql+CΛsl−(cΛ)+1ρΛ(λΛ)∥22(8)\begin{aligned} \mathcal{L}_l^\rho(q_l,s_l,\widehat{z}_l,(\lambda_\Lambda)) \\ &= \frac{1}{2}q_l^T J_lq_l + g_l^Tq_l + \mathbb{I}_+(s_l) + \frac{\sigma}{2}\lVert z_l-\widehat{z}_l\rVert_2^2 \\ &+ \sum_{\Lambda\in\{d^\pm,d,i,j\}}\frac{\rho_\Lambda}{2}\left\lVert B_\Lambda q_l+C_\Lambda s_l-(c_\Lambda)+\frac{1}{\rho_\Lambda}(\lambda_\Lambda)\right\rVert_2^2 \tag*{(8)} \end{aligned}

The parameters σ≈1⋅10−5\sigma\approx1\cdot10^{-5} and ρΛ=ρ‾Λ\rho_\Lambda=\overline{\rho}_\Lambda are the individual step parameters for each individual constraint zl=z^lz_l=\widehat{z}_l and Λ∈{d±,d,i,j}\Lambda\in\{d^\pm,d,i,j\}. The component ρl\rho_l is variable and its update rule is described in Sec. V-B, while ρ‾Λ\overline{\rho}_\Lambda are fixed hand-tuned values for each different constraint set in Λ\Lambda (ρ‾=100\overline{\rho}=100 for equalities, ρ‾=1\overline{\rho}=1 for inequalities).

The ADMM computes the primal (see Sec. IV-A), the split variables (see Sec. IV-B), and the Lagrange multipliers (see Sec. IV-C) in alternating fashion. Once the alternating steps of level ll have converged with ∥Kl∥2<η\lVert K_l\rVert_2<\eta, the active constraint set Al∗\mathcal{A}_{l*} and Al\mathcal{A}_l corresponding to activity in Dl\mathcal{D}_l, Cl\mathcal{C}_l, and Ll−1\mathcal{L}_{l-1} are composed. The algorithm then continues with the ADMM of the next level l+1l+1 projected into the nullspace Nl\mathcal{N}_l of the new active set Al=Al−1∪Al\mathcal{A}_{l}= \mathcal{A}_{l-1}\cup\mathcal{A}_l. The remaining inactive constraints are contained in the updated inactive set Il\mathcal{I}_l. More details on convex hierarchical programming can be found in [22].

Primal update

The parenthesized superscript (+)(+) (abbreviation for (l+1)(l+1)) denotes values of the next ADMM iteration based on the values of the current one (l)(l) (omitted for readability). The primal update follows from:

∇ql(+)Llρ(ql(+),sl,z^l,(λΛ))=[H~l+σI00I]ql(+)+gl+∑Λ∈{d±,d,i,j}BΛTρΛ(BΛql(+)+CΛsl−(cΛ)+1ρΛ(λΛ))=0(9)\begin{aligned} \nabla_{q_l^{(+)}}\mathcal{L}_l^\rho(q_l^{(+)},s_l,\widehat{z}_l,(\lambda_\Lambda)) = \begin{bmatrix} \widetilde{H}_l+\sigma I & & \\ & 0 & \\ & & 0 \\ & & & I \end{bmatrix}q_l^{(+)}+g_l \\ &+\sum_{\Lambda\in\{d^\pm,d,i,j\}}B_\Lambda^T\rho_\Lambda\left(B_\Lambda q_l^{(+)}+C_\Lambda s_l-(c_\Lambda)+\frac{1}{\rho_\Lambda}(\lambda_\Lambda)\right)=0 \tag*{(9)} \end{aligned}

This can be rewritten into the linear system:

Kqρqq(+)=−kqρ(10)K_q^\rho q_q^{(+)}=-k_q^\rho \tag*{(10)}

where:

Kqρ=[H~l+σIz+MPT0LTPS0000R0L00Y](11)K_q^\rho= \begin{bmatrix} \widetilde{H}_l+\sigma I_z+M & P^T & 0 & L^T \\ P & S & 0 & 0 \\ 0 & 0 & R & 0 \\ L & 0 & 0 & Y \end{bmatrix} \tag*{(11)}

and:

M:=ρdA~dTA~d+ρiA~iTA~i+ρjA~jTA~j(12)M:=\rho_d\widetilde{A}_d^T\widetilde{A}_d+\rho_i\widetilde{A}_i^T\widetilde{A}_i+\rho_j\widetilde{A}_j^T\widetilde{A}_j \tag*{(12)}
P:=−ρdA~d,S:=ρdI+ρd+2I,R:=ρd+2I(13)P:=-\rho_d\widetilde{A}_d,\qquad S:=\rho_d I+\rho_{d+}2I,\qquad R:=\rho_{d+}2I \tag*{(13)}
Y:=ρiI,L:=−ρiA~i(14)Y:=\rho_i I,\qquad L:=-\rho_i\widetilde{A}_i \tag*{(14)}

The matrices S,R,Y≻0S,R,Y\succ0 are positive-definite and invertible. The overall matrix Kqρ≻0K_q^\rho\succ0 is then positive definite as well for σ>0\sigma>0 and positive-definite H~l≻0\widetilde{H}_l\succ0. Note that computing MM is the most computationally demanding operation of the algorithm, but only needs to be done once, even in case of ρl\rho_l updates due to the separable nature of ρΛ=ρ‾Λ\rho_\Lambda=\overline{\rho}_\Lambda.

The right hand side kqlρk_{q_l}^\rho writes as:

kzlρ=∑Ψ∈{d,i,j}ρΨA~ΨT(−(b~Ψ)+(wΨ)+1ρΨ(λΨ))(15)k_{z_l}^\rho=\sum_{\Psi\in\{d,i,j\}}\rho_\Psi\widetilde{A}_\Psi^T\left(-(\widetilde{b}_\Psi)+(w_\Psi)+\frac{1}{\rho_\Psi}(\lambda_\Psi)\right) \tag*{(15)}
kvdρ=ρd(b~d−w~d−1ρdλd)+ρd±(wd+−wd−−1ρd±λd+−1ρd±λd−)(16)\begin{aligned} k_{v_d}^\rho=\rho_d\left(\widetilde{b}_d-\widetilde{w}_d-\frac{1}{\rho_d}\lambda_d\right) \\ &+\rho_{d\pm}\left(w_{d+}-w_{d-}-\frac{1}{\rho_{d\pm}}\lambda_{d+}-\frac{1}{\rho_{d\pm}}\lambda_{d-}\right) \tag*{(16)} \end{aligned}
kωdρ=ωd+ρd±(−wd+−wd−+1ρd+λd++1ρd−λd−)(17)k_{\omega_d}^\rho=\omega_d+\rho_{d\pm}\left(-w_{d+}-w_{d-}+\frac{1}{\rho_{d+}}\lambda_{d+}+\frac{1}{\rho_{d-}}\lambda_{d-}\right) \tag*{(17)}
kviρ=ρi(b~i−w~i−1ρiλi)(18)k_{v_i}^\rho=\rho_i\left(\widetilde{b}_i-\widetilde{w}_i-\frac{1}{\rho_i}\lambda_i\right) \tag*{(18)}

The vector (td)(+)(t_d)^{(+)} can be directly computed by:

(td)(+)=−R−1(ktdρ)(19)(t_d)^{(+)}=-R^{-1}(k_{t_d}^\rho) \tag*{(19)}

Furthermore, the following substitutions of (vd)(+)(v_d)^{(+)} and (vi)(+)(v_i)^{(+)} in dependence of zl(+)z_l^{(+)} can be computed by:

(vd)(+)=S−1(−Pzl(+)−(kvdρ))(20)(v_d)^{(+)}=S^{-1}\left(-Pz_l^{(+)}-(k_{v_d}^\rho)\right) \tag*{(20)}
(vi)(+)=Y−1(−Lzl(+)−(kviρ))(21)(v_i)^{(+)}=Y^{-1}\left(-Lz_l^{(+)}-(k_{v_i}^\rho)\right) \tag*{(21)}

Finally, zl(+)z_l^{(+)} is efficiently computed from:

Kzlρzl(+)=(PS−1(kvdρ)+LY−1(kviρ)−kzlρ)(22)K_{z_l}^\rho z_l^{(+)}=\left(PS^{-1}(k_{v_d}^\rho)+LY^{-1}(k_{v_i}^\rho)-k_{z_l}^\rho\right) \tag*{(22)}

for example using a Cholesky decomposition of the normal-matrix Kzlρ∈Rnr×nrK_{z_l}^\rho\in\mathbb{R}^{n_r\times n_r}:

Kzlρ:=(H~l+σIz+M−PTS−1P−LTY−1L)(23)K_{z_l}^\rho:=\left(\widetilde{H}_l+\sigma I_z+M- P^TS^{-1}P-L^TY^{-1}L\right) \tag*{(23)}

Split variable update

The split variable update results from the condition (Λ∈{d±,d,i,j})(\Lambda\in\{d^{\pm},d,i,j\}):

∇sl(+),zl(+)Lρ(ql(+),sl(+),z^l(+),(λd±),…)=[ρΛCΛTCΛsl(+)+CΛTρΛ(BΛql(+)−(cΛ)+1ρΛ(λΛ))z^l(+)−zl(+)]=0(24)\nabla_{s_l^{(+)},z_l^{(+)} }\mathcal{L}^{\rho}(q_l^{(+)},s_l^{(+)},\widehat{z}_l^{(+)},(\lambda_{d\pm}),\ldots) = \left[ \rho_{\Lambda}C_{\Lambda}^{T}C_{\Lambda}s_l^{(+)} + C_{\Lambda}^{T}\rho_{\Lambda}\left(B_{\Lambda}q_l^{(+)}-(c_{\Lambda})+\frac{1}{\rho_{\Lambda}}(\lambda_{\Lambda})\right) \atop \widehat{z}_l^{(+)}-z_l^{(+)} \right]=0 \tag*{(24)}

This leads to the updates z^l(+)=zl(+)\widehat{z}_l^{(+)}=z_l^{(+)} and:

wd+(+)=vd+(+)−td+(+)−1ρd±λd+(25)w_{d+}^{(+)}=v_{d+}^{(+)}-t_{d+}^{(+)}-\frac{1}{\rho_{d\pm}}\lambda_{d+} \tag*{(25)}
wd−(+)=−vd−(+)−td−(+)−1ρd±λd−(26)w_{d-}^{(+)}=-v_{d-}^{(+)}-t_{d-}^{(+)}-\frac{1}{\rho_{d\pm}}\lambda_{d-} \tag*{(26)}
wd/i(+)=−A~d/izl(+)+vd/i(+)+b~d/i−1ρd/iλd/i(27)w_{d/i}^{(+)}=-\widetilde{A}_{d/i}z_l^{(+)}+v_{d/i}^{(+)}+\widetilde{b}_{d/i}-\frac{1}{\rho_{d/i}}\lambda_{d/i} \tag*{(27)}
wj(+)=−A~jzl(+)+b~j−1ρjλj(28)w_j^{(+)}=-\widetilde{A}_jz_l^{(+)}+\widetilde{b}_j-\frac{1}{\rho_j}\lambda_j \tag*{(28)}

Finally, the operator I+I_+ is realized by:

wΛ(+)←max⁡(0,wΛ(+))(29)w_{\Lambda}^{(+)}\leftarrow\max(0,w_{\Lambda}^{(+)}) \tag*{(29)}

Dual update

The Lagrange multipliers are updated by dual ascent [5], which writes as (Λ∈{d±,d,i,j})(\Lambda\in\{d^{\pm},d,i,j\}):

(λΛ)(+)=(λΛ)+ρΛ(BΛql(+)+CΛsl(+)−cΛ)(30)(\lambda_{\Lambda})^{(+)}=(\lambda_{\Lambda})+\rho_{\Lambda}(B_{\Lambda}q_l^{(+)}+C_{\Lambda}s_l^{(+)}-c_{\Lambda}) \tag*{(30)}

Specifically, this leads to the updates:

λd+(+)=λd++ρd±(−vd+(+)+td+(+)+wd+(+))(31)\lambda_{d+}^{(+)}=\lambda_{d+}+\rho_{d\pm}\left(-v_{d+}^{(+)}+t_{d+}^{(+)}+w_{d+}^{(+)}\right) \tag*{(31)}
λd−(+)=λd−+ρd±(vd−(+)+td−(+)+wd−(+))(32)\lambda_{d-}^{(+)}=\lambda_{d-}+\rho_{d\pm}\left(v_{d-}^{(+)}+t_{d-}^{(+)}+w_{d-}^{(+)}\right) \tag*{(32)}
λd/i(+)=λd/i+ρd/i(A~d/izl(+)−vd/i(+)+wd/i(+)−b~d/i)(33)\lambda_{d/i}^{(+)}=\lambda_{d/i}+\rho_{d/i}\left(\widetilde{A}_{d/i}z_l^{(+)}-v_{d/i}^{(+)}+w_{d/i}^{(+)}-\widetilde{b}_{d/i}\right) \tag*{(33)}
λj(+)=λj+ρj(A~jzl(+)+wj(+)−b~j)(34)\lambda_j^{(+)}=\lambda_j+\rho_j\left(\widetilde{A}_jz_l^{(+)}+w_j^{(+)}-\widetilde{b}_j\right) \tag*{(34)}

Computational comparison

Each iteration of our solver (referred to as N\mathcal{N}ADM), N\mathcal{N}IPM [4], PIQP [12], and OSQP [15] involves the sets of operations summarized in Table 1. Due to the lack of elimination of ℓ0\ell_0-norm auxiliaries, the complexities of both PIQP and OSQP are strongly influenced by the number of constraints m≫1m\gg1 (short of cubic due to sparse linear algebra implementations), which is typically large in decision-making. On the other hand, both N\mathcal{N}ADM and N\mathcal{N}IPM are linearly dependent on mm, while N\mathcal{N}ADM needs to conduct the expensive normal-matrix assembly (23) of order O(mn2)\mathcal{O}(mn^2) only once, even if there are step-size parameter ρl\rho_l updates.

OperationN\mathcal{N}ADMN\mathcal{N}IPM [4]PIQP* [12]OSQP* [15]qpOASES [11]
Primal variablesnnnnn+2mn+2mn+2mn+2mn+2mn+2m
KKT matrix dim. (23)n2n^2n2n^2(n+2m)2(n+2m)^2(n+5m)2(n+5m)^2a×(n+2m)a\times(n+2m)
Matrix-vector ops.O(ιmn)\mathcal{O}(\iota mn)O(ιmn)\mathcal{O}(\iota mn)O(ιmn)\mathcal{O}(\iota mn)O(ιmn)\mathcal{O}(\iota mn)O(ι3m(n+2m))\mathcal{O}(\iota3m(n+2m))
Normal-matrix eval. (23)O(mn2)\mathcal{O}(mn^2)O(ιmn2)\mathcal{O}(\iota mn^2)O(mn2)\mathcal{O}(mn^2)––
Matrix factorization (22)O(n3)\mathcal{O}(n^3)O(ιn3)\mathcal{O}(\iota n^3)O(ι(n+2m)3)\mathcal{O}(\iota(n+2m)^3)O((n+5m)3)\mathcal{O}((n+5m)^3)O(a2(n+2m)+ιa(n+2m))\mathcal{O}(a^2(n+2m)+\iota a(n+2m))
Back substitution (22)O(ιn2)\mathcal{O}(\iota n^2)O(ιn2)\mathcal{O}(\iota n^2)O(ι(n+2m)2)\mathcal{O}(\iota(n+2m)^2)O(ι(n+5m)2)\mathcal{O}(\iota(n+5m)^2)O(ιa(n+2m))\mathcal{O}(\iota a(n+2m))
TotalO(mn2+n3+ιn(m+n))\mathcal{O}(mn^2+n^3+\iota n(m+n))O(ι(mn2+n3))\mathcal{O}(\iota(mn^2+n^3))O(ι(n+2m)3)\mathcal{O}(\iota(n+2m)^3)O((n+5m)3+ι(n+5m)2)\mathcal{O}((n+5m)^3+\iota(n+5m)^2)O((n+2m)(a2+ι(m+a)))\mathcal{O}((n+2m)(a^2+\iota(m+a)))

Table 1. Computational cost over ι\iota iterations for m:=∣D1∣m := |\mathbb{D}_1| L0-equality constraints (Ad∈Rm×n)(A_d \in\mathbb{R}^{m\times n}). Solvers marked with * are implemented using sparse linear algebra, such that the computational cost is typically governed by sparsity rather than the raw matrix dimensions. For qpOASES, ι\iota denotes the number of active-set changes, and a≤3ma \le3m is the number of linearly independent active constraints.

Further ADMM components

Equilibration

We apply Ruiz equilibration [23] for preconditioning in order to reduce the number of iterations ι\iota. Here, matrix rows and columns are iteratively scaled by their inverse square root maximum absolute value until they equal to 1. For QPs, it is suggested to scale the Lagrangian Hessian Hl:=∇ql,λlL\mathcal{H}_l:=\nabla_{q_l,\lambda_l}\mathcal{L} [15]. In the specific case of SHQPs, this leads to the scaled matrix H‾l\overline{\mathcal{H}}_l (symmetric indicated by ⋆\star): Σ\Sigma is a diagonal matrix containing the scaling factors:

H‾l=ΣHlΣ(35)\overline{\mathcal{H}}_l=\Sigma\mathcal{H}_l\Sigma \tag*{(35)}

Gray entries indicate the individual Hessian rows and columns as derivatives with respect to the given vector. Overlined matrices and vectors are the equilibrated equivalents of their original counterparts. The matrices EE, FF, DD, V±V^{\pm}, and T±T^{\pm} are scaled diagonal matrices.

In the following, we make an observation which leads to simplifications in the scaled version of K‾qρ\overline{K}^{\rho}_q (11), as off-diagonal elements of the form −ρd±T+V++ρd±T−V−-\rho_{d\pm}T^{+}V^{+}+\rho_{d\pm}T^{-}V^{-} are canceled out. We use the operator Γ\Gamma which returns a vector containing the maximum value of each row of a matrix, or of a row / column block (⋅)(\cdot) of H‾l\overline{\mathcal{H}}_l.

Proposition 1. Throughout the Ruiz equilibration of ∇q,λ2L\nabla^2_{q,\lambda}\mathcal{L}, it holds that E≤1E\le1 and therefore V±=T±=IV^{\pm}=T^{\pm}=I.

Proof. Initially, E=V±=T±=IE=V^{\pm}=T^{\pm}=I, so Γ((v‾d))=Γ((λ‾d±))=1\Gamma((\overline{v}_d))=\Gamma((\overline{\lambda}_{d\pm}))=1 follow immediately. At the same time, Γ((λ‾d))≤1\Gamma((\overline{\lambda}_d))\le1 since for any maximum row value Γ(A‾d)<1\Gamma(\overline{A}_d)<1, we have that Γ(E‾)=1>Γ(A‾d)\Gamma(\overline{E})=1>\Gamma(\overline{A}_d). We then clearly have E←diag⁡(Γ(λ‾d))−1Ediag⁡(Γ(v‾d))−1≤1E\leftarrow\operatorname{diag}\left(\sqrt{\Gamma(\overline{\lambda}_d)}\right)^{-1}E\operatorname{diag}\left(\sqrt{\Gamma(\overline{v}_d)}\right)^{-1}\le1. In the next Ruiz iteration, Γ((v‾d))=Γ((λ‾d±))=1\Gamma((\overline{v}_d))=\Gamma((\overline{\lambda}_{d\pm}))=1 still holds due to the unchanged V±=T±=IV^{\pm}=T^{\pm}=I and E≤1E\leq1. By induction over the Ruiz iterations, the claims follow.

Step-size parameter ρl\rho_l updates

We update the step-size parameter ρl\rho_l (initially, ρl=0.1\rho_l=0.1) by the update rule presented in [15], where the operator ∑∥⋅∥∞\sum\|\cdot\|_{\infty} returns the absolute maximum value of all additive components in KλlK_{\lambda_l} and KqlK_{q_l}:

ρl←ρl∥Kλl∥∞/max⁡(∑∥Kλl∥∞)∥Kql∥∞/max⁡(∑∥Kql∥∞)\rho_l \leftarrow\rho_l\sqrt{\frac{\|K_{\lambda_l}\|_{\infty}/\max\left(\sum\|K_{\lambda_l}\|_{\infty}\right)}{\|K_{q_l}\|_{\infty}/\max\left(\sum\|K_{q_l}\|_{\infty}\right)}}

It balances reduction in the normalized primal and dual residuals KλlK_{\lambda_l} and KqlK_{q_l}, respectively. If ρl\rho_l is updated, KzlρK_{z_l}^{\rho} (22) needs to be updated and re-factorized.

Over-relaxation

In over-relaxed ADMM, the relaxed quantity αBlql(+)←αBlql(+)−(1−α)(Clsl−cl)\alpha B_lq_l^{(+)}\leftarrow\alpha B_lq_l^{(+)}-(1-\alpha)(C_ls_l-c_l) is used in the split-variable and dual-variable updates (Sec. IV-B and Sec. IV-C) in order to reduce the number of iterations ι\iota [24]. The parameter α∈(0,2)\alpha\in(0,2) is the over-relaxation parameter (typically α=1.6\alpha=1.6 [15]).

Warm-starting

Between problem instances, for each priority level ll the previously found step-size parameter ρl−\rho_l^{-}, and the unscaled primal, split, and dual variables qˉl\bar{q}_l, sˉl\bar{s}_l, and λˉl\bar{\lambda}_l, respectively, are stored. A level ll is only warm-started if the current active and inactive constraint sets A∪l−1\mathcal{A}_{\cup l-1} and I∪l−1\mathcal{I}_{\cup l-1} are identical with the ones of the previous problem instance at that level Aˉ∪l−1\bar{\mathcal{A}}_{\cup l-1} and Iˉ∪l−1\bar{\mathcal{I}}_{\cup l-1}, while standard initialization is used otherwise.

Separable decision functions

The separable structure of decision functions fd(χ)f_d(\chi) of the form as in (1) can be exploited for computational gain. One row of the Jacobian JdJ_d of fd(χ)f_d(\chi) writes as:

adT=g^dTGwithg^d:=g(χ)−gd,G:=∂g(χ)∂χ(36)a_d^{T}=\hat{g}_d^{T}G\quad\text{with}\quad\hat{g}_d:=g(\chi)-g_d,\quad G:=\frac{\partial g(\chi)}{\partial\chi} \tag*{(36)}

We use G^:=[⋯g^d⋯]∈Ru×∣Dl∣\hat{G}:=\begin{bmatrix}\cdots&\hat{g}_d&\cdots\end{bmatrix}\in\mathbb{R}^{u\times|\mathcal{D}_l|}. Then, a matrix product evaluated in (11) can be rewritten to:

AdTAd=GTG^G^TG(37)A_d^{T}A_d=G^{T}\hat{G}\hat{G}^{T}G \tag*{(37)}

Evaluating this matrix naively by G~TG~\tilde{G}^{T}\tilde{G} with G~=G^TG\tilde{G}=\hat{G}^{T}G is of order O(∣Dl∣un+∣Dl∣n2)O(|\mathcal{D}_l|un+|\mathcal{D}_l|n^2), so the large factor ∣Dl∣≫1|\mathcal{D}_l|\gg1 is amplified by n2n^2. Instead, by first computing G^G^T\hat{G}\hat{G}^{T}, the overall evaluation is of order O(∣Dl∣u2+un2)O(|\mathcal{D}_l|u^2+un^2), where ∣Dl∣|\mathcal{D}_l| is amplified by only u2u^2. The factor uu is typically small in robotics, for example u=3u=3 for translational reaching.

Evaluation

We compare the proposed solver N\mathcal{N}ADM (cold and warm) with N\mathcal{N}IPM [4] and PIQP [12] (both no warm-start), and OSQP [15] and qpOASES [11] (both cold and warm). PIQP, OSQP, and qpOASES solve SHQP directly without eliminating the auxiliary variables (td)(t_d) and the related constraints. Except for the walking scenario (Sec. VII-D), all solvers terminate at ∥Kl∥∞<1⋅10−3\|K_l\|_{\infty}<1\cdot10^{-3}. Timings purely reflect the solve times, while problem setup / update times, ... are excluded. The robot SHQPs are composed by Pinocchio [25], with PD controllers expressed in the velocity domain [20] (Sec. VII-C and Sec. VII-D). Evaluations are run on an Intel Core Ultra 9 185H CPU with 64 GB RAM.

Large-scale LPs

We first isolate the benefit of exploiting sparse structure over general-purpose solvers based on sparse linear algebra [12, 15], independently of hierarchical effects. For this, we solve large-scale linear programs (LPs) corresponding to the case of p=1p=1 and H1=0H_1=0 in SHQP. Equality constraints are composed as randomized, dense matrices with n=30n=30 and Ad∈R∣D1∣×nA_d\in\mathbb{R}^{|\mathcal{D}_1|\times n} (∣D1∣=10,100,500,1⋅103,…,5.2⋅105|\mathcal{D}_1|=10,100,500,1\cdot10^3,\ldots,5.2\cdot10^5, increment: 1000). For each ∣D1∣|\mathcal{D}_1|, we repeatedly (10×10\times) solve slightly disturbed problems A~d+B\tilde{A}_d+B in order to simulate warm-starting, where BB is a randomized dense matrix with elements in the range of 1⋅10−3∥A~d∥∞1\cdot10^{-3}\|\tilde{A}_d\|_{\infty}.

Figure 3 shows that the proposed warm-started solver N\mathcal{N}ADM achieves a mean speedup of around 21.42±14.91%21.42\pm14.91\% with respect to N\mathcal{N}IPM (123 ±\pm 29 and 16 ±\pm 1 iterations, 0.07±0.050.07\pm0.05s and 0.09±0.060.09\pm0.06s solve time over all runs for N\mathcal{N}ADM and N\mathcal{N}IPM, respectively). On the contrary, the reference solvers PIQP and OSQP are between one to two orders of magnitude slower. qpOASES is significantly slower due to its dense linear algebra implementation, but exhibits a significant drop in active-set changes (typically 0 to 3 iterations) after the initial cold solve. While PIQP does not possess explicit warm-starting functionality, its iteration count is low at 3±0.43\pm0.4 iterations, which emphasizes the need to exploit the structure of sparse programs. PIQP, OSQP, and qpOASES could only be evaluated up to 3.6⋅1053.6\cdot10^5 constraints due to their increased memory requirements (2×2\times variables and 3×3\times more constraints) as no elimination of the sparse auxiliaries takes place.

Speedup of different solvers with reference to NIPM on large-scale LPs

Figure 3. Speedup of different solvers with reference to N\mathcal{N}IPM on large-scale LPs with p=1p=1, n=30n=30, and ∣D1∣=10,100,500,1⋅103,…,5.3⋅105|\mathcal{D}_1|=10,100,500,1\cdot10^3,\ldots,5.3\cdot10^5 equality constraints.

Humanoid inverse kinematic control

ℓ\ellℓ\ellfd/i(χ)≤νd/i, d/i∈Dl/Clf_{d/i}(\chi) \leq\nu_{d/i},\ d/i \in\mathcal{D}_l/\mathcal{C}_l
1Vel. lim. / trust region∣Q˙∣≤0.01|\dot{Q}| \leq0.01
2Angle lim.Q‾≤Q≤Q‾\underline{Q} \leq Q \leq\overline{Q}
3StancefLF/RF/LH=νLF/RF/LHf_{\mathrm{LF/RF/LH}} = \nu_{\mathrm{LF/RF/LH}}
4Coll. av.fcoll(Q)≥νcollf_{\mathrm{coll}}(Q) \geq\nu_{\mathrm{coll}}
5Goal DM∥fRH−fRH,d∥22=νRH,d\lVert f_{\mathrm{RH}} - f_{\mathrm{RH,d}}\rVert_{2}^{2} = \nu_{\mathrm{RH,d}}

Table 2. Inverse kinematic control of humanoid robot, task hierarchy.

Inverse kinematic control of humanoid robot, solve times per control iteration

Figure 4. Inverse kinematic control of humanoid robot, solve times per control iteration.

Here, we evaluate warm-starting and the linear algebra implementation for separable decision functions as outlined in Sec. VI. A simulation is designed where the velocity controlled humanoid robot Unitree G1 χ=R29\chi= \mathbb{R}^{29} needs to touch as many as possible passing objects out of 500.

Figure 4 shows that after 200 iterations of warm-starting NADM\mathcal{N}_{\mathrm{ADM}} (maximum solve time: 4.1 ms, NIPM\mathcal{N}_{\mathrm{IPM}}: 3.0 ms), the solver reaches a steady level of around 1.3±0.31.3 \pm0.3 ms (NIPM\mathcal{N}_{\mathrm{IPM}}: 1.6±0.11.6 \pm0.1), with NADM\mathcal{N}_{\mathrm{ADM}} being 23.5% faster than NIPM\mathcal{N}_{\mathrm{IPM}} over all control iterations. NADM\mathcal{N}_{\mathrm{ADM}} (Sec. VI) and NIPM\mathcal{N}_{\mathrm{IPM}} (Sec. VI), which exploit the structure of the decision functions according to Sec. VI, improve the solve time per control iteration by 10% and 12% over their baselines, respectively.

Drone control

ℓ\ellℓ\ellfd/i(χ)≤νd/i, d/i∈Dl/Clf_{d/i}(\chi) \leq\nu_{d/i},\ d/i \in\mathcal{D}_l/\mathcal{C}_l
1Vel. lim. / trust region∣Q˙∣≤0.01|\dot{Q}| \leq0.01
2Force lim.F‾≤F≤F‾\underline{\mathcal{F}} \leq\mathcal{F} \leq\overline{\mathcal{F}}
3Coll. av.fcoll(Q)≥νcollf_{\mathrm{coll}}(Q) \geq\nu_{\mathrm{coll}}
4Dynamicsfdyn(Q)=νdynf_{\mathrm{dyn}}(Q) = \nu_{\mathrm{dyn}}
5Goal DM∥P−Pd∥22=νd\lVert P - P_d\rVert_{2}^{2} = \nu_d
6Goal trackingfDrone(Q)−P=νif_{\mathrm{Drone}}(Q) - P = \nu_i

Table 3. Drone control, task hierarchy.

In this example, we create an SH-NLP with p=6p = 6 and n=9n = 9 for navigation of a drone with linear dynamics fdynf_{\mathrm{dyn}} through an obstacle course to choose one of 100 targets (see Figure 5). The problem is solved with an instantaneous controller with Δt=20\Delta t = 20 ms. The drone is described by χ=[P,Q,F]\chi= [P, Q, \mathcal{F}], where P∈R3P \in\mathbb{R}^{3} is the current goal location, Q∈R3Q \in\mathbb{R}^{3} is the current drone translation [4], and F∈R3\mathcal{F} \in\mathbb{R}^{3} is the control force. Since the drone is controlled via a proportional-derivative (PD) controller expressed in

Instantaneous hierarchical control of a drone to navigate through an obstacle course, while deciding on one of 100 possible goal locations

Figure 5. Instantaneous hierarchical control of a drone to navigate through an obstacle course, while deciding on one of 100 possible goal locations.

Drone control, number of inner iterations per control iteration

Figure 6. Drone control, number of inner iterations per control iteration.

the velocity domain [20], imposing the decision-making directly on acceleration commands is less effective and does not accurately reflect the true decision-making error to the physical target. Instead, the decision-making is imposed on a separate goal position PP which chooses one of the goal locations PdP_d. The goal location is then realized by a PD controller on the last priority level. Further constraints on velocities (max 0.5 m/s), control forces (6 N and 18 N max. horizontal and vertical), and collision avoidance complete the hierarchy.

Figure 6 and Table 4 show how NADM\mathcal{N}_{\mathrm{ADM}} is warm-started very effectively, where in many iterations the controller applies the same control F\mathcal{F} in subsequent iterations. In contrast, warm-started OSQP still requires around 200 iterations, while qpOASES reaches steady-state after around 24 control iterations.

SolverInner iter.Time95th perc.
NADM44.3±69.744.3 \pm69.70.24±0.180.24 \pm0.180.330.33
NIPM [4]74.7±6.074.7 \pm6.00.67±0.100.67 \pm0.100.750.75
PIQP [12]64.0±3.664.0 \pm3.60.86±0.090.86 \pm0.090.970.97
OSQP [15]254.8±541.5254.8 \pm541.50.38±0.520.38 \pm0.520.340.34
qpOASES [11]3.5±42.93.5 \pm42.99.39±19.329.39 \pm19.329.169.16

Table 4. Drone control, comparison of number of inner iterations and solve time (ms) per control iteration.

Humanoid whole-body control for walking over stepping stones

In this experiment, we demonstrate the integration of hierarchical decision-making within a dynamic humanoid walking controller see Figure 1. Both the left and right feet have to make a decision on a candidate set of ∣Ds∣=200|\mathcal{D}_s| = 200 stepping stones to step on next, while the other foot is fixed on the previously chosen stepping stone. Whole-body dynamics, joint torque limits, 6D contact wrench conditions, collision avoidance, and DCM stability constraints [26] complete the hierarchy given in Table 5. The variable vector χ∈R67\chi\in\mathbb{R}^{67} is composed of the goal location P∈R3\mathcal{P} \in\mathbb{R}^{3}, joint velocities Q˙∈R29\dot{Q} \in\mathbb{R}^{29}, joint torques T∈R23\mathcal{T} \in\mathbb{R}^{23}, and left and right foot contact wrenches γ∈R12\gamma\in\mathbb{R}^{12}. The problem can only be faithfully solved under high-accuracy settings (∥Kl∥∞<1⋅10−12\lVert K_l\rVert_\infty< 1\cdot10^{-12}), which is less suitable for first-order methods like NADM and OSQP. NADM’s maximum iteration count is set to 1⋅1051\cdot10^5, making its computation time of 880.7±413.5880.7 \pm413.5 ms per control iteration not competitive with NIPM’s and PIQP’s runtime of 19.8±2.719.8 \pm2.7 ms and 66.9±8.166.9 \pm8.1 ms, respectively. This highlights the existence of problem classes which require high-accuracy solution regimes.

Unitree G1 whole-body control with integrated hierarchical decision-making for walking over 200 stepping stones

Figure 1. Unitree G1 whole-body control with integrated hierarchical decision-making for walking over 200 stepping stones (current decision: yellow sphere; Sec. VII-D).

ℓ\ellℓ\ellfd/i(χ)≤νd/i, d/i∈Dℓ/Cℓf_{d/i}(\chi) \le\nu_{d/i},\ d/i \in D_\ell/C_\ell
1Angle lim. / Vel. lim.2Q≤Q≤Q‾, ∣P˙∣≤0.01 / ∣Q˙∣≤0.012\quad Q \le Q \le\overline{Q},\ |\dot{P}| \le0.01\ /\ |\dot{Q}| \le0.01
2Torque lim.2T‾≤T≤T‾2\quad\underline{T} \le T \le\overline{T}
3Dynamics2fdyn(Q)=νdyn2\quad f_{\mathrm{dyn}}(Q)=\nu_{\mathrm{dyn}}
4Wrench cone2fwc(γ)≥νwc2\quad f_{\mathrm{wc}}(\gamma)\ge\nu_{\mathrm{wc}}
5Stance-foot2fLF/RF(Q)=νLF/RF2\quad f_{\mathrm{LF/RF}}(Q)=\nu_{\mathrm{LF/RF}}
6Coll. av.2fcoll(Q)≥νcoll2\quad f_{\mathrm{coll}}(Q)\ge\nu_{\mathrm{coll}}
7DCM / upright torso2fstab(Q,Q˙)≥νstab2\quad f_{\mathrm{stab}}(Q,\dot{Q})\ge\nu_{\mathrm{stab}}
8Goal DM0∥P−Pd∥2=νd0\quad\lVert P-P_d\rVert^2=\nu_d
9Swing-foot2fLF/RF(P,Q)=νLF/RF2\quad f_{\mathrm{LF/RF}}(P,Q)=\nu_{\mathrm{LF/RF}}
10Regularization2χ=νi2\quad\chi=\nu_i

Table 5. Humanoid whole-body control for walking over stepping stones, task hierarchy.

Conclusion

In this work, we have presented an ADMM solver for hierarchical decision-making control, which effectively exploits temporal similarities between nonlinear solver subproblems. Evaluation shows that in low- to moderate-accuracy regimes, the solver achieves speedups of up to 23.5% over the structure-exploiting IPM [4]. At the same time, complex scenarios like humanoid walking expose the limitations of ADMM based solvers when high-accuracy solutions are required.

Acknowledgments

This work was supported by the RDF-24-02-060 XJTLU Research Development Fund.

References

  1. [1]D. Song, P. Fernbach, T. Flayols, A. D. Prete, N. Mansard, S. Tonneau, and Y. J. Kim, “Solving footstep planning as a feasibility problem using ℓ1-norm minimization,” IEEE Robotics and Automation Letters, vol. 6, no. 3, pp. 5961–5968, 2021.DOI
  2. [2]T. Dinev, W. Merkt, V. Ivan, I. Havoutis, and S. Vijayakumar, “Sparsity-inducing optimal control via differential dynamic programming,” in IEEE International Conference on Robotics and Automation, 2021, pp. 8216–8222.DOI
  3. [3]J. Yu, J. Yan, Z. Jin, C. Hu, and Z. Wang, “Multi-priority reactive motion control for safe and coordinated dual-arm manipulation in dynamic environments,” IEEE Robotics and Automation Letters, vol. 11, no. 6, pp. 7596–7603, 2026.DOI
  4. [4]K. Pfeiffer, Q. Zhang, Y. Chen, G. O. Boateng, Y. Wang, V. Bonnet, and A. Kheddar, “Integrated Hierarchical Decision-Making in Inverse Kinematic Planning and Control,” in Proceedings of Robotics: Science and Systems, Sydney, Australia, July 2026.arxiv.org/abs/2412.01324
  5. [5]S. Boyd, N. Parikh, E. Chu, B. Peleato, and J. Eckstein, “Distributed optimization and statistical learning via the alternating direction method of multipliers,” Foundations and Trends in Machine Learning, vol. 3, pp. 1–122, 01 2011.
  6. [6]I. Mahajan, K. Nguyen, S. Schoedel, E. Nedumaran, M. Mata, B. Plancher, and Z. Manchester, “Code generation and conic constraints for model-predictive control on microcontrollers with conic-timypc,” in IEEE International Conference on Robotics and Automation (ICRA), 2026.arxiv.org/abs/2403.18149
  7. [7]Y. Shirai, A. Raghunathan, and D. K. Jha, “Hierarchical contact-rich trajectory optimization for multi-modal manipulation using tight convex relaxations,” in 2025 IEEE International Conference on Robotics and Automation and Automation (ICRA), 2025, pp. 9199–9205.DOI
  8. [8]V. Gonçalves, P. Fraisse, A. Crosnier, and B. Adorno, “Parsimonious kinematic control of highly redundant robots,” IEEE Robotics and Automation Letters, vol. 1, no. 1, pp. 65–72, 2016.
  9. [9]E. Candès, M. Wakin, and S. Boyd, “Enhancing sparsity by reweighted ℓ1 minimization,” Journal of Fourier Analysis and Applications, vol. 14, no. 5, pp. 877–905, 2007.arxiv.org/abs/0711.1612
  10. [10]P. E. Gill, W. Murray, and M. A. Saunders, “Snopt: An sqp algorithm for large-scale constrained optimization,” SIAM Rev., vol. 47, no. 1, pp. 99–131, jan. 2005.
  11. [11]H. Ferreau, C. Kirches, A. Potschka, H. Bock, and M. Diehl, “qpOASES: A parametric active-set algorithm for quadratic programming,” Mathematical Programming Computation, vol. 6, no. 4, pp. 327–363, 2014.DOI
  12. [12]R. Schwan, Y. Jiang, D. Kuhn, and C. N. Jones, “PIQP: A proximal interior-point quadratic programming solver,” in IEEE Conference on Decision and Control, 2023, pp. 1088–1093.arxiv.org/abs/2304.00290
  13. [13]K. Koh, S.-J. Kim, and S. Boyd, “An interior-point method for large-scale ℓ1-regularized logistic regression,” Journal of Machine Learning Research, vol. 8, no. 54, pp. 1519–1555, 2007.
  14. [14]J. Gondzio and A. Grothey, “A new unblocking technique to warmstart interior point methods based on sensitivity analysis,” SIAM Journal on Optimization, vol. 19, no. 3, pp. 1184–1210, 2008.DOI
  15. [15]B. Stellato, G. Banjac, P. Goulart, A. Bemporad, and S. Boyd, “OSQP: an operator splitting solver for quadratic programs,” Mathematical Programming Computation, vol. 12, no. 4, pp. 637–672, 2020.
  16. [16]A. Jordana, S. Kleff, A. Meduri, J. Carpentier, N. Mansard, and L. Righetti, “Structure-exploiting sequential quadratic programming for model-predictive control,” IEEE Transactions on Robotics, vol. 41, pp. 4960–4974, 2025.DOI
  17. [17]H. Li, Q. Zhang, A. Cui, and J. Peng, “Minimization of fraction function penalty in compressed sensing,” IEEE Transactions on Neural Networks and Learning Systems, vol. 31, no. 5, pp. 1626–1637, 2020.DOI
  18. [18]R. Fletcher, S. Leyffer, and P. L. Toint, “On the global convergence of a filter–sqp algorithm,” SIAM Journal on Optimization, vol. 13, no. 1, pp. 44–59, 2002.DOI
  19. [19]K. Pfeiffer and A. Kheddar, “Sequential hierarchical least-squares programming for prioritized non-linear optimal control,” Optimization Methods and Software, vol. 39, no. 4, pp. 1104–1142, 2024.DOI
  20. [20]K. Pfeiffer, A. Escande, P. Gerondet, and A. Kheddar, “The hierarchical newton’s method for numerically stable prioritized dynamic control,” IEEE Transactions on Control Systems Technology, vol. 31, no. 4, pp. 1622–1635, 2023.arxiv.org/abs/2303.04428
  21. [21]T. V. Dang, K. V. Ling, and J. Maciejowski, “Banded null basis and admm for embedded mpc,” IFAC-PapersOnLine, vol. 50, no. 1, pp. 13 170–13 175, 2017, 20th IFAC World Congress.DOI
  22. [22]K. Pfeiffer, A. Escande, and L. Righetti, “νIPM-HLSP: an efficient interior-point method for hierarchical least-squares programs,” Optimization and Engineering, vol. 25, no. 2, pp. 759–794, 2024.DOI
  23. [23]P. A. Knight, D. Ruiz, and B. Uçar, “A symmetric preserving algorithm for matrix scaling,” SIAM Journal on Matrix Analysis and Applications, vol. 35, no. 3, pp. 931–955, 2014.DOI
  24. [24]E. Ghadimi, A. Teixeira, A. Shames, and M. Johansson, “Optimal parameter selection for the alternating direction method of multipliers (admm): Quadratic problems,” IEEE Transactions on Automatic Control, 09 2014.
  25. [25]J. Carpentier, G. Buondonno, J. Mirabel, F. Lamiraux, O. Stasse, and N. Mansard, “The Pinocchio C++ library—a fast and flexible implementation of rigid body dynamics algorithms and their analytical derivatives,” in IEEE/ISICE International Symposium on System Integration, 2019, pp. 614–619.DOI
  26. [26]J. Englsberger, C. Ott, and A. Albu-Schäffer, “Three-dimensional bipedal walking control based on divergent component of motion,” IEEE Transactions on Robotics, vol. 31, no. 2, pp. 355–368, Apr. 2015.DOI

Paper details

vlnc:W_7f8zv753
Version
v1 · latest
Published
Fresh — added to the Hub within the last 30 days
Added
License
CC BY 4.0
Usage

views · 0

downloads · 0

A view counts once per browser per day, a download each time the PDF is requested.

  • Computer Science

Not recorded for this paper

View statement
Looking for related papers…
Contents