[doc] Improve doc

Add clarifications on the overall scheme of the sot.
Try to improve the overall organization.

doc/Doxyfile.extra.in

 INPUT                  = @CMAKE_SOURCE_DIR@/include \
                          @CMAKE_SOURCE_DIR@/doc/additionalDoc
-IMAGE_PATH             = @PKG_CONFIG_DOXYGENDOCDIR@/pictures
+IMAGE_PATH             = @CMAKE_SOURCE_DIR@/doc/pictures
 
 FILE_PATTERNS          = *.cc *.cpp *.h *.hh *.hxx
 

doc/additionalDoc/codeorg.h

+/** \page codeorganization Code organization
+
+    \section sec_organization Organization of the code
+   
+The code is based on the <c>dynamic-graph</c> package, which provides the
+framework on which sot-core relies. Hence most of the code in <c>sot-core</c>
+consists of classes that derive from dynamic_graph::Entity.
+These entities are usually
+compiled and linked in their own dynamic library, as a python module
+
+Aside from the python modules, this package installs a core library libsot-core,
+that provides functions and code common to all modules. All python modules
+developed here link with libsot-core. For example, common mathematical
+entities, definitions and functions are in the core library.
+See \ref sot_core_base for a list of what's in this code base.
+
+This library implements
+\li A solver for the control law using tasks and features: the stack of tasks
+\li The framework of Tasks and Features, as defined in \ref Mansard2007, that
+\li Base classes for working with tasks and features, such as special matrices
+(rotation, twist) and vectors (quaternions, etc.)
+\li A basic C++ "machinery", the \ref factory, that "makes things work"
+
+*/

doc/additionalDoc/feature-doc.h

+/* Olivier Stasse
+ * CNRS, LAAS 2019
+ */
+/** \page page_featuredoc Features
+
+    The general definition of feature is provided by
+    dynamicgraph::sot::FeatureAbstract .
+
+    The list of features that are provided are:
+    <ul>
+    <li> Feature1D : \f$ L_2 \f$  norm of \f$ e(t)\f$
+    dynamicgraph::sot::Feature1D</li>
+    <li> FeatureGeneric : Externalisation of the Feature computation 
+    dynamicgraph::sot::FeatureGeneric </li>
+    <li> FeatureJointLimits : Distance to the joint limits and its Jacobian
+    dynamicgraph::sot::FeatureJointLimits </li>
+    <li> FeatureLineDistance : Regulate the distance of a line through a body
+    to a reference point
+    dynamicgraph::sot::FeatureLineDistance </li>
+    <li> FeaturePoint6d : Regulate the distance from a pose on a body to
+    a point reference.
+    dynamicgraph::sot::FeaturePoint6d </li>
+    <li> FeaturePoint6dRelative : Regulate a relative distance between
+    two bodies.
+    dynamicgraph::sot::FeatuePoint6dRelative </li>
+    <li> FeaturePosture : Regulate the posture between the current one
+    and a reference.
+    dynamicgraph::sot::FeaturePosture </li>
+    <li> FeatureReferenceHelper : dynamicgraph::sot::FeatureReferenceHelper
+    </li>
+    <li> FeatureTask : dynamicgraph::sot::FeatureTask </li>
+    <li> FeatureVector3 : dynamicgraph::sot::FeatureVector3 </li>
+    <li> FeatureVisualPoint : : dynamicgraph::sot::FeatureVisualPoint </li>
+    </ul>
+ */

doc/additionalDoc/install-doc.h

+/** \page page_RequirementsInstallation Requirements and Installation
+
+\section sec_requirements Requirements
+This package should be compiled and installed with cmake.
+
+This library is based on several packages:
+ \li <c>dynamic-graph</c> (https://github.com/stack-of-tasks/dynamic-graph)
+ \li <c>eigen</c>
+ \li <c>boost</c>
+ \li <c>pthread</c>
+ \li <c>python</c>
+ \li <c>pinocchio</c> (https://github.com/stack-of-tasks/pinocchio)
+ 
+Their presence will be checked with the use of pkg-config.
+
+\section sec_installation Installation
+\subsection subsec_ins_binaries From binary package
+The recommended way to install sot-core is to use
+<a href="https://robotpkg.openrobots.org/install.html">robotpkg</a> and its
+<a href="https://robotpkg.openrobots.org/robotpkg-wip.html">wip
+extension</a>.
+
+A short and quick way to get it on 16.04 LTS is to do the following:
+\verbatim
+sudo tee /etc/apt/sources.list.d/robotpkg.list <<EOF
+deb [arch=amd64] http://robotpkg.openrobots.org/wip/packages/debian/pub kinetic robotpkg
+deb [arch=amd64] http://robotpkg.openrobots.org/packages/debian/pub kinetic robotpkg
+EOF
+\endverbatim
+This created a file to add robotpkg and robotpkg/wip as apt repositories.
+To identify the packages from this repository the server key must be added:
+\verbatim
+curl http://robotpkg.openrobots.org/packages/debian/robotpkg.key |
+   sudo apt-key add -
+\endverbatim
+The list of available packages is provided through:
+\verbatim
+sudo apt-get update
+\endverbatim
+Finally the package is installed with:
+\verbatim
+sudo apt-get install robotpkg-sot-core-v3
+\endverbatim
+
+
+\subsection subsec_ins_sourcefiles From source files
+Although not recommended you can install the repository
+after installing all the dependencies and type:
+\verbatim
+mkdir _build
+cd _build
+cmake .. -DCMAKE_BUILD_TYPE=RELEASE
+make -j 8
+\endverbatim
+
+To build the documentation
+\verbatim
+make doc
+\endverbatim
+
+To test the code
+\verbatim
+make test
+\endverbatim
+
+
+*/

doc/additionalDoc/namespace-doc.h

+/* Olivier Stasse
+ * CNRS, LAAS 2019
+ */
+
+/** \namespace dynamicgraph::sot
+
+    This is the namespace for a subset of helperd classes related to the
+    implementation of the Stack-Of-Tasks.
+*/
+    

doc/additionalDoc/package.h

@@ -6,81 +6,24 @@ controllers in the Stack of Tasks Framework as defined in \ref Mansard2007.
 It is specifically targeted to retain real-time performance while
 having high level software capabilities, like plugins and python scripting.
 
-\section sec_organization Organization of the code
-
-The code is based on the <c>dynamic-graph</c> package, which provides the
-framework on which sot-core relies. Hence most of the code in <c>sot-core</c>
-consists of classes that derive from dynamic_graph::Entity. These entities are usually
-compiled and linked in their own dynamic library, as a python module
-
-Aside from the python modules, this package installs a core library libsot-core,
-that provides functions and code common to all modules. All python modules
-developed here link with libsot-core. For example, common mathematical
-entities, definitions and functions are in the core library.
-See \ref sot_core_base for a list of what's in this code base.
-
-This library implements
-\li A solver for the control law using tasks and features: the stack of tasks
-\li The framework of Tasks and Features, as defined in \ref Mansard2007, that
-\li Base classes for working with tasks and features, such as special matrices
-(rotation, twist) and vectors (quaternions, etc.)
-\li A basic C++ "machinery", the \ref factory, that "makes things work"
-
-\section using Using the Stack of Tasks
-
-For people who develop software to control a real robot, this package
-alone is insufficient to simulate and control a robot.
-
-The following packages are recommended* in that case:
-\li sot-dynamic
-\li sot-pattern-generator
-\li sot-openhrp (openhrp python module)
-
-* These packages are in development at the time of writing and may
-be separated or joined along the way. Please check their status and updates
-at the jrl-umi3218 web page: https://github.com/jrl-umi3218
-
-\section sec_RequirementsInstallation Requirements and Installation
-
-This package should be compiled and installed with cmake.
-
-This library is based on several packages:
- \li <c>dynamic-graph</c> (https://github.com/jrl-umi3218/dynamic-graph)
- \li <c>jrl-mal</c> (https://github.com/jrl-umi3218/jrl-mal)
- \li <c>lapack</c>
- \li <c>boost</c>
- \li <c>pthread</c>
-
-Their presence will be checked with the use of pkg-config.
-
-\section sot_base Core library
-This package is centered around a core library that implements
-the basic classes needed for operation of the stack of tasks. For
-more information, see \ref sot_core_base.
-
-\section sot_plugins Python Modules
-While the main library provides a basic framework for computation of a
-control law using the Stack of Tasks, it is not expressive enough for
-typical usage scenarios (for example, controlling a humanoid
-robot). Hence, several "specialized" features and tasks have been developed,
-and can be used in the Stack of Tasks. For a list of python modules and a short
-description, see \ref plugins_list.
-
-\section operation Operation of the stack of tasks
-
-\subsection subsec_intro Introduction
-As explained in \ref Mansard2007, tasks can be pushed on the stack
-of tasks (SoT), and thus assigned a priority. The SoT then calculates
-(in an iterative manner) the control law, or joint velocity, necessary
-to realize the tasks (usually, bring a feature to its desired value).
-
-\subsection features Features
-Each feature computes some information
-from measurements, from the robot state,or/and
-from others features. Features should be lightweight
-regarding computation and be included at this level
-only if necessary. See documentation of class dynamicgraph::sot::FeatureAbstract
-for more information.
+The package includes the following functionnalities:
+<ul>
+<li> A parameter server object to register and collect information on your
+robot (see \subpage page_parameter_server) </li> 
+<li> From the internal SoT point of view a class called Device which is
+allowing a direct interaction with your robot.</li>
+<li> From the extern SoT point of view a class which makes the connection
+either with the hardware or with a simulator </li>
+<li> A full kinematic engine implemented in an incremental manner.</li>
+</ul>
+
+\subpage page_RequirementsInstallation 
+
+\subpage page_featuredoc 
+
+\subpage page_references
+    
+\subpage page_sot
 
 \subsection tasks Tasks
 
@@ -94,14 +37,6 @@ calculates a control law as explained in \ref Mansard2007.
 
 See documentation of class dynamicgraph::sot::TaskAbstract for more information.
 
-\section References
-\anchor Mansard2007
-<b>"Task sequencing for sensor-based control"</b>,
-<em>N. Mansard, F. Chaumette,</em>
-IEEE Trans. on Robotics, 23(1):60-72, February 2007
-
-
-
 
 
 @defgroup sot_core_base Sot-core library (libsot-core.[dll|so])
@@ -138,10 +73,6 @@ Hence you can display sot::VectorQuaternion or sot::MatrixTwist objects as
 strings, or set them from the command line (using a script interpreter from
 the dynamic-graph package).
 
-\subsection subsec_Features Features
-The class dynamicgraph::sot::FeatureAbstract is the base class for features.
-all other classes are in entity \ref sot_plugins "python modules".
-For more information on what is a feature, see the documentation of class dynamic_graph::sot::FeatureAbstract.
 
 \subsection subsec_Tasks Tasks
 

doc/additionalDoc/parameter-server-doc.h

+/** \page page_parameter_server Parameter Server
+
+    In order to register and recover information on your robot it is possible
+    to register information using the class dynamicgraph::sot::RobotUtil.
+
+    
+ */

doc/additionalDoc/references-doc.h

+/** \page page_references References
+
+\anchor Mansard2007
+<b>"Task sequencing for sensor-based control"</b>,
+<em>N. Mansard, F. Chaumette,</em>
+IEEE Trans. on Robotics, 23(1):60-72, February 2007
+
+*/

doc/additionalDoc/sot-doc.h

+/** \page page_sot Stack-of-Tasks
+    
+    \f$ {\bf e}(t)= {\bf s}^*(t) - {\bf s}(t) \f$
+    thus:
+    \f$
+    \dot{\bf e}(t)= \dot{\bf s}^*(t) - \dot{\bf s}(t)
+    \f$
+
+    \f$ 
+    \frac{\delta {\bf e}}{\delta {\bf t}}=
+    \frac{\delta {\bf s}^*}{\delta {\bf t}} - 
+    \frac{\delta {\bf s}}{\delta {\bf q}}
+    \frac{\delta {\bf q}}{\delta {\bf t}}
+    \f$
+
+    \f$ 
+    \dot{\bf e} =
+    \dot{\bf s}^* - 
+    {\bf J} \dot{\bf q}
+    \f$
+
+    \f$ 
+    \Leftrightarrow \dot{\bf q}  =
+    {\bf J}^{+}(\dot{\bf s}^* - \dot{\bf e})
+    \f$
+
+    Assuming that:
+    
+    \f$ 
+    \dot{\bf e}^* = -\lambda {\bf e} = 
+    - \lambda
+    ({\bf s}^* - \hat{\bf s} )
+    \f$
+    with \f$ \hat{\bf s}\f$ is the measured feature.
+    
+    Imposing \f$\dot{\bf e} = \dot{\bf e}^* \f$
+    then we have:
+    
+    \f$
+    \dot{\bf q}  =
+    {\bf J}^{+}(\dot{\bf s}^* + \lambda {\bf e}) = 
+    {\bf J}^{+}(\dot{\bf s}^* + \lambda{\bf s}^* - \lambda \hat{\bf s})
+    \f$
+*/

include/sot/core/feature-abstract.hh

@@ -40,6 +40,7 @@ namespace dynamicgraph {
       \ingroup features
       \brief This class gives the abstract definition of a feature.
 
+      \paragraph par_features_definition Definition
       In short, a feature is a data evolving according to time.  It is defined
       by a vector \f${\bf s}(t) \in \mathbb{R}^n \f$ where \f$ t \f$ is the
       time.  By default a feature has a desired \f${\bf s}^*(t) \f$.
@@ -47,7 +48,8 @@ namespace dynamicgraph {
       reference. The
       feature is in charge of collecting its own current state.  A feature is
       supposed to compute an error between its current state and the desired
-      one: \f$ e(t) = {\bf s}^*(t) - {\bf s}(t) \f$.  A feature is supposed to
+      one: \f$ {\bf e}(t) = {\bf s}^*(t) - {\bf s}(t) \f$.
+      A feature is supposed to
       compute a Jacobian according to the robot state vector \f$ \frac{\delta
       {\bf s}(t)}{\delta {\bf q}(t)}\f$.
 
@@ -56,7 +58,43 @@ namespace dynamicgraph {
       evolution of the feature with respect to time: \f$\frac{\partial
       f}{\partial t}\f$.
 
-      \image html pictures/feature.png "Feature diagram: Feature types derive from FeatureAbstract. Each feature has a reference of the same type and compute an error by comparing errorSIN signals from itself and from the reference." width=15cm
+      \paragraph par_features_jacobian Jacobian
+      The Jacobian according to the robot state vector \f$ \frac{\delta
+      {\bf s}(t)}{\delta {\bf q}(t)}\f$ is provided through
+      signal dynamicgraph::sot::FeatureAbstract::jacobianSOUT.
+      This Jacobian is sometimes
+      (see dynamicgraph::sot::FeaturePosition6d)
+      related to a robot's body. In this case the Jacobian can be expressed
+      by a composition of the articular Jacobian specific to this body, and
+      a local Jacobian relating the feature variation and the body velocity:
+      \f$ \frac{\delta {\bf s}(t)}{\delta {\bf q}(t)} =
+      \frac{\delta {\bf s}(t)}{ {\bf v}_{\mathcal{B}}(t)}
+      \frac{{\bf v}_{\mathcal{B}}(t)}{\delta {\bf q}(t)} =
+      {\bf L}({\bf{q}){}^\mathcal{B}{\bf J}(\bf q}) 
+      \f$.
+      Usually \f$ {}^\mathcal{B}{\bf J}(\bf q)\f$ is provided by the package
+      sot-dynamic-pinocchio, and more precisely the Entity dynamic.
+      It can be provided through an input signal, and the Feature has to
+      implement only \f${\bf L}(\bf{q})\f$. It is for instance the case
+      for the class dynamicgraph::sot::FeatureVisualPoint.
+      
+      \image html feature.png "Feature diagram: Feature types derive from
+      FeatureAbstract. Each feature has a reference of the same type and
+      compute an error by comparing
+      errorSIN
+      signals from itself and from the
+      reference." 
+
+      \paragraph par_features_error_dot \f$ \dot{\bf e}(t) \f$
+      Let us recall that a task is defined as:
+      \f$ {\bf e}(t)= {\bf s}^*(t) - {\bf s}(t) \f$
+      thus:
+      \f$ \dot{\bf e}(t)= \dot{\bf s}^*(t) - \dot{\bf s}(t) \f$
+      When a feature is static \f$\dot{\bf s}^*(t)=0 \f$ otherwise one has to
+      provide it. It is done through the signal
+      dynamicgraph::sot::FeatureAbstract::errordotSIN
+      and should
+      be provided in the reference frame of the feature.
     */
     class SOT_CORE_EXPORT FeatureAbstract
       :public Entity
@@ -66,7 +104,8 @@ namespace dynamicgraph {
       static const std::string CLASS_NAME;
 
       /*! \brief Returns the name class. */
-      virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
+      virtual const std::string& getClassName( void ) const
+      { return CLASS_NAME; }
 
       /*! \brief Register the feature in the stack of tasks. */
       void featureRegistration( void );
@@ -148,7 +187,8 @@ namespace dynamicgraph {
 	@{ */
       /*! \brief This vector specifies which dimension are used to perform the computation.
 	For instance let us assume that the feature is a 3D point. If only the Y-axis should
-	be used for computing error, activation and Jacobian, then the vector to specify
+	be used for computing error, activation and Jacobian,
+        then the vector to specify
 	is \f$ [ 0 1 0] \f$.*/
       SignalPtr< Flags,int > selectionSIN;
 
@@ -168,13 +208,16 @@ namespace dynamicgraph {
       SignalTimeDependent< dg::Vector,int > errordotSOUT;
 
       /*! \brief This signal returns the Jacobian of the current value
-	according to the robot state: \f$ J(t) = \frac{\delta{\bf s}^*(t)}{\delta {\bf q}(t)}\f$ */
+	according to the robot state:
+        \f$ J(t) = \frac{\delta{\bf s}^*(t)}{\delta {\bf q}(t)}\f$
+      */
       SignalTimeDependent<dg::Matrix,int> jacobianSOUT;
 
       /*! \brief Returns the dimension of the feature as an output signal. */
       SignalTimeDependent<unsigned int,int> dimensionSOUT;
 
-      /*! \brief This method write a graph description on the file named FileName. */
+      /*! \brief This method write a graph description on the file named
+        FileName. */
       virtual std::ostream & writeGraph(std::ostream & os) const;
 
       /// Return true for children that provide the errordot output signal

include/sot/core/feature-generic.hh

@@ -44,12 +44,12 @@ namespace dg = dynamicgraph;
   \brief Class that defines a generic implementation of the abstract interface for features.
 
   This class is very useful if the feature can be easily computed using
-  the basic operator provided. For instance a free space controller on a end-effector
-  is basically directly computed from the Jacobian provided by dyn and some appropriate
-  addition and soustraction.
-  Instead of building a specific feature for this, it is possible to use the signals
-  and plug the computed error, Jacobian and activation to the input of this
-  generic feature implementation.
+  the basic operator provided. For instance a free space controller on a
+  end-effector  is basically directly computed from the Jacobian provided
+  by dyn and some appropriate  addition and soustraction.
+  Instead of building a specific feature for this, it is possible to use the
+  signals  and plug the computed error, Jacobian and activation to the input
+  of this generic feature implementation.
 
 */
 class SOTFEATUREGENERIC_EXPORT FeatureGeneric

include/sot/core/madgwickahrs.hh

@@ -70,7 +70,7 @@ namespace dynamicgraph {
     <ul>
     <li>m_accelerometerSIN: \f$[a_x, a_y, a_z]^T\f$ in \f$m.s^{-2}\f$</li>
     <li>m_gyroscopeSIN: \f$[g_x, g_y, g_z]^T\f$ in \f$rad.s^{-1}\f$</li>
-    <li>m_imu_quatSOUT: \f$[q_0, q_1, q_2, q_3]^T</li> estimated rotation
+    <li>m_imu_quatSOUT: \f$[q_0, q_1, q_2, q_3]^T\f$ </li> estimated rotation
     as a quaternion</li>
     </ul>