Recursive Hierarchical Projection (RHP)

• Recursive Hierarchical Projection (RHP) is a matrix-based framework for embedding continuously varying null-space projectors within HQP for redundant robotic control.
• It employs a dynamic priority matrix to interpolate task priorities smoothly, reducing computational overhead during transitions.
• RHP maintains strict accuracy for higher-priority tasks, ensuring minimal error and continuity during rapid task reordering in simulations.

Recursive Hierarchical Projection (RHP) is a matrix-based framework for embedding continuously varying null-space projectors within hierarchical quadratic programming (HQP) formulations, specifically in the context of whole-body control (WBC) with dynamic task priority transitions. RHP enables smooth and computationally efficient rearrangement of task priorities for redundant robotic systems, resolving limitations of classical approaches that induce discontinuities or require increased computational effort during task reordering (Han et al., 2021).

1. Motivation and Conceptual Underpinnings

In conventional HQP for redundant robot WBC, strict task priorities are established by projecting lower-priority tasks into the null-spaces of all higher-priority tasks. Each projection typically takes the form $P = I - J^+J$, where $J$ is the Jacobian of the higher-priority task. However, when dynamic task scheduling or reordering is required—such as when collision avoidance must supersede position tracking in response to environmental hazards—these fixed projections induce discontinuities unless additional smoothing heuristics, multiple QP solves per control cycle, or manual blending mechanisms are imposed.

RHP addresses this by introducing a recursively constructed, smoothly varying projection matrix $P_i(\alpha^1, ..., \alpha^n)$ at each hierarchical level $i$. These matrices are driven by a "priority matrix" $\Psi$, whose entries $\alpha_{i,j}\in[0,1]$ encode the degree to which task $j$ occupies the degrees of freedom (DOFs) at level $i$. As these priorities are adjusted, the resulting sequence of $P_i$ projectors morph smoothly, permitting continuous task transitions without an increase in computational overhead or loss of accuracy for higher-priority tasks.

2. Mathematical Formulation and Algorithmic Structure

2.1 Hierarchical HQP with Recursive Projectors

Suppose $L$ hierarchical levels and $J$0 (size $J$1) are given, with initial state estimate $J$2 and $J$3. At each level $J$4 $J$5, RHP-HQP solves

$J$6

subject to inequality and softened equality constraints from all higher ($J$7) levels:

$J$8

The solution update is

$J$9

2.2 Recursive Construction of Projectors

At each $P_i(\alpha^1, ..., \alpha^n)$0, let $P_i(\alpha^1, ..., \alpha^n)$1 index the set of tasks with changed priorities (i.e., $P_i(\alpha^1, ..., \alpha^n)$2), and $P_i(\alpha^1, ..., \alpha^n)$3. The process is as follows:

1. Build a sorted stack $P_i(\alpha^1, ..., \alpha^n)$4 of their Jacobians, ordered by descending $P_i(\alpha^1, ..., \alpha^n)$5.
2. Extract a row-full-rank subset $P_i(\alpha^1, ..., \alpha^n)$6 using Gaussian elimination.
3. Perform thin QR decomposition on $P_i(\alpha^1, ..., \alpha^n)$7 to extract an orthonormal basis $P_i(\alpha^1, ..., \alpha^n)$8 for the row space.
4. Form diagonal $P_i(\alpha^1, ..., \alpha^n)$9, with entries $i$0 by active row index.
5. Update the projector:

$i$1

Special cases:

• $i$2 yields $i$3
• $i$4 recovers the classical null-space projector case
• $i$5 lets $i$6 interpolate continuously, preserving partial task occupation

Scheduling smooth $i$7 functions directly yields continuous $i$8 evolution.

3. Priority Transitions and Control Continuity

A key property of RHP is the guarantee of continuity across task transitions. For arbitrary, smoothly time-varying priorities, only $i$9 QPs are solved per cycle, as in conventional HQP. Each task's transition does not trigger extra QP solves or discontinuous control updates, contrasting with methods relying on piecewise projectors, explicit re-weighting, or state resets.

Comparative Computational Overhead

In simulation (7-DOF arm + 3-DOF waist), RHP-HQP demonstrated no detectable rise in average solve time relative to plain HQP and was within 3–4% of the fastest generalized projector method (GHC). This computational efficiency stems from RHP's reliance on a thin QR plus one small matrix multiplication per level, which is negligible compared to QP solution cost (Han et al., 2021).

4. Task Accuracy and Behavior During Priority Changes

RHP-HQP preserves the strict accuracy of higher-priority tasks during transitions, as each control update projects the residual update only within the (possibly partially-occupied) null-space defined by all higher-priority tasks. Lower-priority solutions are therefore orthogonal, and cannot degrade the accuracy of levels above.

Empirical results showed that:

• On strict hierarchies, RHP-HQP yields maximal hand-orientation error of $\Psi$0 rad and position error of $\Psi$1 mm, matching classical HQP and significantly outperforming GHC (which achieved $\Psi$2 rad and $\Psi$3 mm, respectively).
• During transitions, maximal hand-orientation errors were $\Psi$4 rad (RHP-HQP), $\Psi$5 rad (HQP-swap), and $\Psi$6 rad (GHC), with RHP-HQP attaining superior accuracy for both strict and swapped targets.
• Integrated position errors and cycle times further confirmed the method's performance consistency across task rearrangements.

5. Simulation Protocols and Representative Applications

The methodology was instantiated on the upper body of the Walker X platform (3-DOF waist, 7-DOF arm) for multi-task WBC involving the following tasks:

Task (Editor’s term)	Constraint Type	Description
$\Psi$7	Inequality	Torso collision avoidance
$\Psi$8	Equality	Hand posture (orientation) tracking
$\Psi$9	Equality	Hand position tracking
$\alpha_{i,j}\in[0,1]$0	Inequality	Arm collision avoidance

At each control tick:

• The minimum obstacle distance $\alpha_{i,j}\in[0,1]$1 is monitored to schedule $\alpha_{i,j}\in[0,1]$2 and blend between hierarchies.
• RHP projectors $\alpha_{i,j}\in[0,1]$3 are constructed.
• The HQP chain is solved, and the resulting $\alpha_{i,j}\in[0,1]$4 is applied.

Case studies covered strict hierarchies, continuous transitions, and multi-phase rearrangements to simultaneously achieve collision avoidance and compliance. In all cases, RHP-HQP maintained smooth command trajectories, minimal error accumulation, and low computational overhead (averaging 0.053–0.058 ms per cycle).

6. Strengths and Limitations

Advantages

• Enables continuous, unified treatment of hierarchical transitions before, during, and after swaps within a single HQP recursion.
• Requires no increase in QP solves or significant computational expense; achieves runtimes on par with the fastest available methods.
• Strictly preserves accuracy for higher-priority tasks at all levels.
• Modular workflow: priority scheduling $\alpha_{i,j}\in[0,1]$5 RHP matrix construction $\alpha_{i,j}\in[0,1]$6 HQP solve $\alpha_{i,j}\in[0,1]$7 actuation.

Limitations

• RHP projectors in current form are not dynamically consistent and ignore inertia-weighting; ongoing work is required to extend the technique for mass-aware, physically-consistent projection matrices.
• Hardware validation is not yet reported within the cited work.

7. Context and Comparison with Prior Approaches

Prior methods for HQP-based task priority transition (including those using piecewise null-space projectors with explicit interpolation schemes) often required additional QP solves, increased blending computations, or control re-initialization. Such strategies imposed significant CPU overhead or could not strictly guarantee the preservation of higher-priority task accuracy. Single-QP "generalized projector" methods (e.g., GHC) blend tasks in a single optimization but may permit cross-level error contamination. RHP outperforms these in both computational efficiency and strict hierarchical behavior, particularly during rapidly changing priority conditions (Han et al., 2021).