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