sot-core  4.10.1
Hierarchical task solver plug-in for dynamic-graph.
feature-abstract.hh
Go to the documentation of this file.
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 "sot/core/api.hh"
22 #include "sot/core/deprecated.hh"
23 #include <dynamic-graph/all-signals.h>
24 #include <dynamic-graph/entity.h>
25 #include <sot/core/flags.hh>
26 #include <sot/core/pool.hh>
27 
28 /* STD */
29 #include <string>
30 
31 namespace dynamicgraph {
32 namespace sot {
33 
34 /* --------------------------------------------------------------------- */
35 /* --- CLASS ----------------------------------------------------------- */
36 /* --------------------------------------------------------------------- */
37 
75 class SOT_CORE_EXPORT FeatureAbstract : public Entity {
76 public:
78  static const std::string CLASS_NAME;
79 
81  virtual const std::string &getClassName(void) const { return CLASS_NAME; }
82 
84  void featureRegistration(void);
85 
86  void initCommands(void);
87 
88 public:
90  FeatureAbstract(const std::string &name);
92  virtual ~FeatureAbstract(void){};
93 
103  virtual unsigned int &getDimension(unsigned int &res, int time) = 0;
104 
110  inline unsigned int getDimension(int time) {
111  unsigned int res;
112  getDimension(res, time);
113  return res;
114  }
115 
120  inline unsigned int getDimension(void) const {
121  return dimensionSOUT.accessCopy();
122  }
139  virtual dynamicgraph::Vector &computeError(dynamicgraph::Vector &res,
140  int time) = 0;
141 
147  virtual dynamicgraph::Matrix &computeJacobian(dynamicgraph::Matrix &res,
148  int time) = 0;
149 
154  virtual dynamicgraph::Vector &computeErrorDot(dynamicgraph::Vector &res,
155  int time);
156 
159  /* --- SIGNALS ------------------------------------------------------------ */
160 public:
172  SignalPtr<Flags, int> selectionSIN;
173 
175  SignalPtr<dynamicgraph::Vector, int> errordotSIN;
176 
184  SignalTimeDependent<dynamicgraph::Vector, int> errorSOUT;
185 
188  SignalTimeDependent<dynamicgraph::Vector, int> errordotSOUT;
189 
192  SignalTimeDependent<dynamicgraph::Matrix, int> jacobianSOUT;
193 
195  SignalTimeDependent<unsigned int, int> dimensionSOUT;
196 
199  virtual std::ostream &writeGraph(std::ostream &os) const;
200 
201  virtual SignalTimeDependent<dynamicgraph::Vector, int> &getErrorDot() {
202  return errordotSOUT;
203  }
204 
207  /* --- REFERENCE VALUE S* ------------------------------------------------- */
208 public:
212  virtual void setReference(FeatureAbstract *sdes) = 0;
213  virtual void unsetReference(void) { setReference(NULL); }
214  virtual const FeatureAbstract *getReferenceAbstract(void) const = 0;
215  virtual FeatureAbstract *getReferenceAbstract(void) = 0;
216  virtual bool isReferenceSet(void) const { return false; }
217 
218  virtual void addDependenciesFromReference(void) = 0;
219  virtual void removeDependenciesFromReference(void) = 0;
220 
221  /* Commands for bindings. */
222  void setReferenceByName(const std::string &name);
223  std::string getReferenceByName(void) const;
225 };
226 
227 template <class FeatureSpecialized> class FeatureReferenceHelper {
228  FeatureSpecialized *ptr;
229  FeatureAbstract *ptrA;
230 
231 public:
232  FeatureReferenceHelper(void) : ptr(NULL) {}
233 
234  void setReference(FeatureAbstract *sdes);
235  // void setReferenceByName( const std::string & name );
236  void unsetReference(void) { setReference(NULL); }
237  bool isReferenceSet(void) const { return ptr != NULL; }
238  FeatureSpecialized *getReference(void) { return ptr; }
239  const FeatureSpecialized *getReference(void) const { return ptr; }
240 };
241 
242 template <class FeatureSpecialized>
244  FeatureAbstract *sdes) {
245  ptr = dynamic_cast<FeatureSpecialized *>(sdes);
246  ptrA = ptr;
247 }
248 
249 #define DECLARE_REFERENCE_FUNCTIONS(FeatureSpecialized) \
250  typedef FeatureReferenceHelper<FeatureSpecialized> SP; \
251  virtual void setReference(FeatureAbstract *sdes) { \
252  if (sdes == NULL) { \
253  /* UNSET */ \
254  if (SP::isReferenceSet()) \
255  removeDependenciesFromReference(); \
256  SP::unsetReference(); \
257  } else { \
258  /* SET */ \
259  SP::setReference(sdes); \
260  if (SP::isReferenceSet()) \
261  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__
SignalTimeDependent< dynamicgraph::Vector, int > errordotSOUT
Derivative of the error with respect to time: .
Definition: feature-abstract.hh:188
Definition: feature-abstract.hh:227
SignalPtr< Flags, int > selectionSIN
This vector specifies which dimension are used to perform the computation. For instance let us assume...
Definition: feature-abstract.hh:172
SignalTimeDependent< unsigned int, int > dimensionSOUT
Returns the dimension of the feature as an output signal.
Definition: feature-abstract.hh:195
unsigned int getDimension(int time)
Short method.
Definition: feature-abstract.hh:110
void unsetReference(void)
Definition: feature-abstract.hh:236
virtual bool isReferenceSet(void) const
Definition: feature-abstract.hh:216
#define SOT_CORE_EXPORT
Definition: api.hh:20
SignalTimeDependent< dynamicgraph::Matrix, int > jacobianSOUT
Jacobian of the error wrt the robot state: .
Definition: feature-abstract.hh:192
unsigned int getDimension(void) const
Shortest method.
Definition: feature-abstract.hh:120
virtual ~FeatureAbstract(void)
Default destructor.
Definition: feature-abstract.hh:92
SignalPtr< dynamicgraph::Vector, int > errordotSIN
Derivative of the reference value.
Definition: feature-abstract.hh:175
static const std::string CLASS_NAME
Store the name of the class.
Definition: feature-abstract.hh:78
void setReference(FeatureAbstract *sdes)
Definition: feature-abstract.hh:243
virtual const std::string & getClassName(void) const
Returns the name class.
Definition: feature-abstract.hh:81
const FeatureSpecialized * getReference(void) const
Definition: feature-abstract.hh:239
virtual SignalTimeDependent< dynamicgraph::Vector, int > & getErrorDot()
Definition: feature-abstract.hh:201
virtual void unsetReference(void)
Definition: feature-abstract.hh:213
This class gives the abstract definition of a feature.
Definition: feature-abstract.hh:75
bool isReferenceSet(void) const
Definition: feature-abstract.hh:237
FeatureSpecialized * getReference(void)
Definition: feature-abstract.hh:238
SignalTimeDependent< dynamicgraph::Vector, int > errorSOUT
This signal returns the error between the desired value and the current value : . ...
Definition: feature-abstract.hh:184
FeatureReferenceHelper(void)
Definition: feature-abstract.hh:232
Definition: abstract-sot-external-interface.hh:17