GCC Code Coverage Report

Directory: ./
File: include/sot/core/integrator-euler.hh
Date: 2024-11-13 12:35:17
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