sot-core  4.11.2
Hierarchical task solver plug-in for dynamic-graph.
derivator.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_DERIVATOR_H__
11 #define __SOT_DERIVATOR_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 #include <sot/core/flags.hh>
25 #include <sot/core/pool.hh>
26 
27 /* STD */
28 #include <string>
29 
30 namespace dynamicgraph {
31 namespace sot {
32 
33 /* --------------------------------------------------------------------- */
34 /* --- CLASS ----------------------------------------------------------- */
35 /* --------------------------------------------------------------------- */
36 
37 template <class T> class Derivator : public dynamicgraph::Entity {
38  DYNAMIC_GRAPH_ENTITY_DECL();
39 
40 protected:
41  T memory;
43  double timestep;
44  static const double TIMESTEP_DEFAULT; //= 1.;
45 
46 public: /* --- CONSTRUCTION --- */
47  static std::string getTypeName(void) { return "Unknown"; }
48 
49  Derivator(const std::string &name)
50  : dynamicgraph::Entity(name), memory(), initialized(false),
51  timestep(TIMESTEP_DEFAULT),
52  SIN(NULL, "sotDerivator<" + getTypeName() + ">(" + name + ")::input(" +
53  getTypeName() + ")::sin"),
54  SOUT(boost::bind(&Derivator<T>::computeDerivation, this, _1, _2), SIN,
55  "sotDerivator<" + getTypeName() + ">(" + name + ")::output(" +
56  getTypeName() + ")::sout"),
57  timestepSIN("sotDerivator<" + getTypeName() + ">(" + name +
58  ")::input(double)::dt") {
59  signalRegistration(SIN << SOUT << timestepSIN);
60  timestepSIN.setReferenceNonConstant(&timestep);
61  timestepSIN.setKeepReference(true);
62  }
63 
64  virtual ~Derivator(void){};
65 
66 public: /* --- SIGNAL --- */
67  dynamicgraph::SignalPtr<T, int> SIN;
68  dynamicgraph::SignalTimeDependent<T, int> SOUT;
69  dynamicgraph::Signal<double, int> timestepSIN;
70 
71 protected:
72  T &computeDerivation(T &res, int time) {
73  if (initialized) {
74  res = memory;
75  res *= -1;
76  memory = SIN(time);
77  res += memory;
78  if (timestep != 1.)
79  res *= (1. / timestep);
80  } else {
81  initialized = true;
82  memory = SIN(time);
83  res = memory;
84  res *= 0;
85  }
86  return res;
87  }
88 };
89 // TODO Derivation of unit quaternion?
90 template <>
93  int time) {
94  if (initialized) {
95  res = memory;
96  res.coeffs() *= -1;
97  memory = SIN(time);
98  res.coeffs() += memory.coeffs();
99  if (timestep != 1.)
100  res.coeffs() *= (1. / timestep);
101  } else {
102  initialized = true;
103  memory = SIN(time);
104  res = memory;
105  res.coeffs() *= 0;
106  }
107  return res;
108 }
109 
110 } /* namespace sot */
111 } /* namespace dynamicgraph */
112 
113 #endif // #ifndef __SOT_DERIVATOR_H__
Eigen::Quaternion< double > SOT_CORE_EXPORT VectorQuaternion
Definition: matrix-geometry.hh:77
dynamicgraph::SignalPtr< T, int > SIN
Definition: derivator.hh:64
T memory
Definition: derivator.hh:41
static std::string getTypeName(void)
Definition: derivator.hh:47
Definition: derivator.hh:37
Derivator(const std::string &name)
Definition: derivator.hh:49
bool initialized
Definition: derivator.hh:42
static const double TIMESTEP_DEFAULT
Definition: derivator.hh:44
dynamicgraph::Signal< double, int > timestepSIN
Definition: derivator.hh:69
virtual ~Derivator(void)
Definition: derivator.hh:64
double timestep
Definition: derivator.hh:43
dynamicgraph::SignalTimeDependent< T, int > SOUT
Definition: derivator.hh:68
Definition: abstract-sot-external-interface.hh:17
T & computeDerivation(T &res, int time)
Definition: derivator.hh:72