GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/sot/core/integrator-euler.hh Lines: 16 63 25.4 %
Date: 2023-03-13 12:09:37 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
        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