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