GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
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_INTEGRATOR_EULER_H__ |
||
11 |
#define __SOT_INTEGRATOR_EULER_H__ |
||
12 |
|||
13 |
/* --------------------------------------------------------------------- */ |
||
14 |
/* --- INCLUDE --------------------------------------------------------- */ |
||
15 |
/* --------------------------------------------------------------------- */ |
||
16 |
|||
17 |
/* SOT */ |
||
18 |
#include <dynamic-graph/command-getter.h> |
||
19 |
#include <dynamic-graph/command-setter.h> |
||
20 |
|||
21 |
#include <sot/core/integrator-abstract.hh> |
||
22 |
|||
23 |
/* --------------------------------------------------------------------- */ |
||
24 |
/* --- CLASS ----------------------------------------------------------- */ |
||
25 |
/* --------------------------------------------------------------------- */ |
||
26 |
|||
27 |
namespace dynamicgraph { |
||
28 |
namespace sot { |
||
29 |
|||
30 |
namespace internal { |
||
31 |
template <class coefT> |
||
32 |
bool integratorEulerCoeffIsIdentity(const coefT c) { |
||
33 |
return c == 1; |
||
34 |
} |
||
35 |
|||
36 |
bool integratorEulerCoeffIsIdentity(const Vector c) { return c.isOnes(); } |
||
37 |
|||
38 |
bool integratorEulerCoeffIsIdentity(const Matrix c) { return c.isIdentity(); } |
||
39 |
} // namespace internal |
||
40 |
|||
41 |
/*! |
||
42 |
* \class IntegratorEuler |
||
43 |
* \brief integrates an ODE using a naive Euler integration. |
||
44 |
* TODO: change the integration method. For the moment, the highest |
||
45 |
* derivative of the output signal is computed using the |
||
46 |
* previous values of the other derivatives and the input |
||
47 |
* signal, then integrated n times, which will most certainly |
||
48 |
* induce a huge drift for ODEs with a high order at the denominator. |
||
49 |
*/ |
||
50 |
template <class sigT, class coefT> |
||
51 |
class IntegratorEuler : public IntegratorAbstract<sigT, coefT> { |
||
52 |
public: |
||
53 |
virtual const std::string &getClassName(void) const; |
||
54 |
static std::string getTypeName(void) { return "Unknown"; } |
||
55 |
static const std::string CLASS_NAME; |
||
56 |
|||
57 |
public: |
||
58 |
using IntegratorAbstract<sigT, coefT>::SIN; |
||
59 |
using IntegratorAbstract<sigT, coefT>::SOUT; |
||
60 |
using IntegratorAbstract<sigT, coefT>::numerator; |
||
61 |
using IntegratorAbstract<sigT, coefT>::denominator; |
||
62 |
|||
63 |
public: |
||
64 |
2 |
IntegratorEuler(const std::string &name) |
|
65 |
: IntegratorAbstract<sigT, coefT>(name), |
||
66 |
derivativeSOUT(boost::bind(&IntegratorEuler<sigT, coefT>::derivative, |
||
67 |
this, _1, _2), |
||
68 |
SOUT, |
||
69 |
"sotIntegratorEuler(" + name + |
||
70 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
2 |
")::output(vector)::derivativesout") { |
71 |
✓✗✓✗ |
2 |
this->signalRegistration(derivativeSOUT); |
72 |
|||
73 |
using namespace dynamicgraph::command; |
||
74 |
|||
75 |
✗✗ | 2 |
setSamplingPeriod(0.005); |
76 |
|||
77 |
✓✗✓✗ ✓✗ |
4 |
this->addCommand("setSamplingPeriod", |
78 |
✓✗✓✗ |
2 |
new Setter<IntegratorEuler, double>( |
79 |
*this, &IntegratorEuler::setSamplingPeriod, |
||
80 |
"Set the time during two sampling.")); |
||
81 |
✓✗✓✗ ✓✗ |
4 |
this->addCommand("getSamplingPeriod", |
82 |
✓✗✓✗ |
2 |
new Getter<IntegratorEuler, double>( |
83 |
*this, &IntegratorEuler::getSamplingPeriod, |
||
84 |
"Get the time during two sampling.")); |
||
85 |
|||
86 |
✓✗✓✗ |
2 |
this->addCommand( |
87 |
"initialize", |
||
88 |
✓✗✓✗ ✓✗ |
4 |
makeCommandVoid0( |
89 |
*this, &IntegratorEuler::initialize, |
||
90 |
docCommandVoid0( |
||
91 |
"Initialize internal memory from current value of input"))); |
||
92 |
} |
||
93 |
|||
94 |
virtual ~IntegratorEuler(void) {} |
||
95 |
|||
96 |
protected: |
||
97 |
std::vector<sigT> inputMemory; |
||
98 |
std::vector<sigT> outputMemory; |
||
99 |
|||
100 |
dynamicgraph::SignalTimeDependent<sigT, int> derivativeSOUT; |
||
101 |
|||
102 |
double dt; |
||
103 |
double invdt; |
||
104 |
|||
105 |
public: |
||
106 |
sigT &integrate(sigT &res, int time) { |
||
107 |
sotDEBUG(15) << "# In {" << std::endl; |
||
108 |
|||
109 |
sigT sum; |
||
110 |
sigT tmp1, tmp2; |
||
111 |
const std::vector<coefT> &num = numerator; |
||
112 |
const std::vector<coefT> &denom = denominator; |
||
113 |
|||
114 |
// Step 1 |
||
115 |
tmp1 = inputMemory[0]; |
||
116 |
inputMemory[0] = SIN.access(time); |
||
117 |
sum = num[0] * inputMemory[0]; |
||
118 |
// End of step 1. Here, sum is b_0 X |
||
119 |
|||
120 |
// Step 2 |
||
121 |
int numsize = (int)num.size(); |
||
122 |
for (int i = 1; i < numsize; ++i) { |
||
123 |
tmp2 = inputMemory[i - 1] - tmp1; |
||
124 |
tmp2 *= invdt; |
||
125 |
tmp1 = inputMemory[i]; |
||
126 |
inputMemory[i] = tmp2; |
||
127 |
sum += (num[i] * inputMemory[i]); |
||
128 |
} |
||
129 |
// End of step 2. Here, sum is b_m * d(m)X / dt^m + ... + b_0 X |
||
130 |
|||
131 |
// Step 3 |
||
132 |
int denomsize = (int)denom.size() - 1; |
||
133 |
for (int i = 0; i < denomsize; ++i) { |
||
134 |
sum -= (denom[i] * outputMemory[i]); |
||
135 |
} |
||
136 |
// End of step 3. Here, sum is b_m * d(m)X / dt^m + ... + b_0 X |
||
137 |
// - a_0 Y - ... a_n-1 d(n-1)Y / dt^(n-1) |
||
138 |
|||
139 |
// Step 4 |
||
140 |
outputMemory[denomsize] = sum; |
||
141 |
for (int i = denomsize - 1; i >= 0; --i) { |
||
142 |
outputMemory[i] += (outputMemory[i + 1] * dt); |
||
143 |
} |
||
144 |
// End of step 4. The ODE is integrated |
||
145 |
|||
146 |
inputMemory[0] = SIN.access(time); |
||
147 |
res = outputMemory[0]; |
||
148 |
|||
149 |
sotDEBUG(15) << "# Out }" << std::endl; |
||
150 |
return res; |
||
151 |
} |
||
152 |
|||
153 |
sigT &derivative(sigT &res, int time) { |
||
154 |
if (outputMemory.size() < 2) |
||
155 |
throw dynamicgraph::ExceptionSignal( |
||
156 |
dynamicgraph::ExceptionSignal::GENERIC, |
||
157 |
"Integrator does not compute the derivative."); |
||
158 |
|||
159 |
SOUT.recompute(time); |
||
160 |
res = outputMemory[1]; |
||
161 |
return res; |
||
162 |
} |
||
163 |
|||
164 |
2 |
void setSamplingPeriod(const double &period) { |
|
165 |
2 |
dt = period; |
|
166 |
2 |
invdt = 1 / period; |
|
167 |
} |
||
168 |
|||
169 |
double getSamplingPeriod() const { return dt; } |
||
170 |
|||
171 |
2 |
void initialize() { |
|
172 |
✗✓✗✗ ✓✗ |
2 |
if (denominator.empty() || numerator.empty()) |
173 |
throw dynamicgraph::ExceptionSignal( |
||
174 |
dynamicgraph::ExceptionSignal::GENERIC, |
||
175 |
✓✗✓✗ |
2 |
"The numerator or the denominator is empty."); |
176 |
|||
177 |
// Check that denominator.back is the identity |
||
178 |
if (!internal::integratorEulerCoeffIsIdentity(denominator.back())) |
||
179 |
throw dynamicgraph::ExceptionSignal( |
||
180 |
dynamicgraph::ExceptionSignal::GENERIC, |
||
181 |
"The coefficient of the highest order derivative of denominator " |
||
182 |
"should be 1 (the last pushDenomCoef should be the identity)."); |
||
183 |
|||
184 |
std::size_t numsize = numerator.size(); |
||
185 |
inputMemory.resize(numsize); |
||
186 |
|||
187 |
inputMemory[0] = SIN.accessCopy(); |
||
188 |
for (std::size_t i = 1; i < numsize; ++i) { |
||
189 |
inputMemory[i] = inputMemory[0]; |
||
190 |
} |
||
191 |
|||
192 |
std::size_t denomsize = denominator.size(); |
||
193 |
outputMemory.resize(denomsize); |
||
194 |
for (std::size_t i = 0; i < denomsize; ++i) { |
||
195 |
outputMemory[i] = inputMemory[0]; |
||
196 |
} |
||
197 |
} |
||
198 |
}; |
||
199 |
|||
200 |
} /* namespace sot */ |
||
201 |
} /* namespace dynamicgraph */ |
||
202 |
|||
203 |
#endif |
Generated by: GCOVR (Version 4.2) |