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 $m$,
• and $v$ spans $\mathbb{R}^m$ so that $K v$ spans the kernel of $\frac{\partial{\bf e}}{\partial{\bf q}}$.

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