CS Paper Radar

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

Daniel Morton, Jon Arrizabalaga, Zachary Manchester, Marco Pavone

cs.RO math.OC ·submitted 2026-09-16 17:22 UTC · v1
code/datahot direction

Abstract

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.

Closest papers

Nearest neighbours by meaning across the whole index.

DAOCP: a dual active set solver for optimal control problems

eess.SY math.OC ·2026-09-27 ·Alberto Zaupa, Samuel Erickson, Mikael Johansson
similarity 0.69
Abstract · Submitted to ICRA 2027

We present DAOCP, a dual active set solver for linear quadratic optimal control problems with stage-wise equality and inequality constraints. Active set methods are leading Model Predictive Control benchmarks for full-body robotics, but existing solvers operate on dense QPs, while typical problem dimensions favor methods that exploit the optimal control structure. DAOCP combines the warm-starting capabilities of active set algorithms with the better computational scaling of structure exploiting solvers, by relying on a generalization of the relationship between the Riccati recursion and the Cholesky factorization of the condensed Hessian. This result enables dual active set iterations to operate directly on the original optimal control problem through recursive computations, without explicitly forming the condensed quadratic program. On robotics benchmarks, DAOCP is the fastest of four solvers in four of five scenarios, cutting average solve time on a 58-state Atlas model by 9$\times$ relative to state of the art solvers DAQP and HPIPM.

0.14

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

cs.RO math.OC ·2026-11-08 ·Kai Pfeiffer, Jia Wang, Gordon Owusu Boateng, Yuquan Wang, Vincent Bonnet +1 more
similarity 0.69
Abstract

Sparse hierarchical nonlinear programming (SH-NLP) based on the $\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 $\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.

0.13

Joint-Feasibility Margin Feedback for Bounded-Input Second-Order Control Barrier Functions

cs.RO ·2026-10-12 ·Siyuan Yang, Shaoming He
code/data similarity 0.67
Abstract · 6 pages, 3 figures

Multiple second-order control barrier function constraints can exhaust their common control authority under bounded acceleration, even when the current state remains feasible. We define a signed local joint-feasibility margin whose sign exactly identifies whether the current barrier half-spaces intersect the admissible input set. For unit-scaled distance constraints, it has acceleration units and a support-function dual that identifies weighted constraint conflicts. We predict the margin one sample ahead for nine task-directed candidates, selecting either the least-deviating candidate that attains a desired margin or the candidate with the largest predicted margin before solving the same hard HOCBF quadratic program. A controlled one-step case shows that, at the state predicted under the hard-QP command, two constraints remain individually feasible but become jointly infeasible, whereas the margin-feedback controller retains a positive next margin. Separately, across 20 paired initial conditions, the margin-feedback controller retains the hard-QP command in a low-pressure task and acts at positive margin in a critical task before the hard-QP problem becomes infeasible; its trajectories remain feasible at that time and complete the task. Source code is publicly available. https://github.com/xirhxq/cbf/tree/robio2026-preventive-regulation

0.26

Exact Feasibility Certification and Optimal Responsibility Allocation for Multi-Robot CBF Safety Filters

cs.RO cs.MA ·2026-09-14 ·Chandan Kumar Sah, Jishnu Keshavan
hot direction similarity 0.67
Abstract

Multi-robot Control Barrier Function (CBF) safety filters can become infeasible, but a failed quadratic program (QP) does not indicate why the conflict occurred or how to resolve it. To address this, we develop an exact feasibility certificate for multi-agent CBF filters with heterogeneous control-affine dynamics and convex input sets. The certificate quantifies a feasibility reserve by separating the demand imposed by safety constraints from the available actuator supply. This decomposition shows when CBF gain tuning or increased actuation can and cannot resolve infeasibility, and identifies the agents and interactions responsible for the conflict. We further propose an algorithm to optimally allocate shared safety constraints by maximizing the worst local feasibility margin, yielding a linear program for polyhedral input sets. In $320$ paired closed-loop simulations, the proposed allocation reduces infeasible control steps from roughly $50\%$ to $6.2\%$, and reduces safety-violating runs from $118/160$ to $24/160$. In addition, across $52$ infeasibility events, the certificate identifies an interaction whose relaxation restores feasibility in $94\%$ of cases.

0.51

A Coupled Control Barrier Function Approach to Deadlock-Free Multi-Robot Conflict Resolution

cs.RO eess.SY ·2026-11-12 ·Gary Rostomyan, Ketan Savla, Petros A. Ioannou
similarity 0.66
Abstract

Many robotic applications involve scenarios where multiple robots interact and the decisions of one robot influence the behavior of others. This paper presents a quadratic programming (QP)-based feedback controller for multi-robot conflict resolution with input constraints. Safety is enforced using coupled Control Barrier Function (CBF), while a yielding matrix encodes how robots influence each other. Unlike existing methods with fixed or hard priorities, the proposed framework enables yielding preferences while guaranteeing deadlock-free operation by design. Sufficient conditions are established for the controller to be locally Lipschitz. The optimal CBF sign- indefinite slack is shown to admit a finite uniform bound, which is compensated through an enlarged CBF safety margin to obtain a forward invariant set contained within the desired safe set. The analysis also identifies an irreducible slack component due to finite maximum deceleration that cannot be eliminated by increasing the slack penalty. Finally, sufficient conditions on the yielding matrix and nominal controller are derived to exclude deadlock states. Simulations demonstrate adaptive coordination across yielding preferences and robustness to robot failures.

0.19

LEAP: Gradient Flow Augmented Lagrangian Trajectory Optimization for Contact Constrained Robots

cs.RO ·2026-10-14 ·César E. Ramos Chuquiure, Ryan Tsai, Yutong Zhou, Zihang Huang, Vansh Thakur +6 more
similarity 0.63
Abstract

Generating dynamically feasible motions for legged and manipulating robots requires solving a large, nonlinear, non-convex optimal control problem (OCP) in which contact wrenches must satisfy friction-cone constraints throughout the motion, not only at isolated instants. Existing solvers are sensitive to the initial guess and scale poorly with the number of joints. This paper introduces LEAP, which transcribes the OCP with pseudospectral collocation and, instead of a Newton or SQP solve, integrates a primal--dual gradient flow of a Powell--Hestenes--Rockafellar augmented Lagrangian to compute a solution. Because the flow is stiff, this paper designs a linearly implicit integrator with a Gauss--Newton metric, momentum, and a merit-based step controller that keeps the augmented Lagrangian monotone. Across locomotion benchmarks on five robot platforms, from planar hoppers to 32-joint humanoids, LEAP is the only method to solve every tested problem, achieves substantial speedups over Crocoddyl and Aligator on several benchmarks, and reaches trackable references at smaller compute budgets. The resulting trajectories are validated by tracking them in a high-fidelity simulator and on hardware. We will make the code public upon final acceptance of this paper.

0.24

Safe Ergodic Control for Multi-Robot Systems via Quadratic Programming

cs.RO ·2026-10-04 ·Yo Toyomoto, Mohamed Elobaid, Bryce L. Ferguson, Alberto Quattrini Li, Shinkyu Park
similarity 0.62
Abstract

Ergodic control drives robots to spend time in each region in proportion to a spatial distribution of interest, making it well suited for dense spatiotemporal environmental monitoring. Existing safe ergodic controllers rely on offline trajectory optimization or hierarchical architectures, which limit real-time applicability and decouple the ergodicity objective from the safety constraint. This paper presents a quadratic programming (QP)-based controller that treats ergodicity and safety jointly. We first introduce a Gaussian-kernel ergodic metric that, unlike the classical indicator-based metric, is time differentiable. This allows the exponential decay of the metric to be imposed as a time-varying control barrier function (CBF) constraint, relaxed by a slack variable, alongside hard CBF constraints for region containment and inter-robot collision avoidance. We establish that the resulting QP remains feasible from any safe initial configuration and that, up to the slack term, the ergodic metric decays exponentially. Simulations show improved performance over baseline methods, and experiments with three aerial vehicles validate the work on real hardware.

0.16

Continuous-Time Motion Planning and Steering with Global Convergence

cs.RO ·2026-10-22 ·Mohsen Amiri, Sima Didari, Davoud Ataee Tarzanagh, Hankyu Moon, Yeongdae Kwon
benchmark similarity 0.62
Abstract

Motion planning is central to robotics, and it is increasingly used to correct the motions proposed by learned planners and policies. In both uses, constraints must hold at every instant of the motion, not only at sampled times. To our knowledge, no algorithm provably converges to a globally optimal motion once a smoothness cost is combined with such constraints. We cast planning and steering as a single continuous-time program: robots avoid one another and static or moving obstacles, and a learned proposal is steered by keeping the motion close to it. The program is elementary to state, yet nonconvex. We propose GlobMP (Global Motion Planning), a gradient projection method over piecewise-polynomial curves, on which each constraint is one polynomial per piece that must stay nonnegative over the whole motion; it alternates cost reduction with feasibility restoration. Under mild assumptions, we show that, from a fine grid start, its cost approaches the global optimum as the number of polynomial pieces grows, with every constraint met to any chosen tolerance, or exactly on stable instances. We demonstrate its superior performance over strong baselines in real and synthetic experiments, including teams of up to $200$ robots, multi-robot planning on the Robotarium testbed and an extended benchmark, maze navigation, and steering tactile-reactive policies on a bimanual humanoid.

0.16