GCC Code Coverage Report

Directory: ./
File: include/sot/core/feature-abstract.hh
Date: 2024-12-13 12:22:33
Exec Total Coverage
Lines: 10 15 66.7%
Branches: 1 2 50.0%
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
1/2
✓ Branch 0 taken 4 times.
✗ Branch 1 not taken.
 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__
290