integrator-abstract.hh

Go to the documentation of this file.

/*
 * Copyright 2010,
 * François Bleibel,
 * Olivier Stasse,
 *
 * CNRS/AIST
 *
 */

#ifndef __SOT_INTEGRATOR_ABSTRACT_H__
#define __SOT_INTEGRATOR_ABSTRACT_H__

/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */

/* Matrix */
#include <dynamic-graph/linear-algebra.h>

/* SOT */
#include <dynamic-graph/all-signals.h>
#include <dynamic-graph/command-bind.h>
#include <dynamic-graph/entity.h>
#include <dynamic-graph/pool.h>

#include <sot/core/debug.hh>
#include <sot/core/flags.hh>

/* STD */
#include <string>

/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */

namespace dynamicgraph {
namespace sot {

template <class sigT, class coefT>
class IntegratorAbstract : public dynamicgraph::Entity {
 public:
  IntegratorAbstract(const std::string &name)
      : dynamicgraph::Entity(name),
        SIN(NULL, "sotIntegratorAbstract(" + name + ")::input(vector)::sin"),
        SOUT(boost::bind(&IntegratorAbstract<sigT, coefT>::integrate, this, _1,
                         _2),
             SIN, "sotIntegratorAbstract(" + name + ")::output(vector)::sout") {
    signalRegistration(SIN << SOUT);

    using namespace dynamicgraph::command;

    const std::string typeName =
        Value::typeName(dynamicgraph::command::ValueHelper<coefT>::TypeID);

    addCommand(
        "pushNumCoef",
        makeCommandVoid1(
            *this, &IntegratorAbstract::pushNumCoef,
            docCommandVoid1("Push a new numerator coefficient", typeName)));
    addCommand(
        "pushDenomCoef",
        makeCommandVoid1(
            *this, &IntegratorAbstract::pushDenomCoef,
            docCommandVoid1("Push a new denominator coefficient", typeName)));

    addCommand(
        "popNumCoef",
        makeCommandVoid0(*this, &IntegratorAbstract::popNumCoef,
                         docCommandVoid0("Pop a new numerator coefficient")));
    addCommand(
        "popDenomCoef",
        makeCommandVoid0(*this, &IntegratorAbstract::popDenomCoef,
                         docCommandVoid0("Pop a new denominator coefficient")));
  }

  virtual ~IntegratorAbstract() {}

  virtual sigT &integrate(sigT &res, int time) = 0;

 public:
  void pushNumCoef(const coefT &numCoef) { numerator.push_back(numCoef); }
  void pushDenomCoef(const coefT &denomCoef) {
    denominator.push_back(denomCoef);
  }
  void popNumCoef() { numerator.pop_back(); }
  void popDenomCoef() { denominator.pop_back(); }

  const std::vector<coefT> &numCoeffs() const { return numerator; }
  void numCoeffs(const std::vector<coefT> &coeffs) { numerator = coeffs; }

  const std::vector<coefT> &denomCoeffs() const { return denominator; }
  void denomCoeffs(const std::vector<coefT> &coeffs) { denominator = coeffs; }

 public:
  dynamicgraph::SignalPtr<sigT, int> SIN;

  dynamicgraph::SignalTimeDependent<sigT, int> SOUT;

  virtual void display(std::ostream &os) const {
    os << this->getClassName() << ": " << getName() << '\n' << "  ";
    if (numerator.empty() || denominator.empty()) {
      os << "ill-formed.";
      return;
    }
    os << numerator[0];
    for (std::size_t i = 1; i < numerator.size(); ++i)
      os << " + " << numerator[i] << " s^" << i;
    os << "\n  " << denominator[0];
    for (std::size_t i = 1; i < denominator.size(); ++i)
      os << " + " << denominator[i] << " s^" << i;
  }

 protected:
  std::vector<coefT> numerator;
  std::vector<coefT> denominator;
};

} /* namespace sot */
} /* namespace dynamicgraph */

#endif