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 |
|
|
|
24 |
|
|
#include <sot/core/flags.hh> |
25 |
|
|
#include <sot/core/matrix-geometry.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 |
|
|
|
38 |
|
|
template <class T> |
39 |
|
|
class Derivator : public dynamicgraph::Entity { |
40 |
|
|
DYNAMIC_GRAPH_ENTITY_DECL(); |
41 |
|
|
|
42 |
|
|
protected: |
43 |
|
|
T memory; |
44 |
|
|
bool initialized; |
45 |
|
|
double timestep; |
46 |
|
|
static const double TIMESTEP_DEFAULT; //= 1.; |
47 |
|
|
|
48 |
|
|
public: /* --- CONSTRUCTION --- */ |
49 |
|
|
static std::string getTypeName(void) { return "Unknown"; } |
50 |
|
|
|
51 |
|
|
Derivator(const std::string &name) |
52 |
|
|
: dynamicgraph::Entity(name), |
53 |
|
|
memory(), |
54 |
|
|
initialized(false), |
55 |
|
|
timestep(TIMESTEP_DEFAULT), |
56 |
|
|
SIN(NULL, "sotDerivator<" + getTypeName() + ">(" + name + ")::input(" + |
57 |
|
|
getTypeName() + ")::sin"), |
58 |
|
|
SOUT(boost::bind(&Derivator<T>::computeDerivation, this, _1, _2), SIN, |
59 |
|
|
"sotDerivator<" + getTypeName() + ">(" + name + ")::output(" + |
60 |
|
|
getTypeName() + ")::sout"), |
61 |
|
|
timestepSIN("sotDerivator<" + getTypeName() + ">(" + name + |
62 |
|
|
")::input(double)::dt") { |
63 |
|
|
signalRegistration(SIN << SOUT << timestepSIN); |
64 |
|
|
timestepSIN.setReferenceNonConstant(×tep); |
65 |
|
|
timestepSIN.setKeepReference(true); |
66 |
|
|
} |
67 |
|
|
|
68 |
|
|
virtual ~Derivator(void){}; |
69 |
|
|
|
70 |
|
|
public: /* --- SIGNAL --- */ |
71 |
|
|
dynamicgraph::SignalPtr<T, int> SIN; |
72 |
|
|
dynamicgraph::SignalTimeDependent<T, int> SOUT; |
73 |
|
|
dynamicgraph::Signal<double, int> timestepSIN; |
74 |
|
|
|
75 |
|
|
protected: |
76 |
|
|
T &computeDerivation(T &res, int time) { |
77 |
|
|
if (initialized) { |
78 |
|
|
res = memory; |
79 |
|
|
res *= -1; |
80 |
|
|
memory = SIN(time); |
81 |
|
|
res += memory; |
82 |
|
|
if (timestep != 1.) res *= (1. / timestep); |
83 |
|
|
} else { |
84 |
|
|
initialized = true; |
85 |
|
|
memory = SIN(time); |
86 |
|
|
res = memory; |
87 |
|
|
res *= 0; |
88 |
|
|
} |
89 |
|
|
return res; |
90 |
|
|
} |
91 |
|
|
}; |
92 |
|
|
// TODO Derivation of unit quaternion? |
93 |
|
|
template <> |
94 |
|
|
VectorQuaternion &Derivator<VectorQuaternion>::computeDerivation( |
95 |
|
|
VectorQuaternion &res, int time) { |
96 |
|
|
if (initialized) { |
97 |
|
|
res = memory; |
98 |
|
|
res.coeffs() *= -1; |
99 |
|
|
memory = SIN(time); |
100 |
|
|
res.coeffs() += memory.coeffs(); |
101 |
|
|
if (timestep != 1.) 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__ |