Hierarchical Quadratic Programming (HQP)

• Hierarchical Quadratic Programming is a sequential framework that solves multiple quadratic programs with lexicographic priority, ensuring higher-priority tasks are always satisfied.
• It uses recursive null-space projection methods to isolate lower-priority task subspaces while respecting critical constraints, enabling robust and real-time control.
• HQP has practical applications in robotics, including human-robot interaction and minimally invasive surgery, with empirical benchmarks showing sub-millisecond computational performance.

Hierarchical Quadratic Programming (HQP) is a sequential optimization framework for solving multi-objective control problems under strict priority orderings. In robotics, HQP has become foundational for real-time, task-prioritized control of redundant and mobile-manipulator systems subject to diverse objectives and physical constraints. By enforcing a lexicographic (hierarchical) solution discipline through null-space projections or equivalent recursive elimination, HQP ensures that satisfaction of higher-priority objectives is never compromised by the optimization of lower-priority tasks.

1. Mathematical Structure and Lexicographic Formulation

Let $x\in\mathbb R^n$ denote the decision variables (e.g., joint velocities, generalized velocities, possibly augmented with other task variables). HQP organizes $N$ tasks/levels, each defined by a quadratic cost $J_i(x)=\frac{1}{2}x^\top Q_i x + c_i^\top x$, equality constraints $A_i x = b_i$, and inequality constraints $C_i x \le d_i$ at level $i$. The sequence of QP subproblems is defined as follows:

• At each level $i$, solve:

$$\begin{aligned} \min_x~&J_i(x) \ \mathrm{s.t.}~& A_i x = b_i\ & C_i x \le d_i\ & A_j x = b_j \quad \forall~j < i\ & C_j x \le d_j \quad \forall~j < i~\text{(active inequalities only)} \end{aligned}$$

• Lower-priority solutions are constrained to the affine subspace defined by all higher-priority active equalities. In practice, one builds a projector $N_{i-1}$ onto the null-space of the stacked equalities up to level $i-1$ and parametrizes $x = x^*_{i-1} + N_{i-1} \Delta x$, where $\Delta x$ is optimized at level $i$ (Tassi et al., 2022, Han et al., 2021, Xu et al., 2024).

This paradigm enforces strict lexicographic optimality: lower-priority costs are minimized only within the exact solution set of all higher-priority requirements.

2. Hierarchy Enforcement and Recursive Null-space Methods

Hierarchical enforcement is achieved through recursive null-space projections. After solving for $x^*_{i-1}$ at level $i-1$, the level-$i$ QP is restricted to the subspace satisfying $A_j x = b_j,~\forall~j < i$. The null-space projector $N_{i-1}$ is commonly constructed as

$N_{i-1} = I - A_{1:i-1}^+\;A_{1:i-1}$

where $A_{1:i-1}$ stacks all equality constraints up to $i-1$. This approach maintains strict satisfaction of all higher-priority equalities (Colan et al., 2024, Colan et al., 2024). Efficient implementations exploit sequential QP solvers with active-set or interior-point methods and code generation for real-time performance (e.g., OSQP, qpOASES).

Priority transitions, where the order or weight of tasks changes online (e.g., collision avoidance becoming urgent as an obstacle approaches), can induce discontinuities if not properly handled. Recursive Hierarchical Projection (RHP) matrices allow for continuous transitions by constructing intermediate projectors parameterized by an occupation matrix ($\Lambda_i^{A,s}$), interpolating between full and partial enforcement across levels (Han et al., 2021).

3. Nonlinear, Inequality, and Extended Hierarchical QP Frameworks

While standard HQP addresses stacked QPs with convex quadratic costs and linear constraints, several extensions exist:

• Task-priority step-filter and trust-region frameworks handle nonlinear least-squares objectives, using sequential hierarchical least-squares programming (S-HLSP) with trust-region adaptation and filter mechanisms for global convergence, enabling application to nonlinear optimal control and prioritized inverse kinematics (Pfeiffer et al., 2023).
• Dual and differentiable HLSP solvers: The dual formulation of HLSP as a quadratically constrained least-squares program (QCLSP) enables use of ADMM, providing differentiable and parallelizable solutions, which is required for integration in end-to-end learning or distributed optimization (Pfeiffer, 27 May 2025).
• Soft priorities and weighted transitions: Several works use gain interpolation (e.g., $\beta_a$) to softly blend between competing secondary tasks (such as pose tracking and collision avoidance in teleoperation), realizing smooth transitions between priorities instead of hard task swaps (Colan et al., 2024).

4. Representative Applications in Robotics

HQP is widely deployed for multi-task control of redundant robots, human-robot interaction (HRI), and surgical robotics.

• Human-robot collaboration: Augmented HQP (AHQP) incorporates human action recognition and real-time ergonomic mapping. The state vector is expanded to include both robot joint velocities $\dot{q}$ and desired end-effector velocities $\dot{x}_d$, with slack variables for workspace constraints. Vision-driven updates to feasible regions allow robots to anticipate human intent and optimize for ergonomic safety, as validated through substantial reductions in human ergonomic loads measured by REBA proxies (Tassi et al., 2022).
• Minimally invasive surgery (MIS): HQP enables strict prioritization of critical constraints such as Remote Center of Motion (RCM) on endoscopes or surgical tools, with lower levels devoted to tool pose tracking, collision avoidance, manipulability maximization, and joint limit handling. Both theoretical and experimental results demonstrate sub-millimeter RCM error and robust, real-time collision avoidance with dynamic obstacles (Colan et al., 2024, Colan et al., 2024).
• Physical Human-Robot Interaction (pHRI): Direction-constrained HQP incorporates angular velocity cone constraints to align robot end-effector velocities with human intent, using parallel minimum-error and minimum-angle QP subproblems and merging the solutions on the direction-cone boundary. Admittance control is adapted to be variable, enhancing responsiveness at constraint boundaries (Xu et al., 2024).

5. Algorithmic Variants, Computation, and Real-Time Considerations

The canonical HQP approach is sequential: each level solves a small QP, with lower-priority levels restricted to the null-space of higher-priority constraints. Computational complexity per control cycle is $O(N n^3)$, where $N$ is the number of levels and $n$ is the variable dimension. In practice, for robotic control $N\ll n$, and rank-deficient tasks further reduce QP sizes (Han et al., 2021).

Recent advances include:

• Recursive Hierarchical Projection (RHP) for seamless priority transitions without extra QPs or loss of accuracy (Han et al., 2021).
• ADMM-based dual solvers (D-HADM) for HLSPs under equality constraints, with invertible block substitution techniques enabling factorization in just the variable block, accelerating computation by up to an order of magnitude over interior-point dual methods (Pfeiffer, 27 May 2025).
• Sparse reduced-Hessian interior-point methods and turnback LU-based null-space basis computation for banded constraint matrices, achieving $O(n)$ solve times in large-scale inverse kinematics (Pfeiffer et al., 2023).

All real-time control benchmarks report feasible performance at sub-millisecond timescales for 6-DOF and 7-DOF manipulators (Tassi et al., 2022, Colan et al., 2024, Han et al., 2021).

6. Empirical Results and Quantitative Metrics

Empirical evaluation across application domains demonstrates HQP’s effectiveness:

• Human-ergonomics: Average REBA proxy score reduced from $\sim4.2$ to $0.12$ under AHQP in collaborative reorientation tasks (Tassi et al., 2022).
• MIS: Maximum RCM deviation $\leq0.4$ mm, mean tracking error $\leq10$ px, real-time cycle $\leq0.4$ ms (Colan et al., 2024).
• Priority transitions: RHP–HQP yields peak orientation errors $<0.062\times10^{-3}$ rad and mean cycle times $<0.06$ ms through complex handovers (Han et al., 2021).
• Collision avoidance: HQP automatically modulates the weight of collision tasks, maintaining strict RCM and achieving mean end-effector error $<0.5$ mm under dynamic obstacles (Colan et al., 2024).
• Physical HRI: Direction-constrained HQP produces smoother trajectories and eliminates interaction delays near constraint boundaries, confirmed at 1 kHz rates with a 7-DOF arm (Xu et al., 2024).

7. Limitations, Scope, and Extensions

• Sequential HQP is not differentiable across priority boundaries, limiting use in gradient-based learning unless dual/ADMM variants are employed (Pfeiffer, 27 May 2025).
• Equality constraints are directly handled; inclusion of general inequalities and non-convex constraints often necessitates either convex relaxations or restrictive assumptions about feasibility or slack variable handling (Pfeiffer, 27 May 2025, Pfeiffer et al., 2023).
• Fully dynamic consistency, e.g., incorporating inertia into projectors for dynamic robots, remains an extension area (Han et al., 2021). A plausible implication is that future approaches will tighten the integration of HQP with learning-based schedulers and higher-level planners.

HQP’s robust hierarchy management, broad applicability to redundancy resolution, constraint enforcement, and multi-objective control, and strong empirical record in critical robotics domains make it the preeminent framework for strict priority control under quadratic objectives. For implementers, selection among primal sequential, recursive, dual/ADMM, or nonlinear variants should be dictated by constraint structure, transition smoothness demands, and timing requirements.