nHQP

The nHQP solver (nHQP.h) is based on the work from authors: de Lasa et al. (Prioritized Optimization for Task-Space Control) and permits to resolve hierarchical QP problems with generic inequality constraints.

As in the case of the iHQP solver, hierarchies are solved by a cascade of QP problems, but in this case, the null-space of high priority tasks is exploited to solve only the remaining available free DOFs, resulting in successive optimizations with reduced number of variables.

The generic k-th priority is implemented as:

\[\begin{align} &\mathbf{\bar{x}}_k^* = arg\min\limits_{\mathbf{\bar{x}}_k} \lVert \mathbf{\bar{A}}_k\mathbf{\bar{x}}_k - \mathbf{\bar{b}}_k\rVert \newline &s.t. \quad \mathbf{\bar{d}}_{m,k}\leq \mathbf{\bar{C}}_k\mathbf{x}\leq\mathbf{\bar{d}}_{M,k} \end{align}\]

with:

\[\begin{split}\begin{align} \mathbf{\bar{A}}_k &= \mathbf{A}_k\mathbf{N}_{k-1}, \\ \mathbf{\bar{b}}_k &= \mathbf{b}_k-\mathbf{A}_k\mathbf{x}_{k-1}, \\ \mathbf{\bar{C}}_k &= \mathbf{A}_k\mathbf{N}_{k-1}, \\ [\mathbf{\bar{d}}_{m,k}, \mathbf{\bar{d}}_{M,k}] &= [\mathbf{d}_{m,k}-\mathbf{A}_k\mathbf{x}_{k-1}, \mathbf{d}_{M,k}-\mathbf{A}_k\mathbf{x}_{k-1}] \end{align}\end{split}\]

and \(\mathbf{N}_{k-1}\) is a cumulative base of the null-space of the previous k-1 levels computed as:

\[\mathbf{N}_{k-1} = \mathbf{N}_{k-2} * Null(\mathbf{A}_{k-1}\mathbf{N}_{k-2}).\]

The final solution is computed ad \(\mathbf{x}_k = \mathbf{x}_{k-1} + \mathbf{N}_{k-1}*\mathbf{\bar{x}}_k^*\).

For the first priority level \(k = 0\):

\[\begin{split}\begin{align} \mathbf{N}_{0} &= \mathbf{I}, \\ \mathbf{x}_{0} &= \mathbf{0}, \\ \mathbf{N}_{1} &= Null(\mathbf{A}_1). \end{align}\end{split}\]

Notice that \(\mathbf{x}_k \in \mathbb{R}^p\), \(\mathbf{x}_{k-1} \in \mathbb{R}^l\), …, \(\mathbf{x}_1 \in \mathbb{R}^n\), with \(p < l < ... < n\). \(Null(\mathbf{H})\) can be computed thorugh SVD; in particular we use the Bidiagonal Divide and Conquer SVD provided by Eigen.

The back-ends available for the nHQP front-end are:

It is worth noticing that the computation of the SVD permits to access to information on the rank of the Task matrix with the possiblity to apply some strategy to handle singularities. In particular, we added regularization to the \(\mathbf{A}\) matrix and \(\mathbf{b}\) vector based on the singular Cartesian directions, and to the Hessian matrix based on the null-space.