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\):
The scalar cost function associated to \(\mathcal{T}_3\) will be:
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.