10 #ifndef __SOT_INTEGRATOR_EULER_H__ 11 #define __SOT_INTEGRATOR_EULER_H__ 18 #include <dynamic-graph/command-getter.h> 19 #include <dynamic-graph/command-setter.h> 31 template <
class coefT>
50 template <
class sigT,
class coefT>
53 virtual const std::string &getClassName(
void)
const;
69 "sotIntegratorEuler(" + name +
70 ")::output(vector)::derivativesout") {
71 this->signalRegistration(derivativeSOUT);
73 using namespace dynamicgraph::command;
75 setSamplingPeriod(0.005);
77 this->addCommand(
"setSamplingPeriod",
78 new Setter<IntegratorEuler, double>(
80 "Set the time during two sampling."));
81 this->addCommand(
"getSamplingPeriod",
82 new Getter<IntegratorEuler, double>(
84 "Get the time during two sampling."));
91 "Initialize internal memory from current value of input")));
107 sotDEBUG(15) <<
"# In {" << std::endl;
111 const std::vector<coefT> &num = numerator;
112 const std::vector<coefT> &denom = denominator;
115 tmp1 = inputMemory[0];
116 inputMemory[0] = SIN.access(time);
117 sum = num[0] * inputMemory[0];
121 int numsize = (int)num.size();
122 for (
int i = 1; i < numsize; ++i) {
123 tmp2 = inputMemory[i - 1] - tmp1;
125 tmp1 = inputMemory[i];
126 inputMemory[i] = tmp2;
127 sum += (num[i] * inputMemory[i]);
132 int denomsize = (int)denom.size() - 1;
133 for (
int i = 0; i < denomsize; ++i) {
134 sum -= (denom[i] * outputMemory[i]);
140 outputMemory[denomsize] = sum;
141 for (
int i = denomsize - 1; i >= 0; --i) {
142 outputMemory[i] += (outputMemory[i + 1] * dt);
146 inputMemory[0] = SIN.access(time);
147 res = outputMemory[0];
149 sotDEBUG(15) <<
"# Out }" << std::endl;
154 if (outputMemory.size() < 2)
155 throw dynamicgraph::ExceptionSignal(
156 dynamicgraph::ExceptionSignal::GENERIC,
157 "Integrator does not compute the derivative.");
159 SOUT.recompute(time);
160 res = outputMemory[1];
172 if (denominator.empty() || numerator.empty())
173 throw dynamicgraph::ExceptionSignal(
174 dynamicgraph::ExceptionSignal::GENERIC,
175 "The numerator or the denominator is empty.");
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).");
184 std::size_t numsize = numerator.size();
185 inputMemory.resize(numsize);
187 inputMemory[0] = SIN.accessCopy();
188 for (std::size_t i = 1; i < numsize; ++i) {
189 inputMemory[i] = inputMemory[0];
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];
bool integratorEulerCoeffIsIdentity(const coefT c)
Definition: integrator-euler.hh:32
double dt
Definition: integrator-euler.hh:102
void initialize()
Definition: integrator-euler.hh:171
double invdt
Definition: integrator-euler.hh:103
integrates an ODE. If Y is the output and X the input, the following equation is integrated: a_p * d(...
Definition: integrator-abstract.hh:46
sigT & derivative(sigT &res, int time)
Definition: integrator-euler.hh:153
static const std::string CLASS_NAME
Definition: integrator-euler.hh:55
static std::string getTypeName(void)
Definition: integrator-euler.hh:54
dynamicgraph::SignalTimeDependent< sigT, int > derivativeSOUT
Definition: integrator-euler.hh:100
#define sotDEBUG(level)
Definition: debug.hh:165
integrates an ODE using a naive Euler integration. TODO: change the integration method. For the moment, the highest derivative of the output signal is computed using the previous values of the other derivatives and the input signal, then integrated n times, which will most certainly induce a huge drift for ODEs with a high order at the denominator.
Definition: integrator-euler.hh:51
IntegratorEuler(const std::string &name)
Definition: integrator-euler.hh:64
double getSamplingPeriod() const
Definition: integrator-euler.hh:169
virtual ~IntegratorEuler(void)
Definition: integrator-euler.hh:94
void setSamplingPeriod(const double &period)
Definition: integrator-euler.hh:164
Definition: abstract-sot-external-interface.hh:17
std::vector< sigT > inputMemory
Definition: integrator-euler.hh:97
std::vector< sigT > outputMemory
Definition: integrator-euler.hh:98
sigT & integrate(sigT &res, int time)
Definition: integrator-euler.hh:106