Directory: | ./ |
---|---|
File: | include/sot/core/integrator-euler.hh |
Date: | 2024-08-13 12:13:25 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 18 | 67 | 26.9% |
Branches: | 27 | 133 | 20.3% |
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 |
6/12✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
|
2 | derivativeSOUT(boost::bind(&IntegratorEuler<sigT, coefT>::derivative, |
67 | this, _1, _2), | ||
68 | SOUT, | ||
69 | "sotIntegratorEuler(" + name + | ||
70 | 2 | ")::output(vector)::derivativesout") { | |
71 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | this->signalRegistration(derivativeSOUT); |
72 | |||
73 | using namespace dynamicgraph::command; | ||
74 | |||
75 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
2 | setSamplingPeriod(0.005); |
76 | |||
77 |
3/6✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
|
4 | this->addCommand("setSamplingPeriod", |
78 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | new Setter<IntegratorEuler, double>( |
79 | *this, &IntegratorEuler::setSamplingPeriod, | ||
80 | "Set the time during two sampling.")); | ||
81 |
3/6✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
|
4 | this->addCommand("getSamplingPeriod", |
82 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | new Getter<IntegratorEuler, double>( |
83 | *this, &IntegratorEuler::getSamplingPeriod, | ||
84 | "Get the time during two sampling.")); | ||
85 | |||
86 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | this->addCommand( |
87 | "initialize", | ||
88 |
3/6✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
|
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/6✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
|
2 | if (denominator.empty() || numerator.empty()) |
173 |
1/2✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
4 | throw dynamicgraph::ExceptionSignal( |
174 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | dynamicgraph::ExceptionSignal::GENERIC, |
175 | "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 | ||
204 |