integrator-euler.hh

Go to the documentation of this file.

/*
 * Copyright 2010,
 * François Bleibel,
 * Olivier Stasse,
 *
 * CNRS/AIST
 *
 */
 
#ifndef __SOT_INTEGRATOR_EULER_H__
#define __SOT_INTEGRATOR_EULER_H__
 
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
/* SOT */
#include <dynamic-graph/command-getter.h>
#include <dynamic-graph/command-setter.h>
 
#include <sot/core/integrator-abstract.hh>
 
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
 
namespace dynamicgraph {
namespace sot {
 
namespace internal {
template <class coefT>
bool integratorEulerCoeffIsIdentity(const coefT c) {
  return c == 1;
}
 
bool integratorEulerCoeffIsIdentity(const Vector c) { return c.isOnes(); }
 
bool integratorEulerCoeffIsIdentity(const Matrix c) { return c.isIdentity(); }
}  // namespace internal
 
template <class sigT, class coefT>
class IntegratorEuler : public IntegratorAbstract<sigT, coefT> {
 public:
  virtual const std::string &getClassName(void) const;
  static std::string getTypeName(void) { return "Unknown"; }
  static const std::string CLASS_NAME;
 
 public:
  using IntegratorAbstract<sigT, coefT>::SIN;
  using IntegratorAbstract<sigT, coefT>::SOUT;
  using IntegratorAbstract<sigT, coefT>::numerator;
  using IntegratorAbstract<sigT, coefT>::denominator;
 
 public:
  IntegratorEuler(const std::string &name)
      : IntegratorAbstract<sigT, coefT>(name),
        derivativeSOUT(boost::bind(&IntegratorEuler<sigT, coefT>::derivative,
                                   this, _1, _2),
                       SOUT,
                       "sotIntegratorEuler(" + name +
                           ")::output(vector)::derivativesout") {
    this->signalRegistration(derivativeSOUT);
 
    using namespace dynamicgraph::command;
 
    setSamplingPeriod(0.005);
 
    this->addCommand("setSamplingPeriod",
                     new Setter<IntegratorEuler, double>(
                         *this, &IntegratorEuler::setSamplingPeriod,
                         "Set the time during two sampling."));
    this->addCommand("getSamplingPeriod",
                     new Getter<IntegratorEuler, double>(
                         *this, &IntegratorEuler::getSamplingPeriod,
                         "Get the time during two sampling."));
 
    this->addCommand(
        "initialize",
        makeCommandVoid0(
            *this, &IntegratorEuler::initialize,
            docCommandVoid0(
                "Initialize internal memory from current value of input")));
  }
 
  virtual ~IntegratorEuler(void) {}
 
 protected:
  std::vector<sigT> inputMemory;
  std::vector<sigT> outputMemory;
 
  dynamicgraph::SignalTimeDependent<sigT, sigtime_t> derivativeSOUT;
 
  double dt;
  double invdt;
 
 public:
  sigT &integrate(sigT &res, sigtime_t time) {
    sotDEBUG(15) << "# In {" << std::endl;
 
    sigT sum;
    sigT tmp1, tmp2;
    const std::vector<coefT> &num = numerator;
    const std::vector<coefT> &denom = denominator;
 
    // Step 1
    tmp1 = inputMemory[0];
    inputMemory[0] = SIN.access(time);
    sum = num[0] * inputMemory[0];
    // End of step 1. Here, sum is b_0 X
 
    // Step 2
    size_type numsize = (size_type)num.size();
    for (size_type i = 1; i < numsize; ++i) {
      tmp2 = inputMemory[i - 1] - tmp1;
      tmp2 *= invdt;
      tmp1 = inputMemory[i];
      inputMemory[i] = tmp2;
      sum += (num[i] * inputMemory[i]);
    }
    // End of step 2. Here, sum is b_m * d(m)X / dt^m + ... + b_0 X
 
    // Step 3
    size_type denomsize = (size_type)denom.size() - 1;
    for (size_type i = 0; i < denomsize; ++i) {
      sum -= (denom[i] * outputMemory[i]);
    }
    // End of step 3. Here, sum is b_m * d(m)X / dt^m + ... + b_0 X
    //                           - a_0 Y - ... a_n-1 d(n-1)Y / dt^(n-1)
 
    // Step 4
    outputMemory[denomsize] = sum;
    for (size_type i = denomsize - 1; i >= 0; --i) {
      outputMemory[i] += (outputMemory[i + 1] * dt);
    }
    // End of step 4. The ODE is integrated
 
    inputMemory[0] = SIN.access(time);
    res = outputMemory[0];
 
    sotDEBUG(15) << "# Out }" << std::endl;
    return res;
  }
 
  sigT &derivative(sigT &res, sigtime_t time) {
    if (outputMemory.size() < 2)
      throw dynamicgraph::ExceptionSignal(
          dynamicgraph::ExceptionSignal::GENERIC,
          "Integrator does not compute the derivative.");
 
    SOUT.recompute(time);
    res = outputMemory[1];
    return res;
  }
 
  void setSamplingPeriod(const double &period) {
    dt = period;
    invdt = 1 / period;
  }
 
  double getSamplingPeriod() const { return dt; }
 
  void initialize() {
    if (denominator.empty() || numerator.empty())
      throw dynamicgraph::ExceptionSignal(
          dynamicgraph::ExceptionSignal::GENERIC,
          "The numerator or the denominator is empty.");
 
    // Check that denominator.back is the identity
    if (!internal::integratorEulerCoeffIsIdentity(denominator.back()))
      throw dynamicgraph::ExceptionSignal(
          dynamicgraph::ExceptionSignal::GENERIC,
          "The coefficient of the highest order derivative of denominator "
          "should be 1 (the last pushDenomCoef should be the identity).");
 
    std::size_t numsize = numerator.size();
    inputMemory.resize(numsize);
 
    inputMemory[0] = SIN.accessCopy();
    for (std::size_t i = 1; i < numsize; ++i) {
      inputMemory[i] = inputMemory[0];
    }
 
    std::size_t denomsize = denominator.size();
    outputMemory.resize(denomsize);
    for (std::size_t i = 0; i < denomsize; ++i) {
      outputMemory[i] = inputMemory[0];
    }
  }
};
 
} /* namespace sot */
} /* namespace dynamicgraph */
 
#endif