Large-Scale Hierarchical Decision-Making in Robot Control via ADMM
Authors: Kai Pfeiffer, Jia Wang, Gordon Owusu Boateng, Yuquan Wang, Vincent Bonnet, Abderrahmane Kheddar
Organizations: AIAT, Xi’an Jiaotong-Liverpool University, China. · X Square Robot, China. · IPAL, CNRS, Singapore · LAAS-CNRS, University of Toulouse, France. · LIRMM-CNRS, University of Montpellier, France.
Sparse hierarchical nonlinear programming (SH-NLP) based on the ℓ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-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.
Figures & tables
Fig. 1: Unitree G1 whole-body control with integrated hierarchical decision-making for walking over 200 stepping stones (current decision: yellow sphere; Sec. VII-D ).
Fig. 2: Comparison between continuous log [ 9 ] and fraction function surrogates [ 17 ] of the ℓ0 -norm on squared decision functions d2 (see ( 1 )).
Operation
N ADM
N IPM [ 4 ]
PIQP ∗ [ 12 ]
OSQP ∗ [ 15 ]
qpOASES [ 11 ]
Primal variables
n
n
n+2m
n+2m
n+2m
KKT matrix dim. ( 22 )
n2
n2
(n+2m)2
(n+5m)2
a×(n+2m)
Matrix-vector ops.
O(ιmn)
O(ιmn)
O(ιmn)
O(ιmn)
O(ι3m(n+2m))
Normal-matrix eval. ( 22 )
O(mn2)
O(ιmn2)
O(mn2)
–
–
Matrix factorization ( 21 )
O(n3)
O(ιn3)
O(ι(n+2m)3)
O((n+5m)3)
O(a2(n+2m)+ιa(n+2m))
Back substitution ( 21 )
O(ιn2)
O(ιn2)
O(ι(n+2m)2)
O(ι(n+5m)2)
O(ιa(n+2m))
TABLE 1: Computational cost over ι iterations for m:=∣D1∣ L0-equality constraints ( Ad∈Rm×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, ι denotes the number of active-set changes, and a≤3m is the number of linearly independent active constraints.
Fig. 3: Speedup of different solvers with reference to N IPM on large-scale LPs with p=1 , n=30 , and ∣D1∣=10,100,500,1⋅103,...,5.3⋅105 equality constraints.
l
ℓ
fd/i(χ)≦νd/i,d/i∈Dl/Cl
1
Vel. lim. / trust region
2
∣Q˙∣≤0.01
2
Angle lim.
2
Q≤Q≤Q
3
Stance
2
fLF/RF/LH=νLF/RF/LH
4
Coll. av.
2
fcoll(Q)≥νcoll
5
Goal DM
0
∥fRH−fRH,d∥22=νRH,d
TABLE 2: Inverse kinematic control of humanoid robot, task hierarchy.
Fig. 4: Inverse kinematic control of humanoid robot, solve times per control iteration.
l
ℓ
fd/i(χ)≦νd/i,d/i∈Dl/Cl
1
Vel. lim. / trust region
2
∣Q˙∣≤0.01
2
Force lim.
2
F≤F≤F
3
Coll. av.
2
fcoll(Q)≥νcoll
4
Dynamics
2
fdyn(Q)=νdyn
5
Goal DM
0
∥P−Pd∥22=νd
6
Goal tracking
2
fDrone(Q)−P=νi
TABLE 3: Drone control, task hierarchy.
Fig. 5: Instantaneous hierarchical control of a drone to navigate through an obstacle course, while deciding on one of 100 possible goal locations.
Fig. 6: Drone control, number of inner iterations per control iteration.
Solver
Inner iter.
Time
95 th perc.
N ADM
44.3±69.7
0.24±0.18
0.33
N IPM [ 4 ]
74.7±6.0
0.67±0.10
0.75
PIQP [ 12 ]
64.0±3.6
0.86±0.09
0.97
OSQP [ 15 ]
254.8±541.5
0.38±0.52
0.34
qpOASES [ 11 ]
3.5±42.9
9.39±19.32
9.16
TABLE 4: Drone control, comparison of number of inner iterations and solve time (ms) per control iteration.
l
ℓ
fd/i(χ)≦νd/i,d/i∈Dl/Cl
1
Angle lim. / Vel. lim.
2
Q≤Q≤Q , ∣P˙∣≤0.01 / ∣Q˙∣≤0.01
2
Torque lim.
2
T≤T≤T
3
Dynamics
2
fdyn(Q)=νdyn
4
Wrench cone
2
fwc(γ)≥νwc
5
Stance-foot
2
fLF/RF(Q)=νLF/RF
6
Coll. av.
2
fcoll(Q)≥νcoll
TABLE 5: Humanoid whole-body control for walking over stepping stones, task hierarchy.
Departments of Mechanical Engineering and Aeronautics & Astronautics, Stanford University, USA · Department of Aeronautics and Astronautics, Massachusetts Institute of Technology, USA