Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/sot/core/feature-abstract.hh	Lines:	10	15	66.7 %
Date:	2023-03-13 12:09:37	Branches:	1	2	50.0 %


/*
 * Copyright 2010,
 * François Bleibel,
 * Olivier Stasse,
 *
 * CNRS/AIST
 *
 */

#ifndef __SOT_FEATURE_ABSTRACT_H__
#define __SOT_FEATURE_ABSTRACT_H__

/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */

/* Matrix */
#include <dynamic-graph/linear-algebra.h>

/* SOT */
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/entity.h>

#include <sot/core/flags.hh>
#include <sot/core/pool.hh>

#include "sot/core/api.hh"

/* STD */
#include <string>

namespace dynamicgraph {
namespace sot {

/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */

/*! \class FeatureAbstract
  \ingroup features
  \brief This class gives the abstract definition of a feature.

  \par par_features_definition Definition
  In short, a feature is a data evolving according to time.  It is defined
  by a vector \f${\bf s}({\bf q}) \in \mathbb{R}^n \f$ where \f$ {\bf q} \f$ is
  a robot configuration, which depends on the time \f$ t \f$. By default a
  feature has a desired \f${\bf s}^*(t) \f$. \f${\bf s}^*\f$ is provided by
  another feature of the same type called 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) = e({\bf q}(t), t) =
  {\bf s}({\bf q}(t)) \ominus {\bf s}^*(t) \f$. Here, \f$ \ominus \f$ is the
  difference operator of Lie group in which \f$ {\bf s} \f$ and \f$ {\bf s}^*
  \f$ are. The documentation below assumes the Lie group is a vector space and
  \f$\ominus\f$ is the usual difference operator.

  A feature computes:
  \li the Jacobian according to the robot state vector \f$ J =
      \frac{\partial e}{\partial {\bf q}} = \frac{\partial{\bf s}}{\partial {\bf
  q}}\f$. \li the partial derivative of the error \f$ e \f$: \f$ \frac{\partial
  e}{\partial t} = - \frac{d{\bf s}^*}{dt}\f$.

  The task is in general computed from the value of the feature at the
  current instant \f$E(t) = e({\bf q},t)\f$. The derivative of \f$ E \f$ is:
  \f[
    \frac{dE}{dt} = J({\bf q}) \dot{q} + \frac{\partial e}{\partial t}
  \f]

  \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."

*/
class SOT_CORE_EXPORT FeatureAbstract : public Entity {
 public:
  /*! \brief Store the name of the class. */
  static const std::string CLASS_NAME;

  /*! \brief Returns the name class. */
  virtual const std::string &getClassName(void) const { return CLASS_NAME; }

  /*! \brief Register the feature in the stack of tasks. */
  void featureRegistration(void);

  void initCommands(void);

 public:
  /*! \brief Default constructor: the name of the class should be given. */
  FeatureAbstract(const std::string &name);
  /*! \brief Default destructor */
  virtual ~FeatureAbstract(void){};

  /*! \name Methods returning the dimension of the feature.
    @{ */

  /*! \brief Verbose method.
    \par res: The integer in which the dimension will be return.
    \par time: The time at which the feature should be considered.
    \return Dimension of the feature.
    \note Be careful with features changing their dimension according to time.
  */
  virtual unsigned int &getDimension(unsigned int &res, int time) = 0;

  /*! \brief Short method
    \par time: The time at which the feature should be considered.
    \return Dimension of the feature.
    \note Be careful with features changing their dimension according to time.
  */
  inline unsigned int getDimension(int time) {
    unsigned int res;
    getDimension(res, time);
    return res;
  }

  /*! \brief Shortest method
    \return Dimension of the feature.
    \note The feature is not changing its dimension according to time.
  */
  inline unsigned int getDimension(void) const {
    return dimensionSOUT.accessCopy();
  }
  /*! @} */

  /*! \name Methods to control internal computation.
    The main idea is that some feature may have a lower frequency
    than the internal control loop. In this case, the methods for
    computation are called only when needed.

    @{*/

  /*! \brief Compute the error between the desired feature and
    the current value of the feature measured or deduced from the robot state.

    \par[out] res: The error will be set into res.
    \par[in] time: The time at which the error is computed.
    \return The vector res with the appropriate value.
  */
  virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res,
                                             int time) = 0;

  /*! \brief Compute the Jacobian of the error according the robot state.

    \par[out] res: The matrix in which the error will be written.
    \return The matrix res with the appropriate values.
  */
  virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res,
                                                int time) = 0;

  /// Callback for signal errordotSOUT
  ///
  /// Copy components of the input signal errordotSIN defined by selection
  /// flag selectionSIN.
  virtual dynamicgraph::Vector &computeErrorDot(dynamicgraph::Vector &res,
                                                int time);

  /*! @} */

  /* --- SIGNALS ------------------------------------------------------------ */
 public:
  /*! \name Signals
    @{
  */

  /*! \name Input signals:
    @{ */
  /*! \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
    is \f$ [ 0 1 0] \f$.*/
  SignalPtr<Flags, int> selectionSIN;

  /// Derivative of the reference value.
  SignalPtr<dynamicgraph::Vector, int> errordotSIN;

  /*! @} */

  /*! \name Output signals:
    @{ */

  /*! \brief This signal returns the error between the desired value and
    the current value : \f$ E(t) = {\bf s}(t) - {\bf s}^*(t)\f$ */
  SignalTimeDependent<dynamicgraph::Vector, int> errorSOUT;

  /*! \brief Derivative of the error with respect to time:
   * \f$ \frac{\partial e}{\partial t} = - \frac{d{\bf s}^*}{dt} \f$ */
  SignalTimeDependent<dynamicgraph::Vector, int> errordotSOUT;

  /*! \brief Jacobian of the error wrt the robot state:
   * \f$ J = \frac{\partial {\bf s}}{\partial {\bf q}}\f$ */
  SignalTimeDependent<dynamicgraph::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. */
  virtual std::ostream &writeGraph(std::ostream &os) const;

  virtual SignalTimeDependent<dynamicgraph::Vector, int> &getErrorDot() {
    return errordotSOUT;
  }

  /*! @} */

  /* --- REFERENCE VALUE S* ------------------------------------------------- */
 public:
  /*! \name Reference
    @{
  */
  virtual void setReference(FeatureAbstract *sdes) = 0;
  virtual void unsetReference(void) { setReference(NULL); }
  virtual const FeatureAbstract *getReferenceAbstract(void) const = 0;
  virtual FeatureAbstract *getReferenceAbstract(void) = 0;
  virtual bool isReferenceSet(void) const { return false; }

  virtual void addDependenciesFromReference(void) = 0;
  virtual void removeDependenciesFromReference(void) = 0;

  /* Commands for bindings. */
  void setReferenceByName(const std::string &name);
  std::string getReferenceByName(void) const;
  /*! @} */
};

template <class FeatureSpecialized>
class FeatureReferenceHelper {
  FeatureSpecialized *ptr;
  FeatureAbstract *ptrA;

 public:
  FeatureReferenceHelper(void) : ptr(NULL) {}

  void setReference(FeatureAbstract *sdes);
  // void setReferenceByName( const std::string & name );
  void unsetReference(void) { setReference(NULL); }
  bool isReferenceSet(void) const { return ptr != NULL; }
  FeatureSpecialized *getReference(void) { return ptr; }
  const FeatureSpecialized *getReference(void) const { return ptr; }
};

template <class FeatureSpecialized>
void FeatureReferenceHelper<FeatureSpecialized>::setReference(
    FeatureAbstract *sdes) {
  ptr = dynamic_cast<FeatureSpecialized *>(sdes);
  ptrA = ptr;
}

#define DECLARE_REFERENCE_FUNCTIONS(FeatureSpecialized)             \
  typedef FeatureReferenceHelper<FeatureSpecialized> SP;            \
  virtual void setReference(FeatureAbstract *sdes) {                \
    if (sdes == NULL) {                                             \
      /* UNSET */                                                   \
      if (SP::isReferenceSet()) removeDependenciesFromReference();  \
      SP::unsetReference();                                         \
    } else {                                                        \
      /* SET */                                                     \
      SP::setReference(sdes);                                       \
      if (SP::isReferenceSet()) addDependenciesFromReference();     \
    }                                                               \
  }                                                                 \
  virtual const FeatureAbstract *getReferenceAbstract(void) const { \
    return SP::getReference();                                      \
  }                                                                 \
  virtual FeatureAbstract *getReferenceAbstract(void) {             \
    return (FeatureAbstract *)SP::getReference();                   \
  }                                                                 \
  bool isReferenceSet(void) const { return SP::isReferenceSet(); }  \
  virtual void addDependenciesFromReference(void);                  \
  virtual void removeDependenciesFromReference(void)
/* END OF define DECLARE_REFERENCE_FUNCTIONS */

#define DECLARE_NO_REFERENCE                                           \
  virtual void setReference(FeatureAbstract *) {}                      \
  virtual const FeatureAbstract *getReferenceAbstract(void) const {    \
    return NULL;                                                       \
  }                                                                    \
  virtual FeatureAbstract *getReferenceAbstract(void) { return NULL; } \
  virtual void addDependenciesFromReference(void) {}                   \
  virtual void removeDependenciesFromReference(void) {}                \
  /* To force a ; */ bool NO_REFERENCE
/* END OF define DECLARE_REFERENCE_FUNCTIONS */

}  // namespace sot
}  // namespace dynamicgraph

#endif  // #ifndef __SOT_FEATURE_ABSTRACT_H__


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			/*
2			* Copyright 2010,
3			* François Bleibel,
4			* Olivier Stasse,
5			*
6			* CNRS/AIST
7			*
8			*/
9
10			#ifndef __SOT_FEATURE_ABSTRACT_H__
11			#define __SOT_FEATURE_ABSTRACT_H__
12
13			/* --------------------------------------------------------------------- */
14			/* --- INCLUDE --------------------------------------------------------- */
15			/* --------------------------------------------------------------------- */
16
17			/* Matrix */
18			#include <dynamic-graph/linear-algebra.h>
19
20			/* SOT */
21			#include <dynamic-graph/all-signals.h>
22			#include <dynamic-graph/entity.h>
23
24			#include <sot/core/flags.hh>
25			#include <sot/core/pool.hh>
26
27			#include "sot/core/api.hh"
28
29			/* STD */
30			#include <string>
31
32			namespace dynamicgraph {
33			namespace sot {
34
35			/* --------------------------------------------------------------------- */
36			/* --- CLASS ----------------------------------------------------------- */
37			/* --------------------------------------------------------------------- */
38
39			/*! \class FeatureAbstract
40			\ingroup features
41			\brief This class gives the abstract definition of a feature.
42
43			\par par_features_definition Definition
44			In short, a feature is a data evolving according to time. It is defined
45			by a vector \f${\bf s}({\bf q}) \in \mathbb{R}^n \f$ where \f$ {\bf q} \f$ is
46			a robot configuration, which depends on the time \f$ t \f$. By default a
47			feature has a desired \f${\bf s}^(t) \f$. \f${\bf s}^\f$ is provided by
48			another feature of the same type called reference. The feature is in charge of
49			collecting its own current state. A feature is supposed to compute an error
50			between its current state and the desired one: \f$ E(t) = e({\bf q}(t), t) =
51			{\bf s}({\bf q}(t)) \ominus {\bf s}^*(t) \f$. Here, \f$ \ominus \f$ is the
52			difference operator of Lie group in which \f$ {\bf s} \f$ and \f$ {\bf s}^*
53			\f$ are. The documentation below assumes the Lie group is a vector space and
54			\f$\ominus\f$ is the usual difference operator.
55
56			A feature computes:
57			\li the Jacobian according to the robot state vector \f$ J =
58			\frac{\partial e}{\partial {\bf q}} = \frac{\partial{\bf s}}{\partial {\bf
59			q}}\f$. \li the partial derivative of the error \f$ e \f$: \f$ \frac{\partial
60			e}{\partial t} = - \frac{d{\bf s}^*}{dt}\f$.
61
62			The task is in general computed from the value of the feature at the
63			current instant \f$E(t) = e({\bf q},t)\f$. The derivative of \f$ E \f$ is:
64			\f[
65			\frac{dE}{dt} = J({\bf q}) \dot{q} + \frac{\partial e}{\partial t}
66			\f]
67
68			\image html feature.png "Feature diagram: Feature types derive from
69			FeatureAbstract. Each feature has a reference of the same type and
70			compute an error by comparing
71			errorSIN
72			signals from itself and from the
73			reference."
74
75			*/
76			class SOT_CORE_EXPORT FeatureAbstract : public Entity {
77			public:
78			/! \brief Store the name of the class. /
79			static const std::string CLASS_NAME;
80
81			/! \brief Returns the name class. /
82			virtual const std::string &getClassName(void) const { return CLASS_NAME; }
83
84			/! \brief Register the feature in the stack of tasks. /
85			void featureRegistration(void);
86
87			void initCommands(void);
88
89			public:
90			/! \brief Default constructor: the name of the class should be given. /
91			FeatureAbstract(const std::string &name);
92			/! \brief Default destructor /
93		16	virtual ~FeatureAbstract(void){};
94
95			/*! \name Methods returning the dimension of the feature.
96			@{ */
97
98			/*! \brief Verbose method.
99			\par res: The integer in which the dimension will be return.
100			\par time: The time at which the feature should be considered.
101			\return Dimension of the feature.
102			\note Be careful with features changing their dimension according to time.
103			*/
104			virtual unsigned int &getDimension(unsigned int &res, int time) = 0;
105
106			/*! \brief Short method
107			\par time: The time at which the feature should be considered.
108			\return Dimension of the feature.
109			\note Be careful with features changing their dimension according to time.
110			*/
111			inline unsigned int getDimension(int time) {
112			unsigned int res;
113			getDimension(res, time);
114			return res;
115			}
116
117			/*! \brief Shortest method
118			\return Dimension of the feature.
119			\note The feature is not changing its dimension according to time.
120			*/
121			inline unsigned int getDimension(void) const {
122			return dimensionSOUT.accessCopy();
123			}
124			/! @} /
125
126			/*! \name Methods to control internal computation.
127			The main idea is that some feature may have a lower frequency
128			than the internal control loop. In this case, the methods for
129			computation are called only when needed.
130
131			@{*/
132
133			/*! \brief Compute the error between the desired feature and
134			the current value of the feature measured or deduced from the robot state.
135
136			\par[out] res: The error will be set into res.
137			\par[in] time: The time at which the error is computed.
138			\return The vector res with the appropriate value.
139			*/
140			virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res,
141			int time) = 0;
142
143			/*! \brief Compute the Jacobian of the error according the robot state.
144
145			\par[out] res: The matrix in which the error will be written.
146			\return The matrix res with the appropriate values.
147			*/
148			virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res,
149			int time) = 0;
150
151			/// Callback for signal errordotSOUT
152			///
153			/// Copy components of the input signal errordotSIN defined by selection
154			/// flag selectionSIN.
155			virtual dynamicgraph::Vector &computeErrorDot(dynamicgraph::Vector &res,
156			int time);
157
158			/! @} /
159
160			/* --- SIGNALS ------------------------------------------------------------ */
161			public:
162			/*! \name Signals
163			@{
164			*/
165
166			/*! \name Input signals:
167			@{ */
168			/*! \brief This vector specifies which dimension are used to perform the
169			computation. For instance let us assume that the feature is a 3D point. If
170			only the Y-axis should be used for computing error, activation and Jacobian,
171			then the vector to specify
172			is \f$ [ 0 1 0] \f$.*/
173			SignalPtr<Flags, int> selectionSIN;
174
175			/// Derivative of the reference value.
176			SignalPtr<dynamicgraph::Vector, int> errordotSIN;
177
178			/! @} /
179
180			/*! \name Output signals:
181			@{ */
182
183			/*! \brief This signal returns the error between the desired value and
184			the current value : \f$ E(t) = {\bf s}(t) - {\bf s}^(t)\f$ /
185			SignalTimeDependent<dynamicgraph::Vector, int> errorSOUT;
186
187			/*! \brief Derivative of the error with respect to time:
188			* \f$ \frac{\partial e}{\partial t} = - \frac{d{\bf s}^}{dt} \f$ /
189			SignalTimeDependent<dynamicgraph::Vector, int> errordotSOUT;
190
191			/*! \brief Jacobian of the error wrt the robot state:
192			* \f$ J = \frac{\partial {\bf s}}{\partial {\bf q}}\f$ */
193			SignalTimeDependent<dynamicgraph::Matrix, int> jacobianSOUT;
194
195			/! \brief Returns the dimension of the feature as an output signal. /
196			SignalTimeDependent<unsigned int, int> dimensionSOUT;
197
198			/*! \brief This method write a graph description on the file named
199			FileName. */
200			virtual std::ostream &writeGraph(std::ostream &os) const;
201
202		135	virtual SignalTimeDependent<dynamicgraph::Vector, int> &getErrorDot() {
203		135	return errordotSOUT;
204			}
205
206			/! @} /
207
208			/* --- REFERENCE VALUE S* ------------------------------------------------- */
209			public:
210			/*! \name Reference
211			@{
212			*/
213			virtual void setReference(FeatureAbstract *sdes) = 0;
214			virtual void unsetReference(void) { setReference(NULL); }
215			virtual const FeatureAbstract *getReferenceAbstract(void) const = 0;
216			virtual FeatureAbstract *getReferenceAbstract(void) = 0;
217			virtual bool isReferenceSet(void) const { return false; }
218
219			virtual void addDependenciesFromReference(void) = 0;
220			virtual void removeDependenciesFromReference(void) = 0;
221
222			/* Commands for bindings. */
223			void setReferenceByName(const std::string &name);
224			std::string getReferenceByName(void) const;
225			/! @} /
226			};
227
228			template <class FeatureSpecialized>
229			class FeatureReferenceHelper {
230			FeatureSpecialized *ptr;
231			FeatureAbstract *ptrA;
232
233			public:
234		8	FeatureReferenceHelper(void) : ptr(NULL) {}
235
236			void setReference(FeatureAbstract *sdes);
237			// void setReferenceByName( const std::string & name );
238			void unsetReference(void) { setReference(NULL); }
239		36	bool isReferenceSet(void) const { return ptr != NULL; }
240		48	FeatureSpecialized *getReference(void) { return ptr; }
241			const FeatureSpecialized *getReference(void) const { return ptr; }
242			};
243
244			template <class FeatureSpecialized>
245		4	void FeatureReferenceHelper<FeatureSpecialized>::setReference(
246			FeatureAbstract *sdes) {
247	✓✗	4	ptr = dynamic_cast<FeatureSpecialized *>(sdes);
248		4	ptrA = ptr;
249		4	}
250
251			#define DECLARE_REFERENCE_FUNCTIONS(FeatureSpecialized) \
252			typedef FeatureReferenceHelper<FeatureSpecialized> SP; \
253			virtual void setReference(FeatureAbstract *sdes) { \
254			if (sdes == NULL) { \
255			/* UNSET */ \
256			if (SP::isReferenceSet()) removeDependenciesFromReference(); \
257			SP::unsetReference(); \
258			} else { \
259			/* SET */ \
260			SP::setReference(sdes); \
261			if (SP::isReferenceSet()) addDependenciesFromReference(); \
262			} \
263			} \
264			virtual const FeatureAbstract *getReferenceAbstract(void) const { \
265			return SP::getReference(); \
266			} \
267			virtual FeatureAbstract *getReferenceAbstract(void) { \
268			return (FeatureAbstract *)SP::getReference(); \
269			} \
270			bool isReferenceSet(void) const { return SP::isReferenceSet(); } \
271			virtual void addDependenciesFromReference(void); \
272			virtual void removeDependenciesFromReference(void)
273			/* END OF define DECLARE_REFERENCE_FUNCTIONS */
274
275			#define DECLARE_NO_REFERENCE \
276			virtual void setReference(FeatureAbstract *) {} \
277			virtual const FeatureAbstract *getReferenceAbstract(void) const { \
278			return NULL; \
279			} \
280			virtual FeatureAbstract *getReferenceAbstract(void) { return NULL; } \
281			virtual void addDependenciesFromReference(void) {} \
282			virtual void removeDependenciesFromReference(void) {} \
283			/* To force a ; */ bool NO_REFERENCE
284			/* END OF define DECLARE_REFERENCE_FUNCTIONS */
285
286			} // namespace sot
287			} // namespace dynamicgraph
288
289			#endif // #ifndef __SOT_FEATURE_ABSTRACT_H__