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{array}{c}{\mathbf{A}}_1\\ {\mathbf{A}}_2\end{array}\right],\left[\begin{array}{cc}{\mathbf{W}}_1& \mathbf{0}\\ \mathbf{0}& {\mathbf{W}}_2\end{array}\right],\left[\begin{array}{c}{\mathbf{b}}_1\\ {\mathbf{b}}_2\end{array}\right],{\mathbf{c}}_1+{\mathbf{c}}_2\}.$$

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

$$\begin{array}{rl}{\mathcal{F}}_3& ={(\left[\begin{array}{c}{\mathbf{A}}_1\\ {\mathbf{A}}_2\end{array}\right]\mathbf{x}-\left[\begin{array}{c}{\mathbf{b}}_1\\ {\mathbf{b}}_2\end{array}\right])}^T\left[\begin{array}{cc}{\mathbf{W}}_1& \mathbf{0}\\ \mathbf{0}& {\mathbf{W}}_2\end{array}\right](\left[\begin{array}{c}{\mathbf{A}}_1\\ {\mathbf{A}}_2\end{array}\right]\mathbf{x}-\left[\begin{array}{c}{\mathbf{b}}_1\\ {\mathbf{b}}_2\end{array}\right])+{({\mathbf{c}}_1+{\mathbf{c}}_2)}^T\mathbf{x}=\\ & =\sum _{i=1}^2\Vert {\mathbf{A}}_i\mathbf{x}-{\mathbf{b}}_i{\Vert }_{{\mathbf{W}}_i}+\sum _{i=1}^2{\mathbf{c}}_i^T\mathbf{x},\end{array}$$

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.