diff --git a/doc/Doxyfile.extra.in b/doc/Doxyfile.extra.in
index d3f1e574e1f0933032771448685f6e5a1e804738..1d951dfd4c54684840d5f3b18542474ae74a5ef5 100644
--- a/doc/Doxyfile.extra.in
+++ b/doc/Doxyfile.extra.in
@@ -1,6 +1,6 @@
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
diff --git a/doc/additionalDoc/codeorg.h b/doc/additionalDoc/codeorg.h
new file mode 100644
index 0000000000000000000000000000000000000000..9aaac92db4c7289f98ca93be052f63b03b837567
--- /dev/null
+++ b/doc/additionalDoc/codeorg.h
@@ -0,0 +1,24 @@
+/** \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"
+
+*/
diff --git a/doc/additionalDoc/feature-doc.h b/doc/additionalDoc/feature-doc.h
new file mode 100644
index 0000000000000000000000000000000000000000..50dec82f9644b3007126f8b582461f5abe57b66f
--- /dev/null
+++ b/doc/additionalDoc/feature-doc.h
@@ -0,0 +1,35 @@
+/* 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>
+ */
diff --git a/doc/additionalDoc/install-doc.h b/doc/additionalDoc/install-doc.h
new file mode 100644
index 0000000000000000000000000000000000000000..c2d81abbcda3028142d1bad1147f11bf823a3ba1
--- /dev/null
+++ b/doc/additionalDoc/install-doc.h
@@ -0,0 +1,67 @@
+/** \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
+
+
+*/
diff --git a/doc/additionalDoc/namespace-doc.h b/doc/additionalDoc/namespace-doc.h
new file mode 100644
index 0000000000000000000000000000000000000000..49e338a4c613b64213e912ba7a81cee297556590
--- /dev/null
+++ b/doc/additionalDoc/namespace-doc.h
@@ -0,0 +1,10 @@
+/* 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.
+*/
+
diff --git a/doc/additionalDoc/package.h b/doc/additionalDoc/package.h
index e4ea907e6d9dc871696b2420b034cce9674482fe..d8213c76d7778a3742ff4777a0708ff5e915bdeb 100644
--- a/doc/additionalDoc/package.h
+++ b/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
diff --git a/doc/additionalDoc/parameter-server-doc.h b/doc/additionalDoc/parameter-server-doc.h
new file mode 100644
index 0000000000000000000000000000000000000000..834da845ba0d1da91bd8781dbcb4b66701b18541
--- /dev/null
+++ b/doc/additionalDoc/parameter-server-doc.h
@@ -0,0 +1,7 @@
+/** \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.
+
+
+ */
diff --git a/doc/additionalDoc/references-doc.h b/doc/additionalDoc/references-doc.h
new file mode 100644
index 0000000000000000000000000000000000000000..83c10a2584aa75f86d840ad7751731a2ab97b596
--- /dev/null
+++ b/doc/additionalDoc/references-doc.h
@@ -0,0 +1,8 @@
+/** \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
+
+*/
diff --git a/doc/additionalDoc/sot-doc.h b/doc/additionalDoc/sot-doc.h
new file mode 100644
index 0000000000000000000000000000000000000000..c9275621596a3f75f44c647b971ac2c242d8094e
--- /dev/null
+++ b/doc/additionalDoc/sot-doc.h
@@ -0,0 +1,44 @@
+/** \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$
+*/
diff --git a/include/sot/core/feature-abstract.hh b/include/sot/core/feature-abstract.hh
index 869f1947a45144ad7d836a41bb573a18b80d9d32..8828364cc38f147b7ce71497c16a8e278f268004 100644
--- a/include/sot/core/feature-abstract.hh
+++ b/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
diff --git a/include/sot/core/feature-generic.hh b/include/sot/core/feature-generic.hh
index 47529e0ec4b612c787c2457d08774091c45cdd2f..d0b2050179e2f073b440359da5876726a0319cf1 100644
--- a/include/sot/core/feature-generic.hh
+++ b/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
diff --git a/include/sot/core/madgwickahrs.hh b/include/sot/core/madgwickahrs.hh
index bb66592764d31a21e587c0f195cdde1dbcea10d9..81b426bbba176a295112c5d0bde0a29a89a21aad 100644
--- a/include/sot/core/madgwickahrs.hh
+++ b/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>