---
title: Hierarchical Quadratic Programming (HQP)
url: https://www.emergentmind.com/topics/hierarchical-quadratic-programming-hqp
type: topic
---

# Hierarchical Quadratic Programming (HQP)

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$ [2207.03435, 2109.07236, 2410.16922].

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 [2409.10135, 2406.09982]. 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 [2109.07236].

## 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 [2302.11891].
- **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 [2505.21071].
- **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 [2409.10135].

## 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 [2207.03435].
- **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 [2406.09982, 2409.10135].
- **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 [2410.16922].

## 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 [2109.07236].

Recent advances include:
- **Recursive Hierarchical Projection (RHP)** for seamless priority transitions without extra QPs or loss of accuracy [2109.07236].
- **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 [2505.21071].
- **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 [2302.11891].

All real-time control benchmarks report feasible performance at sub-millisecond timescales for 6-DOF and 7-DOF manipulators [2207.03435, 2406.09982, 2109.07236].

## 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 [2207.03435].
- **MIS**: Maximum RCM deviation $\leq0.4$ mm, mean tracking error $\leq10$ px, real-time cycle $\leq0.4$ ms [2406.09982].
- **Priority transitions**: RHP–HQP yields peak orientation errors $<0.062\times10^{-3}$ rad and mean cycle times $<0.06$ ms through complex handovers [2109.07236].
- **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 [2409.10135].
- **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 [2410.16922].

## 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 [2505.21071].
- **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 [2505.21071, 2302.11891].
- **Fully dynamic consistency**, e.g., incorporating inertia into projectors for dynamic robots, remains an extension area [2109.07236]. *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.

Source: https://www.emergentmind.com/topics/hierarchical-quadratic-programming-hqp