sot-core: Stack-of-Tasks

As explained in class dynamicgraph::sot::FeatureAbstract,

${\bf E}(t) = {\bf e}({\bf q}(t), t)= {\bf s}({\bf q}(t)) - {\bf s}^*(t)$

thus:

$\dot{\bf E}= \frac{\partial{\bf e}}{\partial{\bf q}} \dot{\bf q} + \frac{\partial{\bf e}}{\partial t} \\$

The features are responsible for computing:

${\bf E}(t)$
$\frac{\partial{\bf e}}{\partial t}$
$\frac{\partial{\bf e}}{\partial{\bf q}}$

The class dynamicgraph::sot::Task takes some features outputs as inputs. It imposes an exponential decrease, i.e. $\dot{\bf E} = -\lambda {\bf E}$ . It gives:

$-\lambda {\bf e} = \frac{\partial{\bf e}}{\partial{\bf q}} \dot{\bf q} + \frac{\partial{\bf e}}{\partial t}$

and computes solutions as:

$\dot{\bf q} = \frac{\partial{\bf e}}{\partial{\bf q}}^{\dagger} \left( - \lambda {\bf e} - \frac{\partial{\bf e}}{\partial t} \right) + K v$

where:

$\frac{\partial{\bf e}}{\partial{\bf q}}^{\dagger}$ is the pseudo-inverse of $\frac{\partial{\bf e}}{\partial{\bf q}}$ ,
$\dot{\bf q} \in \mathbb{R}^n$ ,
$K \in \mathcal{M}_{n,m}(\mathbb{R})$ is a base of the kernel of $\frac{\partial{\bf e}}{\partial{\bf q}}$ , of dimension ,
and spans $\mathbb{R}^m$ so that spans the kernel of $\frac{\partial{\bf e}}{\partial{\bf q}}$ .

$v$ is a free parameters left to tasks of lower priority.