--- title: Augmented HQP for Collaborative Robotics url: https://www.emergentmind.com/topics/augmented-hqp-ahqp type: topic --- # Augmented HQP for Collaborative Robotics Augmented Hierarchical Quadratic Programming (AHQP) is a real-time, lexicographic multi-task control framework for human-robot collaboration that hierarchically integrates vision-based human action recognition, adaptive workspace soft constraints, and ergonomics optimization. AHQP adapts the standard "stack-of-tasks" hierarchical quadratic programming (HQP) paradigm by simultaneously optimizing both robot joint velocities and desired end-effector (EE) velocities, enabling fine-grained, ergonomic, and sociable robot behavior during collaborative tasks. This formulation allows intuitive human guidance of robotic systems, seamless online adaptation of shared workspaces, and direct incorporation of posture-friendly motion, as demonstrated in tasks such as tool handover, cooperative manipulation, and human-following mobile manipulation [2207.03435]. ## 1. Mathematical Structure and Optimization Hierarchy The AHQP framework is grounded in the HQP formalism, which solves a stack of $p$ prioritized quadratic programs for a generic decision variable $\chi \in \mathbb{R}^s$, subject at each priority level $k=1\,\ldots,p$ to distinct costs and constraints: \[ \begin{aligned} &\min_{\chi}\; \tfrac12\|\mathbf{A}_k\,\chi - \mathbf{b}_k\|^2\\ &\text{s.t.}\quad \mathbf{C}_1\chi\le d_1,\;\dots,\;\mathbf{C}_k\chi\le d_k,\\ &\phantom{\text{s.t.}\quad}\mathbf{E}_1\chi= f_1,\;\dots,\;\mathbf{E}_k\chi= f_k, \end{aligned} \] with $\mathbf{A}_k$, $\mathbf{b}_k$ defining the cost for the $k$-th task, $\mathbf{C}_i$, $d_i$ the inequality constraints, and $\mathbf{E}_i$, $f_i$ as equality constraints. Enforcing strict prioritization requires lower-priority solutions to lie in the nullspace of higher-priority tasks, operationalized by stacking equality constraints from superior levels. A central innovation in AHQP lies in the design of the augmented decision variable: \[ \chi = \begin{bmatrix}\dot{q} \\ \dot{x}_d \end{bmatrix} \in \mathbb{R}^{n+m}, \] where $\dot{q}\in \mathbb{R}^n$ are the $n$-DoF robot joint velocities and $\dot{x}_d\in\mathbb{R}^m$ are the desired end-effector velocities simultaneously optimized online. This enables a tight coupling of robot kinematics with workspace and ergonomics objectives [2207.03435]. ## 2. Task Levels and Cost Functions AHQP organizes control objectives into a lexicographically ordered stack of three core levels: 1. **Primary Task: Closed-Loop Inverse Kinematics (CLIK)** The top-priority QP enforces a closed-loop inverse-kinematics objective over $\chi$. With $J$ the robot Jacobian, $x_a$ the actual EE pose, and $x_d(t-1)$ the previous desired pose, the cost is \[ \min_{\dot{q}, \dot{x}_d} \| J\,\dot{q} - (I + K_p\Delta t)\,\dot{x}_d - K_p(x_d(t-1) - x_a) \|^2, \] where $K_p$ is the proportional gain and $\Delta t$ the integration step. 2. **Secondary Task: Soft Constraint on Shared Workspace (HRSW)** To allow dynamic deformation of the human-robot shared workspace, the EE goal $x_d$ is constrained via a slackened box constraint \[ x_{d,\min} - s \le x_d \le x_{d,\max} + s, \qquad s \ge 0, \] where $s\in\mathbb{R}^m$ is a slack vector penalized in a second-level QP \[ \min_{\chi, s} \tfrac{1}{2} \|s\|^2 \quad \text{s.t.} \quad C_s\begin{bmatrix}\chi \\ s\end{bmatrix} \le d_s. \] 3. **Tertiary Task: Ergonomics Optimization** Ergonomics is addressed by fitting a lightweight Cartesian map $f(x_d)$ to approximate REBA-based human comfort scores from hand data, resulting in a quadratic tertiary cost \[ \min_{\chi} \tfrac{1}{2}\chi^T H_{\text{ergo}}\chi + g_{\text{ergo}}^T\chi, \] with $H_{\text{ergo}}$ and $g_{\text{ergo}}$ obtained offline from human demonstrations. The solver proceeds lexicographically, at each level projecting the candidate solution into the nullspace of higher-priority tasks to ensure strict task ordering [2207.03435]. ## 3. Augmentation Relative to Standard HQP Standard HQP for inverse kinematics fixes $\dot{x}_d$ a priori from a trajectory generator, optimizing $\chi = \dot{q}$ exclusively. AHQP augments this structure by treating $\dot{x}_d$ as an optimization variable, yielding several key modifications: - The CLIK cost incorporates $(I+K_p\Delta t)\dot{x}_d$ directly in the first-level QP. - The slack cost on $s$ explicitly softens workspace boundaries, allowing adaptive shaping of the shared human-robot region in real time. - The ergonomics cost is handled directly within the same hierarchical stack, removing the need for external planners. This unification enables real-time integration of kinematic consistency, workspace adaptation, and ergonomic optimization within a single lexicographic QP loop [2207.03435]. ## 4. Vision-Driven Task Adaptation and Human-Robot Interaction AHQP tightly couples external vision modules with hierarchical control via: - **Object-Surface Classification:** A ResNet+SVM classifier on RGB images provides a binary output $y_\text{surf}\in\{\text{drilled},\text{smooth}\}$; during collaborative states (recognized via action detection), this constraint dynamically restricts EE orientation by updating $x_{d,\min}, x_{d,\max}$. - **Action Recognition:** SlowOnly@ResNet50, pretrained on Kinetics-400, infers a class confidence vector $y_{\text{act}}\in\mathbb{R}^{30}$. If the "start walk" action is top-ranked, the HRSW window shifts adaptively along direction $u_\text{move}$, modifying workspace bounds as \[ x_{d,\min}(t) = x_{d,\min}(t-1) + \alpha u_\text{move},\quad x_{d,\max}(t) = x_{d,\max}(t-1) + \alpha u_\text{move}, \] where $\alpha$ is a step parameter. - **3D Human-Hand Tracking:** OpenPose processes RGB-D frames to estimate human hand positions $x_h$. Assuming $x_h \approx x_d$, the ergonomics cost $e_s = f(x_d)$ can be evaluated directly online. This perception pipeline allows for instantaneous, vision-driven adaptation of both workspace and ergonomic constraints, enabling sociable human-commanded robot behavior [2207.03435]. ## 5. End-to-End Algorithmic Workflow AHQP's real-time control stack, as implemented on the MOCA platform (Franka Panda with mobile base), follows this loop at approximately 1 kHz: 1. Acquire camera streams (RGB and RGB-D). 2. Classify object surfaces via ResNet+SVM to determine $y_\text{surf}$. 3. Extract hand positions $x_h$ using OpenPose keypoints. 4. Recognize human actions using SlowOnly@ResNet50, yielding $y_\text{act}$. 5. Decode collaborative state and direction; update shared workspace constraints and EE goals accordingly. 6. Assemble Level 1 cost ($A_\text{clik}$, $b_\text{clik}$) with joint and EE limits. 7. Define Level 2 cost ($\|s\|^2$) and constraints ($C_s$, $d_s$). 8. Add Level 3 ergonomics cost ($\chi^T H_\text{ergo}\chi + g_\text{ergo}^T\chi$). 9. Solve the lexicographic QP for optimal $\chi^* = [\dot{q}^*;\ \dot{x}_d^* ]$ and $s^*$. 10. Issue $\dot{q}^*$ to robot control (impedance law) and update $x_d$ with $\dot{x}_d^*$. This pipeline enables fluid, closed-loop adaptation to human intent and environment while prioritizing both task performance and user comfort [2207.03435]. ## 6. Empirical Validation and Performance The AHQP framework demonstrates the following quantitative results on the MOCA platform: - **Object-surface classification:** 100% accuracy with ResNet50+SVM over a 2,000-image benchmark. - **Action recognition:** 86.55% accuracy on the HRI30 dataset with SlowOnly@ResNet50 pretrained on Kinetics-400. - **Ergonomics in tool-handover:** AHQP maintains a human ergonomics score $e_s \approx 0.12$ versus 4.2 without ergonomics optimization. - **Iterative workspace adaptation:** During “follow-the-human” trials, cumulative REBA-based scores remain below 1.0 with AHQP and rise above 3.5 without ergonomics. - **Operator usability:** NASA-TLX surveys reveal reduced mental and physical load, and improved perceived performance, for users assisted by AHQP's ergonomics optimization. Collectively, these results highlight AHQP's effectiveness in fusing action-recognition signals, adaptive workspace soft constraints, and ergonomics into a unified real-time controller, yielding fluid and sociable human-robot interaction while promoting user comfort and trust in automation [2207.03435]. Source: https://www.emergentmind.com/topics/augmented-hqp-ahqp