cs.RONov 8, 2026

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. · 

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.

Figures & tables

Explore similar work

Mar 7, 2026

ACLM: ADMM-Based Distributed Model Predictive Control for Collaborative Loco-Manipulation

Collaborative transportation of heavy payloads via loco-manipulation is a challenging yet essential capability for legged robots operating in complex, unstructured environments. Centralized planning methods, e.g., holistic trajectory optimization, capture dynamic coupling among robots and payloads but scale poorly with system size, limiting real-time applicability. In contrast, hierarchical and fully decentralized approaches often neglect force and dynamic interactions, leading to conservative behavior. This study proposes an Alternating Direction Method of Multipliers (ADMM)-based distributed model predictive control framework for collaborative loco-manipulation with a team of quadruped robots with manipulators. By exploiting the payload-induced coupling structure, the global optimal control problem is decomposed into parallel individual-robot-level subproblems with consensus constraints. The distributed planner operates in a receding-horizon fashion and achieves fast convergence, requiring only a few ADMM iterations per planning cycle. A wrench-aware whole-body controller executes the planned trajectories, tracking both motion and interaction wrenches. Extensive simulations with up to four robots demonstrate scalability, real-time performance, and robustness to model uncertainty.
Sep 16, 2026

ElastiQP: An Always-Feasible QP Solver for Constrained Robot Control

As robot capabilities increase, quadratic programming (QP)-based controllers must account for a similarly increasing number of constraints to ensure safe, reliable operation. Yet, with each added constraint, this introduces more chances of momentary conflict: in which case, a QP solver that returns an "infeasible" status leaves the controller with nothing to execute. To address this, we introduce ElastiQP, a modified dual active-set QP solver that relaxes every inequality constraint with an exact, per-constraint l1 penalty while keeping equality constraints (dynamics) hard. Notably, ElastiQP does so by folding the slack variables into the solver analytically, maintaining a constant size of the condensed linear system. On a suite of robot control benchmarks, ElastiQP achieves microsecond-level performance, matching or outperforming leading modern solvers on feasible problems. On infeasible problems, ElastiQP handles these gracefully, confining violations to strictly the conflicting inequality terms, returning a usable solution up to 40x faster than the best alternative solvers. ElastiQP is available as an open-source C++ header-only library, with Python and JAX interfaces, at https://github.com/StanfordASL/elastiqp.
Nov 14, 2026

PSK: A Matrix-Free Single-Loop Method for K-Level Physics-Based Control

Physics-based control is central to robot learning, but differentiating through implicit physics steps typically requires Hessian or Karush--Kuhn--Tucker (KKT) linear solves; we ask whether gradient steps suffice. Each simulation step minimizes an energy under many contact and collision constraints. Without constraints, we view control over KK steps as KK-level optimization with strongly convex lower levels, recast as one min--max problem. Our Penalized Single-loop KK-level method (PSK) uses only gradients and a projection, updates all levels in parallel, and reaches an εε-stationary control in O(ε−3)O(ε^{-3}) iterations, with a KK-independent exponent but a KK-dependent constant. Contact-rich constraints, however, typically violate a constraint qualification (CQ). We replace each constraint by a smooth wall that acts on a band of constant width. Under mild assumptions that need no CQ, every exact solution of the banded lower levels is then feasible and banded Fritz--John, and PSK keeps the O(ε−3)O(ε^{-3}) complexity. Without a CQ that is uniform in the band width, however, a shrinking band loses the KK-independent exponent: with width εε and fixed K≥2K\geq2, deterministic zero-respecting methods need Ω(ε−(K+3)/4)Ω(ε^{-(K+3)/4}) calls on a hard instance whose control dimension grows with 1/ε1/ε. We prove our assumptions for smoothed kinematic and dynamic multi-robot navigation among segment obstacles. In navigation experiments, PSK, tuned per case, reaches every goal with sixteen levels and, at equal time, reaches a lower loss than tuned unrolling in 17 of 18 cases, while implicit differentiation is lower in 10.