Working on sphinx documentation

docs/source/constraint.rst

@@ -1,6 +1,5 @@
 Constraint
 ==========
-
 In OpenSoT, **constraints** are utilized to limit a cost function. Considering a specific *linear constraint*, denoted as :math:`\mathcal{C}_1`, it is defined by its corresponding matrices and vectors:
 
 .. math::
@@ -21,7 +20,7 @@ for **bounds** in the form :math:`\mathbf{d}_{m,1} \leq \mathbf{x} \leq \mathbf{
 
 for **equality constraints** in the form :math:`\mathbf{C}_1\mathbf{x} = \mathbf{d}_1`.
 
-A *Constraint* object is inherited from the base class `Constraint.h <https://advrhumanoids.github.io/OpenSoT/api/classOpenSoT_1_1Constraint.html#exhale-class-classopensot-1-1constraint>`__, where the method ``update(const Vector_type& x)`` must be implemented to assign:
+A *Constraint* object is inherited from the base class ``OpenSoT::Constraint`` in `Constraint.h <https://advrhumanoids.github.io/OpenSoT/api/classOpenSoT_1_1Constraint.html#exhale-class-classopensot-1-1constraint>`__, where the method ``update(const Vector_type& x)`` must be implemented to assign:
 
 - the matrix :math:`\mathbf{C}` as ``Eigen::MatrixXd _Aineq`` and the vectors :math:`\mathbf{d}_m` and :math:`\mathbf{d}_M` as ``Eigen::VectorXd _bLowerBound`` and ``Eigen::VectorXd _bUpperBound`` for an **inequality constraint**;
 - the vectors :math:`\mathbf{d}_m` and :math:`\mathbf{d}_M` as ``Eigen::VectorXd _lowerBound`` and ``Eigen::VectorXd _upperBound`` for a **bound**;
@@ -45,3 +44,17 @@ A *SubConstraints* can be instantiated from a *Constraint* using the ``SubConstr
    A *SubConstraint* holds a reference to the *Constraint* that was used to instantiate it. As a result, any modifications applied to the primary *Constraint* are reflected in the *SubCOnstraint*, and vice versa. **When the** ``update(const Vector_type &x)`` **method is invoked on the** *SubConstraint*, **the referenced** *Constraint* **is also updated.**
 
 A common application of a *SubConstraint* is to focus on a specific portion of a constraint. For instance, in a *Joint Limits Constraint*, a *SubCOnstraint* can be employed to do not consider limitations on the floating-base part of the state.
+
+BilateralConstraint:
+--------------------
+A *constraint* can be created also from bare ``Eigen::MatrixXd`` and ``Eigen::VectorXd`` using the ``BilateralConstraint`` class in `BilateralConstraint.h <file:///home/enrico/catkin_ws/external/OpenSoT/docs/build/html/api/classOpenSoT_1_1constraints_1_1BilateralConstraint.html>`__:
+
+.. code-block:: cpp
+   
+   //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);
+

docs/source/index.rst

@@ -28,6 +28,7 @@ Main features:
    intro
    task
    constraint
+   stack
    aknowledge
    api/library_root
 

docs/source/stack.rst

@@ -0,0 +1,118 @@
+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 <https://homepages.laas.fr/ostasse/mansard_icar09.pdf>`__). 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.:
+
+.. code-block:: cpp
+   
+   //Creates a task
+   Eigen::MatrixXd A(2,2); A.rand();
+   Eigen::VectorXd b(2); b.rand();
+   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 :math:`\mathcal{T}_3 = \mathcal{T}_1 + \mathcal{T}_2`:
+
+.. math::
+  
+   \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 :math:`\mathcal{T}_3` will be:
+
+.. math::
+
+   \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.:
+
+.. code-block:: cpp
+
+   //Creates tasks
+   Eigen::MatrixXd A(2,2);
+   Eigen::VectorXd b(2);
+   auto t1 = std::make_shared<OpenSoT::tasks::GenericTask>("task1", A.rand(), b.rand());
+   auto t2 = std::make_shared<OpenSoT::tasks::GenericTask>("task2", A.rand(), b.rand());
+	
+   //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.: 
+
+.. code-block:: cpp
+
+   //Creates tasks
+   Eigen::MatrixXd A(3,3);
+   Eigen::VectorXd b(3);
+   auto t1 = std::make_shared<OpenSoT::tasks::GenericTask>("task1", A.rand(), b.rand());
+   auto t2 = std::make_shared<OpenSoT::tasks::GenericTask>("task2", A.rand(), b.rand());
+	
+   //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:
+
+.. code-block:: cpp
+
+   //Creates tasks
+   Eigen::MatrixXd A(2,2);
+   Eigen::VectorXd b(2);
+   auto t1 = std::make_shared<OpenSoT::tasks::GenericTask>("task1", A.rand(), b.rand());
+   auto t2 = std::make_shared<OpenSoT::tasks::GenericTask>("task2", A.rand(), b.rand());
+   
+   //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:
+
+.. code-block:: cpp
+
+   //Creates tasks
+   Eigen::MatrixXd A(2,2);
+   Eigen::VectorXd b(2);
+   auto t1 = std::make_shared<OpenSoT::tasks::GenericTask>("task1", A.rand(), b.rand());
+   auto t2 = std::make_shared<OpenSoT::tasks::GenericTask>("task2", A.rand(), b.rand());
+   
+   //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 <https://www.worldscientific.com/doi/abs/10.1142/S0219843621500080>`__) that permits to create complex QP problems.
+
+The *stack* object is implemented through the ``OpenSoT::AutoStack`` class in `Autostack.h <https://advrhumanoids.github.io/OpenSoT/api/classOpenSoT_1_1AutoStack.html>`__. **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.** 

docs/source/task.rst

@@ -1,6 +1,5 @@
 Task
 ====
-
 In OpenSoT, **tasks** are utilized to construct a cost function. Considering a specific *task*, denoted as :math:`\mathcal{T}_1`, it is defined by its corresponding matrices and vectors:
 
 .. math::
@@ -18,32 +17,10 @@ which defines the scalar cost function:
    & = \mathbf{x}^T\mathbf{A}_1^T\mathbf{W}_1\mathbf{A}_1\mathbf{x} -2\mathbf{b}_1^T\mathbf{W}_1\mathbf{A}_1\mathbf{x} + \mathbf{c}_1^T\mathbf{x}.  
    \end{align}  
    
-Multiple *Tasks* can be summed together to form complex cost fucntions, for instance :math:`\mathcal{T}_3 = \mathcal{T}_1 + \mathcal{T}_2`:
-
-.. math::
-  
-   \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 :math:`\mathcal{T}_3` will be:
-
-.. math::
-
-   \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*.
-
 .. note:: 
    Most of the time, the :math:`\mathbf{c}` term is used to implement the *Lasso* or an *L-1 norm*; thus, it is typically not utilized.
 
-A *Task* object is inherited from the base class `Task.h <https://advrhumanoids.github.io/OpenSoT/api/classOpenSoT_1_1Task.html#exhale-class-classopensot-1-1task>`__, where the method ``_update(const Vector_type &x)`` must be implemented to assign the matrix :math:`\mathbf{A}` as ``Eigen::MatrixXd _A``, along with the vectors :math:`\mathbf{b}` as ``Eigen::VectorXd _b`` and :math:`\mathbf{c}` as ``Eigen::VectorXd _c``. **This assignment will take place every time the** ``update(const Vector_type &x)`` **method is invoked.**   
+A *Task* object is inherited from the base class ``OpenSoT::Task`` in `Task.h <https://advrhumanoids.github.io/OpenSoT/api/classOpenSoT_1_1Task.html#exhale-class-classopensot-1-1task>`__, where the method ``_update(const Vector_type &x)`` must be implemented to assign the matrix :math:`\mathbf{A}` as ``Eigen::MatrixXd _A``, along with the vectors :math:`\mathbf{b}` as ``Eigen::VectorXd _b`` and :math:`\mathbf{c}` as ``Eigen::VectorXd _c``. **This assignment will take place every time the** ``update(const Vector_type &x)`` **method is invoked.**   
 
 The ``Hessian Type`` can be set according to the type of Hessian generated by the task as:
 
@@ -80,3 +57,16 @@ A *SubTask* can be instantiated from a *Task* using the ``SubTask(TaskPtr taskPt
    A *SubTask* holds a reference to the *Task* that was used to instantiate it. As a result, any modifications applied to the primary *Task* are reflected in the *SubTask*, and vice versa. **When the** ``update(const Vector_type &x)`` **method is invoked on the** *SubTask*, **the referenced** *Task* **is also updated.**
 
 A common application of a *SubTask* is to focus on a specific portion of a task. For instance, in a *Cartesian Task*, a *SubTask* can be employed to isolate the positional component while disregarding the orientation.
+
+GenericTask
+-----------
+A *task* can be created also from bare ``Eigen::MatrixXd`` and ``Eigen::VectorXd`` using the ``GenericTask`` class in `GenericTask.h <file:///home/enrico/catkin_ws/external/OpenSoT/docs/build/html/api/classOpenSoT_1_1tasks_1_1GenericTask.html>`__:
+
+.. code-block:: cpp
+   
+   //Creates a task
+   Eigen::MatrixXd A(2,2); A.rand();
+   Eigen::VectorXd b(2); b.rand();
+   auto t1 = std::make_shared<OpenSoT::tasks::GenericTask>("task1", A, b);
+
+Various set methods can be used to update the internal matrices and vectors. A *linear task* can be created through the ``GenericLPTask`` in `GenericLPTask.h <file:///home/enrico/catkin_ws/external/OpenSoT/docs/build/html/api/classOpenSoT_1_1tasks_1_1GenericLPTask.html>`__.