Started solver part with eHQP documentation

docs/source/constraint.rst

@@ -67,7 +67,7 @@ Another way to create a *constraint* from bare ``Eigen::MatrixXd`` and ``Eigen::
    //Creates a constraint
    Eigen::MatrixXd C(2,2); C.setIdentity();
    Eigen::VectorXd l(2), u(2);
-   l.rand(); u.rand();
+   l.Random(); u.Random();
    
    //Create variable
    OpenSoT::AffineHelper C2(2,2);

docs/source/index.rst

@@ -29,6 +29,7 @@ Main features:
    task
    constraint
    stack
+   solver
    variables
    aknowledge
    api/library_root

docs/source/solver.rst

@@ -0,0 +1,29 @@
+Solver
+======
+A **solver** implements a mathematical method to resolve a QP/LP problem (a *stack*).
+A *solver* is implemented deriving from the ``OpenSoT::Solver`` class in `Solver.h <https://advrhumanoids.github.io/OpenSoT/api/classOpenSoT_1_1Solver.html>`__ implementing the virtual method ``solve(Vector_type& solution)``.
+
+eHQP
+----
+The ``eHQP`` solver (`eHQP.h <https://advrhumanoids.github.io/OpenSoT/api/classOpenSoT_1_1solvers_1_1eHQP.html>`__) is based on the work from *Flacco et. al*: *Prioritized Multi-Task Motion Control of Redundant Robots under Hard Joint Constraints* (`paper <https://khatib.stanford.edu/publications/pdfs/Flacco_2012.pdf>`__) and permits to resolve QP problem with **equality only constraints**, without considering the :math:`\mathbf{c}` term in the cost function. Singularities are checked and avoided thorugh damped pseudo-inverse. Solution of the problem is provided by the use of ``Eigen::JacobiSVD`` (`webpage <https://eigen.tuxfamily.org/dox/classEigen_1_1JacobiSVD.html>`__).
+
+The eHQP solver permits to set mulitple levels of *hard* priorities between tasks. When two or more tasks are at a *soft* priority, they are intended to be summed in the same cost function with a different weight, for example, given two tasks :math:`\mathcal{T}_1` and :math:`\mathcal{T}_2`, is possible to write a cost function where both the tasks are solved together but with :math:`\mathcal{T}_1` which is 10 times more important than :math:`\mathcal{T}_2`:
+
+.. math::
+
+   \mathcal{T}_3 = \mathcal{T}_1 + 0.1*\mathcal{T}_2
+   
+meaning that when the solver will give more importance (ten times) to :math:`\mathcal{T}_1` w.r.t. :math:`\mathcal{T}_2`. However, if the two tasks are minimizing different quantities, they may not be scaled the same, leading to scaling issues (e.g. position in meters and orientation in radiants). Therefore, the weight *0.1* may not be enough to properly set a *soft* priority between the tasks. 
+
+In these cases is possible to set a **hard** priority, meaning that the highest priority task will be minimized regardless the less priority task. There are several techniques to enforce hard priority between tasks, for example *null-space projection*, which is implemented in the ``eHQP`` solver. To enforce hard-priorities between tasks using the *MoT* you just need to use the ``/`` operator:
+
+.. math::
+
+   \mathcal{S}_1 = \mathcal{T}_1 / \mathcal{T}_2 
+   
+where :math:`\mathcal{S}_1` is a *stack*. **Notice that the way the** *hard* **priority is implemented depends on the particular** *solver* **used**. *Soft* and *hard* priorities can be mixed together and several layers can be created up to saturate the Degrees of Freedom (DoFs) of the problem:
+
+.. math::
+
+   \mathcal{S}_1 = \mathcal{T}_1 / (\mathcal{T}_2+\mathcal{T}_3) / \mathcal{T}_4
+

docs/source/stack.rst

@@ -8,8 +8,8 @@ A single *task* can be used to initialize a *stack* by using the ``/=`` operator
 .. code-block:: cpp
    
    //Creates a task
-   Eigen::MatrixXd A(2,2); A.rand();
-   Eigen::VectorXd b(2); b.rand();
+   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
@@ -45,8 +45,8 @@ Multiple *tasks* can be summed up using the ``+`` operator. Relative weights can
    //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());
+   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; 
@@ -60,8 +60,8 @@ A *subtask* can be used as well inside a *stack*. A simple way to create a *subt
    //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());
+   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};
@@ -74,8 +74,8 @@ A *constraint* can be associated to *task* or a *stack* using the ``<<`` operato
    //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());
+   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();
@@ -94,8 +94,8 @@ Multiple *constraints* can be included using ``<<``. *Tasks* can be also used as
    //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());
+   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();

docs/source/task.rst

@@ -65,8 +65,8 @@ A *task* can be created also from bare ``Eigen::MatrixXd`` and ``Eigen::VectorXd
 .. code-block:: cpp
    
    //Creates a task
-   Eigen::MatrixXd A(2,2); A.rand();
-   Eigen::VectorXd b(2); b.rand();
+   Eigen::MatrixXd A(2,2); A.Random();
+   Eigen::VectorXd b(2); b.Random();
    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 <https://advrhumanoids.github.io/OpenSoT/api/classOpenSoT_1_1tasks_1_1GenericLPTask.html>`__.