Stack

The concept of Stack of Tasks was first introduced by Nicolas Mansard et al. in the seminal work: A Versatile Generalized Inverted Kinematics Implementation for Collaborative Working Humanoid Robots: The Stack of Tasks (paper). In OpenSoT a stack consists in one or more tasks and constraints and their relations.

A single task can be used to initialize a stack by using the /= operator, e.g.:

//Creates a task
Eigen::MatrixXd A(2,2); A.Random();
Eigen::VectorXd b(2); b.Random();
auto t1 = std::make_shared<OpenSoT::tasks::GenericTask>("task1", A, b);

//Creates a stack
OpenSoT::AutoStack::Ptr stack /= t1;

Multiple Tasks can be summed together to form complex cost functions, for instance \(\mathcal{T}_3 = \mathcal{T}_1 + \mathcal{T}_2\):

\[\mathcal{T}_3 = \left\{ \begin{bmatrix}\mathbf{A}_1\newline \mathbf{A}_2 \end{bmatrix}, \begin{bmatrix}\mathbf{W}_1 & \mathbf{0}\newline \mathbf{0} & \mathbf{W}_2 \end{bmatrix}, \begin{bmatrix} \mathbf{b}_1\newline \mathbf{b}_2 \end{bmatrix}, \mathbf{c}_1 + \mathbf{c}_2 \right\}.\]

The scalar cost function associated to \(\mathcal{T}_3\) will be:

\[\begin{align} \mathcal{F}_3 %& = \lVert \begin{bmatrix}\mathbf{A}_1\newline \mathbf{A}_2 \end{bmatrix}\mathbf{x} - \begin{bmatrix} \mathbf{b}_1\newline \mathbf{b}_2 \end{bmatrix}\rVert_{\begin{bmatrix}\mathbf{W}_1 & \mathbf{0}\newline \mathbf{0} & \mathbf{W}_2 \end{bmatrix}} + \left(\mathbf{c}_1 + \mathbf{c}_2\right)^T\mathbf{x} = \newline & = \left( \begin{bmatrix}\mathbf{A}_1\newline \mathbf{A}_2 \end{bmatrix}\mathbf{x} - \begin{bmatrix} \mathbf{b}_1\newline \mathbf{b}_2 \end{bmatrix} \right)^T \begin{bmatrix}\mathbf{W}_1 & \mathbf{0}\newline \mathbf{0} & \mathbf{W}_2 \end{bmatrix} \left( \begin{bmatrix}\mathbf{A}_1\newline \mathbf{A}_2 \end{bmatrix}\mathbf{x} - \begin{bmatrix} \mathbf{b}_1\newline \mathbf{b}_2 \end{bmatrix} \right) + \left(\mathbf{c}_1 + \mathbf{c}_2\right)^T\mathbf{x} = \newline %& = \left(\mathbf{x}^T \begin{bmatrix} \mathbf{A}_1^T & \mathbf{A}_2^T \end{bmatrix} - \begin{bmatrix}\mathbf{b}_1^T & \mathbf{b}_2^T \end{bmatrix}\right) \begin{bmatrix}\mathbf{W}_1 & \mathbf{0}\newline \mathbf{0} & \mathbf{W}_2 \end{bmatrix} \left( \begin{bmatrix}\mathbf{A}_1\newline \mathbf{A}_2 \end{bmatrix}\mathbf{x} - \begin{bmatrix} \mathbf{b}_1\newline \mathbf{b}_2 \end{bmatrix} \right) + \left(\mathbf{c}_1 + \mathbf{c}_2\right)^T\mathbf{x} = \newline %& = \left( \begin{bmatrix}\mathbf{x}^T\mathbf{A}_1^T\mathbf{W}_1 & \mathbf{x}^T\mathbf{A}_2^T\mathbf{W}_2 \end{bmatrix} - \begin{bmatrix} \mathbf{b}_1^T\mathbf{W}_1 & \mathbf{b}_2^T\mathbf{W}_2 \end{bmatrix}\right) \left( \begin{bmatrix}\mathbf{A}_1\newline \mathbf{A}_2 \end{bmatrix}\mathbf{x} - \begin{bmatrix} \mathbf{b}_1\newline \mathbf{b}_2 \end{bmatrix} \right) + \left(\mathbf{c}_1 + \mathbf{c}_2\right)^T\mathbf{x} = \newline %& = \mathbf{x}^T\mathbf{A}_1^T\mathbf{W}_1\mathbf{A}_1\mathbf{x} + \mathbf{x}^T\mathbf{A}_2^T\mathbf{W}_2\mathbf{A}_2\mathbf{x} -2\mathbf{b}_1^T\mathbf{W}_1\mathbf{A}_1\mathbf{x} -2\mathbf{b}_2^T\mathbf{W}_2\mathbf{A}_2\mathbf{x} + \left(\mathbf{c}_1 + \mathbf{c}_2\right)^T\mathbf{x} = \newline & = \sum_{i = 1}^{2} \lVert \mathbf{A}_i\mathbf{x} - \mathbf{b}_i\rVert_{\mathbf{W}_i} + \sum_{i = 1}^{2}\mathbf{c}_i^T\mathbf{x}, \end{align}\]

i.e. a weighted sum of cost functions associated to each Task.

Multiple tasks can be summed up using the + operator. Relative weights can be set by using the * operator, e.g.:

//Creates tasks
Eigen::MatrixXd A(2,2);
Eigen::VectorXd b(2);
auto t1 = std::make_shared<OpenSoT::tasks::GenericTask>("task1", A.Random(), b.Random());
auto t2 = std::make_shared<OpenSoT::tasks::GenericTask>("task2", A.Random(), b.Random());

//Creates a stack
OpenSoT::AutoStack::Ptr stack = t1 + 0.1*t2;

relative weights can be either scalar or matrices of the right size.

A subtask can be used as well inside a stack. A simple way to create a subtask is to use the % operator. Notice that is not important to keep track of the generated subtask once it is inserted inside a stack, e.g.:

//Creates tasks
Eigen::MatrixXd A(3,3);
Eigen::VectorXd b(3);
auto t1 = std::make_shared<OpenSoT::tasks::GenericTask>("task1", A.Random(), b.Random());
auto t2 = std::make_shared<OpenSoT::tasks::GenericTask>("task2", A.Random(), b.Random());

//Creates a stack
std::list<unsigned int> idx {0, 1};
OpenSoT::AutoStack::Ptr stack = t1%idx + 0.1*t2;

A constraint can be associated to task or a stack using the << operator:

//Creates tasks
Eigen::MatrixXd A(2,2);
Eigen::VectorXd b(2);
auto t1 = std::make_shared<OpenSoT::tasks::GenericTask>("task1", A.Random(), b.Random());
auto t2 = std::make_shared<OpenSoT::tasks::GenericTask>("task2", A.Random(), b.Random());

//Creates a constraint
Eigen::MatrixXd C(2,2); C.setIdentity();
Eigen::VectorXd l(2), u(2);
l[0] = l[1] = -10.;
u[0] = u[1] =  10.;
auto c1 = std::make_shared<OpenSoT::constraint::BilateralConstraint>("constraint1", C, l, u);

//Creates a stack
OpenSoT::AutoStack::Ptr stack = (t1 + 0.1*t2)<<c1;

Multiple constraints can be included using <<. Tasks can be also used as equality constraints and directly added:

//Creates tasks
Eigen::MatrixXd A(2,2);
Eigen::VectorXd b(2);
auto t1 = std::make_shared<OpenSoT::tasks::GenericTask>("task1", A.Random(), b.Random());
auto t2 = std::make_shared<OpenSoT::tasks::GenericTask>("task2", A.Random(), b.Random());

//Creates a constraint
Eigen::MatrixXd C(2,2); C.setIdentity();
Eigen::VectorXd l(2), u(2);
l[0] = l[1] = -10.;
u[0] = u[1] =  10.;
auto c1 = std::make_shared<OpenSoT::constraint::BilateralConstraint>("constraint1", C, l, u);

//Creates a stack
OpenSoT::AutoStack::Ptr stack /= t2
stack<<t1<<c1;

Note

Constraints can also be directly added to tasks. However this feature is not commonly used preferring to associate the constraint directly to the stack.

The % operator can be used as well to constraints to create subconstraints.

All these operators consists in a Domain Specific Language (DSL) named Math of Tasks (MoT) that permits to create complex QP problems.

The stack object is implemented through the OpenSoT::AutoStack class in Autostack.h. An autostack carries pointers to the associated tasks and constraints, therefore when the autostack is updated through the update(const Eigen::VectorXd & state) method, also the internal tasks and constraints are updated.