iHQP

The iHQP solver (iHQP.h) is based on the work from Kanoun et. al: Kinematic control of redundant manipulators: generalizing the task priority framework to inequality tasks (paper) and permits to resolve hierarchical QP problems with generic equality and inequality constraints.

Hierarchies are solved by a cascade of QP problems where high-level tasks become constraints in lower level ones. Let’s consider for example the following stack:

\[\mathcal{S} = (\mathcal{T}_1 / \mathcal{T}_2)<<\mathcal{C}_1\]

This correspond to the following cascade of QPs:

  1. a QP solving \(\mathcal{T}_1<<\mathcal{C}_1\) and computing the higher priority solution ::math::mathbf{x}_1:

\[\begin{align} &\mathbf{x}_1 = \text{arg}\min\limits_{\mathbf{x}} \lVert \mathbf{A}_1\mathbf{x} - \mathbf{b}_1\rVert \newline &s.t. \quad \mathbf{d}_m\leq \mathbf{C}\mathbf{x}\leq\mathbf{d}_M \end{align}\]

  1. a QP solving \(\mathcal{T}_2<<\mathcal{C}_1<<\mathcal{T}_1^{opt}\) and computing the final solution \(\mathbf{x}_2 = \mathbf{x}^{opt}\):

\[\begin{align} &\mathbf{x}_2 = \text{arg}\min\limits_{\mathbf{x}} \lVert \mathbf{A}_2\mathbf{x} - \mathbf{b}_2\rVert \newline &s.t. \quad \mathbf{d}_m\leq \mathbf{C}\mathbf{x}\leq\mathbf{d}_M \newline & \quad \quad \quad \mathbf{A}_1\mathbf{x} = \mathbf{A}_1\mathbf{x}_1 \end{align}\]

the term \(\mathbf{A}_1\mathbf{x} = \mathbf{A}_1\mathbf{x}_1\) is called optimality constraint.

The generic k-th priority is implemented as:

\[\begin{split}\begin{align} &\mathbf{x}_k = \text{arg}\min\limits_{\mathbf{x}} \lVert \mathbf{A}_k\mathbf{x} - \mathbf{b}_k\rVert \nonumber \\ &s.t. \quad \mathbf{d}_m\leq \mathbf{C}\mathbf{x}\leq\mathbf{d}_M \nonumber \\ & \quad \quad \quad \mathbf{A}_1\mathbf{x} = \mathbf{A}_1\mathbf{x}_1 \nonumber \\ & \quad \quad \quad \mathbf{A}_2\mathbf{x} = \mathbf{A}_2\mathbf{x}_2 \nonumber \\ & \quad \quad \quad \quad \quad \ \vdots \nonumber \\ & \quad \quad \mathbf{A}_{k-1}\mathbf{x} = \mathbf{A}_{k-1}\mathbf{x}_{k-1} \nonumber \end{align}\end{split}\]

The iHQP solver consists in a front-end (derived from OpenSoT::Solver), that prepares the optimization problems to be solved starting from the given stack, and a back-end (derived from the OpenSoT::solvers::BackEnd base class in `BackEnd.h <>`__) which solves the optimization problem using an out-of-the-box QP solver. Different back-ends are available in OpenSoT and can be used in different front-ends. For the iHQP there are available the following back-ends:

Note

Some front-ends, e.g. qpOASES, permit to solve LP problems, i.e. optimization problems where the \(\mathbf{A}\) matrix is null (hence also the Hessian is null). GLPK is a LP dedicated solver that can not be used to solve QP problems.

The iHQP constructor permits to decide which back-end solver to use, as well as specify a vector of back-ends, one per hierarchy level.

Reqularization of the cost function can be added to handle task singularities as well as non square problems, i.e. when \(\mathbf{A}\in\mathbb{R}^{m \times n}\) with \(m < n\), through the constructor parameter eps_regularisation. Different values for each level of the stack can be set thorugh the setEpsRegularisation(const double eps, const unsigned int i) method.

Note

The regularization term can be seen as task with \(\mathbf{A} = \mathbf{I}\in\mathbb{R}^{n \times n}\), \(\mathbf{b} = \mathbf{0}\in\mathbb{R}^n\), and \(\mathbf{W} = \epsilon \in \mathbb{R}\). However the regularization is not considered when computing the optimality constraint of a level i, in order to do not saturate the remaining null-space. This may cause instability of the solution at the level i-1, which is well-posed at the level i due to the presence of the regularization term, and become ill-posed as a constraint at the level i-1. This problem can be solved directly acting on the task matrix \(\mathbf{A}_i\) as done in the nHQP solver.

Another way to specify a task that is NOT moved to the next level of priority when computing the optimality constraint, hence is used as a regularization, is through the setRegularisationTask(OpenSoT::tasks::Aggregated::TaskPtr regularisation_task) method of the OpenSoT::AutoStack class. For example in this way is possible to regularize at the velocity level a task written in acceleration.

Warning

The setRegularisationTask(OpenSoT::tasks::Aggregated::TaskPtr regularisation_task) method of the OpenSoT::AutoStack class is considered only by the iHQP solver for the moment!

The setActiveStack(const unsigned int i, const bool flag) permits to select a stack level to do not be solved during the next solve procedure.

The setOptions(const unsigned int i, const boost::any &opt) permits to set the options of the back-end at the i-th priority level. getBackEnd(const unsigned int i, BackEnd::Ptr& back_end) return a back-end pointer at the i-th level.