Direction-Constrained HQP

• Direction-Constrained HQP is a multi-level optimization framework that incorporates angular constraints to prevent abrupt command redirections in robotics and control systems.
• It decomposes each level into minimum-error and minimum-angle subproblems, enabling a convex blend that satisfies both task precision and directional limits.
• The framework enhances physical human-robot interaction through variable admittance control and efficient eigenvalue-based solvers, ensuring smooth and computationally fast responses.

A direction-constrained hierarchical quadratic program (HQP) is a multi-level optimization framework that explicitly incorporates directional (angular) constraints into the hierarchy of quadratic programs classically used in robotics, control, and signal processing. The direction-constrained HQP prevents undesirable or abrupt command redirections when task or safety constraints become active, particularly in contexts requiring human-robot interaction (pHRI) or physical constraint enforcement. This article surveys the formal structure, solution methods, algorithmic innovations, and application scenarios of direction-constrained HQP with a focus on physical interaction and efficient computational approaches.

1. Standard Hierarchical Quadratic Programming Formulation

HQP is a mechanism for the strict prioritization of multiple, potentially conflicting, equality and inequality task requirements. Task priorities are encoded lexicographically: constraints at higher levels have strictly higher precedence than those below. For $N$ prioritized task levels ($k=1,\ldots,N$), the canonical HQP is:

$$\min_{u,\,w_1,\ldots,w_N}^{\rm lex} \left(\|w_1\|^2, \ldots, \|w_N\|^2\right)$$

$$\text{s.t.} \quad \forall k=1,\ldots, N: \begin{cases} A_k u = b_k + w_k, \ C_k u \leq d_k. \end{cases}$$

At each level $k$, $u$ is the control, $A_k$, $b_k$ encode the equality-task (with slack $w_k$ for relaxation), and $C_k$, $k=1,\ldots,N$0 represent convex inequality-task constraints (e.g., joint limits, collision avoidance). Rather than stacking all levels, HQP typically solves at each $k=1,\ldots,N$1 a reduced QP that strictly enforces all higher priority equalities:

$k=1,\ldots,N$2

where $k=1,\ldots,N$3 denotes the aggregation of all higher-level Jacobians.

2. Directional Constraints: Motivation and Formalism

Standard HQP permits the task-space velocity or command $k=1,\ldots,N$4 to change direction arbitrarily when close to an active inequality constraint. In the context of smooth pHRI or safety-critical robotics, this can lead to "jerky" or unpredictable behavior that violates human intent. To address this, direction-constrained HQP restricts the maximum allowable angle between the actual command $k=1,\ldots,N$5 and its nominal reference $k=1,\ldots,N$6 via the constraint:

$k=1,\ldots,N$7

or equivalently,

$k=1,\ldots,N$8

Here, $k=1,\ldots,N$9 is a user-defined angular threshold: $$\min_{u,\,w_1,\ldots,w_N}^{\rm lex} \left(\|w_1\|^2, \ldots, \|w_N\|^2\right)$$0 enforces perfect alignment (task scaling), while $$\min_{u,\,w_1,\ldots,w_N}^{\rm lex} \left(\|w_1\|^2, \ldots, \|w_N\|^2\right)$$1 suppresses the constraint (recovering standard HQP). This form introduces strong nonlinearity due to the normalization, increasing the challenge of direct optimization.

3. Two-Subproblem Decomposition and Solution Procedure

To address the non-convexity induced by the angular constraint, the level-$$\min_{u,\,w_1,\ldots,w_N}^{\rm lex} \left(\|w_1\|^2, \ldots, \|w_N\|^2\right)$$2 problem is decoupled into two parallel subproblems:

• Minimum-Error Problem $$\min_{u,\,w_1,\ldots,w_N}^{\rm lex} \left(\|w_1\|^2, \ldots, \|w_N\|^2\right)$$3: Minimize $$\min_{u,\,w_1,\ldots,w_N}^{\rm lex} \left(\|w_1\|^2, \ldots, \|w_N\|^2\right)$$4 over $$\min_{u,\,w_1,\ldots,w_N}^{\rm lex} \left(\|w_1\|^2, \ldots, \|w_N\|^2\right)$$5 subject to all previously active equalities and present inequalities. The solution, $$\min_{u,\,w_1,\ldots,w_N}^{\rm lex} \left(\|w_1\|^2, \ldots, \|w_N\|^2\right)$$6, corresponds to classic HQP and yields the smallest task-space error.
• Minimum-Angle Problem $$\min_{u,\,w_1,\ldots,w_N}^{\rm lex} \left(\|w_1\|^2, \ldots, \|w_N\|^2\right)$$7: Minimize the angle $$\min_{u,\,w_1,\ldots,w_N}^{\rm lex} \left(\|w_1\|^2, \ldots, \|w_N\|^2\right)$$8 subject to the same constraints. The corresponding solution, $$\min_{u,\,w_1,\ldots,w_N}^{\rm lex} \left(\|w_1\|^2, \ldots, \|w_N\|^2\right)$$9, finds the closest achievable direction to $$\text{s.t.} \quad \forall k=1,\ldots, N: \begin{cases} A_k u = b_k + w_k, \ C_k u \leq d_k. \end{cases}$$0 (not necessarily in magnitude), accounting for actuation and safety limits.

The minimum-angle problem admits a closed-form, scaled solution via projection into the feasible subspace, with scale $$\text{s.t.} \quad \forall k=1,\ldots, N: \begin{cases} A_k u = b_k + w_k, \ C_k u \leq d_k. \end{cases}$$1 determined and clamped to the interval compatible with all active inequalities.

After both subproblems are solved with a shared iterative active-set solver on $$\text{s.t.} \quad \forall k=1,\ldots, N: \begin{cases} A_k u = b_k + w_k, \ C_k u \leq d_k. \end{cases}$$2, the admissible candidates $$\text{s.t.} \quad \forall k=1,\ldots, N: \begin{cases} A_k u = b_k + w_k, \ C_k u \leq d_k. \end{cases}$$3 and $$\text{s.t.} \quad \forall k=1,\ldots, N: \begin{cases} A_k u = b_k + w_k, \ C_k u \leq d_k. \end{cases}$$4 are combined via a parameterized convex blend:

$$\text{s.t.} \quad \forall k=1,\ldots, N: \begin{cases} A_k u = b_k + w_k, \ C_k u \leq d_k. \end{cases}$$5

where $$\text{s.t.} \quad \forall k=1,\ldots, N: \begin{cases} A_k u = b_k + w_k, \ C_k u \leq d_k. \end{cases}$$6 is chosen as the smallest parameter such that the angle deviation constraint is exactly met:

$$\text{s.t.} \quad \forall k=1,\ldots, N: \begin{cases} A_k u = b_k + w_k, \ C_k u \leq d_k. \end{cases}$$7

Tracking error $$\text{s.t.} \quad \forall k=1,\ldots, N: \begin{cases} A_k u = b_k + w_k, \ C_k u \leq d_k. \end{cases}$$8 is strictly decreasing in $$\text{s.t.} \quad \forall k=1,\ldots, N: \begin{cases} A_k u = b_k + w_k, \ C_k u \leq d_k. \end{cases}$$9, while the directional deviation $k$0 is monotonic or V-shaped, accommodating precise constraint satisfaction through a 1D search in $k$1.

4. Variable Admittance Control under Directional Constraints

For physical HRI, the lowest-priority (level $k$2) task typically involves matching robot motion to human-applied forces via admittance control. The classical second-order admittance controller is:

$k$3

yielding $k$4 at steady state. However, proximity to active directional or inequality constraints can induce large mismatches between the desired $k$5 and achieved end-effector velocity $k$6. The error $k$7 may become significant.

Dynamic adjustment of the damping matrix $k$8 is introduced to reduce $k$9, especially near constraint boundaries:

$u$0

where $u$1 are positive gains and $u$2 ensures a minimum damping floor. Increased damping inhibits runaway $u$3 when the operator pushes into a constraint; decreased damping accelerates response when the operator pulls away. This closed-loop regulation improves intent tracking and reduces interaction delays.

5. Algorithmic Workflow

Each control cycle in direction-constrained HQP proceeds as follows:

1. Measure the human-applied force $u$4; update admittance $u$5.
2. Construct the task hierarchy: t-eq and t-iq constraints for levels $u$6, and the interaction task at $u$7 with $u$8.
3. For $u$9 to $A_k$0:
   • Initialize the active set $A_k$1.
   • Iterate:
     • Solve $A_k$2 to obtain $A_k$3.
     • Solve $A_k$4 to obtain $A_k$5.
     • Update $A_k$6 for violated constraints; recompute null spaces as necessary.
     • Prune inactive constraints using KKT conditions on $A_k$7.
   • Terminate on $A_k$8 convergence, merge $A_k$9 via convex-blend to $b_k$0.
   • Update null-space projectors.
4. Send $b_k$1 to the robot, compute actual $b_k$2, update $b_k$3, and $b_k$4.

This procedure ensures all t-eq and t-iq constraints are satisfied at every priority level, enforces strict bounds on task-space angular deviation, and guarantees smooth, interpretable responses at constraint boundaries.

6. Homogeneous Quadratic Programs with Directional Constraints

A broad family of directionally-constrained quadratic programs can be formulated as:

$b_k$5

where each $b_k$6 encodes quadratic or angular constraints; $b_k$7. By Cholesky decomposition, this reduces to:

$b_k$8

The KKT conditions lead to a minimum-eigenvalue structure, enabling efficient 1D or 2D search (for up to three constraints) rather than full semidefinite relaxation. Subspace constraints and exact angular bounds, e.g., $b_k$9, are directly representable in the quadratic form and handled within the same framework by constructing $w_k$0 as $w_k$1 or suitable projections.

For two constraints, the search reduces to a maximum over three candidate points (associated with boundary and intersection conditions), while three constraints require comparing up to seven candidates. For $w_k$2-dimensional variables, the method achieves global optimality at a per-iteration cost of one minimum-eigenvalue computation ($w_k$3), substantially more efficient than SDR approaches ($w_k$4) (Gaurav et al., 2013).

7. Application Scenarios and Performance Characteristics

The direction-constrained HQP framework has been extensively evaluated in pHRI scenarios, such as a 7-DOF robotic arm under mixed task and safety constraints. Empirical results demonstrate that:

• Motion smoothness: Directional constraints enforce continuous, human-intuitive corrections, eliminating abrupt velocity jumps when constraints activate [$w_k$5].
• Task consistency and safety: The two-subproblem split and coordinated active-set solver guarantee all constraints are respected, even under multi-level prioritization.
• Reduced interaction delay: Variable admittance control minimizes velocity mismatches at the constraint boundary, especially during intent reversals.
• Computational efficiency: In MIMO relay design and related signal processing HQCQP instances, eigenvalue-based direction-constrained solvers achieve 16x–720x speedup compared to SDR at $w_k$6 accuracy (Gaurav et al., 2013).

These properties make direction-constrained HQP applicable not only to human-robot collaborative control but also to convexified subproblems in communication and estimation.

---

Summary Table: Key Aspects of Direction-Constrained HQP

Feature	Standard HQP	Direction-Constrained HQP
Direction regulation	None; arbitrary	Explicit angle-bound at each task level
Subproblem decomposition	Single QP per priority	Minimum-error and minimum-angle subproblems
Solution blending	Not applicable	Convex blend enforces angular constraint
Human-robot interaction	No specific improvement	Smoother, intention-aligned response
Computational requirement	QP per level	Two parallel subproblems + 1D blend

Enforcing angular constraints within the HQP hierarchy delivers robust, interpretable, and efficient solutions to multi-task motion and control problems where constraint-activated direction changes must remain bounded and comprehensible.