DynamicalSystem.hpp

Go to the documentation of this file.

#ifndef _DYNAMICALSYSTEM_H_
#define _DYNAMICALSYSTEM_H_

#include "eigen_realtime/eigen_realtime_check.hpp" // Include this before Eigen header files

#include <string>
#include <vector>
#include <eigen3/Eigen/Core>

#include "dmpbbo_io/EigenBoostSerialization.hpp"

namespace DmpBbo {

class DynamicalSystem
{

public:
  
   DynamicalSystem(int order, double tau, Eigen::VectorXd initial_state, Eigen::VectorXd attractor_state, std::string name);

  virtual ~DynamicalSystem(void);

  virtual DynamicalSystem* clone(void) const = 0;
  
   virtual void differentialEquation(
     const Eigen::Ref<const Eigen::VectorXd>& x, 
     Eigen::Ref<Eigen::VectorXd> xd
   ) const = 0;


  virtual void analyticalSolution(const Eigen::VectorXd& ts, Eigen::MatrixXd& xs, Eigen::MatrixXd& xds) const  = 0;

  virtual void integrateStart(Eigen::Ref<Eigen::VectorXd> x, Eigen::Ref<Eigen::VectorXd> xd) const;

  void integrateStart(const Eigen::VectorXd& x_init, Eigen::Ref<Eigen::VectorXd> x, Eigen::Ref<Eigen::VectorXd> xd);

  virtual void integrateStep(double dt, const Eigen::Ref<const Eigen::VectorXd> x, Eigen::Ref<Eigen::VectorXd> x_updated, Eigen::Ref<Eigen::VectorXd> xd_updated) const;

  friend std::ostream& operator<<(std::ostream& output, const DynamicalSystem& dyn_sys) {
    output << dyn_sys.toString();
    return output;
  }

  virtual std::string toString(void) const = 0;

  enum IntegrationMethod { EULER, RUNGE_KUTTA };

  inline void set_integration_method(IntegrationMethod integration_method) {
    integration_method_ = integration_method;
  }

  inline int dim(void) const {
    return dim_;
  }

   inline int dim_orig(void) const {
    return dim_orig_;
   }

  inline double tau(void) const { return tau_; }

  inline virtual void set_tau(double tau) {
    assert(tau>0.0);
    tau_ = tau;
  }

  inline Eigen::VectorXd initial_state(void) const { return initial_state_; }

  inline void initial_state(Eigen::VectorXd& initial_state) const 
  { 
    initial_state=initial_state_;
  }
  
  inline virtual void set_initial_state(const Eigen::VectorXd& initial_state) {
    assert(initial_state.size()==dim_orig_);
    initial_state_ = initial_state;
  }

  inline Eigen::VectorXd attractor_state(void) const { return attractor_state_; }

  inline void attractor_state(Eigen::VectorXd& attractor_state) const 
  { 
    attractor_state=attractor_state_;
  }

  inline virtual void set_attractor_state(const Eigen::Ref<const Eigen::VectorXd>& attractor_state) {
    assert(attractor_state.size()==dim_orig_);
    attractor_state_ = attractor_state;
  }

  inline std::string name(void) const { return name_; }

  inline virtual void set_name(std::string name) {
    name_ = name;
  }
  
protected:
  inline void set_dim(int dim) {
    dim_ = dim;
  }

private:

  void integrateStepEuler(double dt, const Eigen::Ref<const Eigen::VectorXd> x, Eigen::Ref<Eigen::VectorXd> x_updated, Eigen::Ref<Eigen::VectorXd> xd_updated) const;

  void integrateStepRungeKutta(double dt, const Eigen::Ref<const Eigen::VectorXd> x, Eigen::Ref<Eigen::VectorXd> x_updated, Eigen::Ref<Eigen::VectorXd> xd_updated) const;

  int dim_;

 int dim_orig_;

  double tau_;

  Eigen::VectorXd initial_state_;

  Eigen::VectorXd attractor_state_;

  std::string name_;

  IntegrationMethod integration_method_;
  

protected:
  DynamicalSystem(void) {};
   
private:
  friend class boost::serialization::access;
  
  template<class Archive>
  void serialize(Archive & ar, const unsigned int version)
  {
    ar & BOOST_SERIALIZATION_NVP(dim_);
    ar & BOOST_SERIALIZATION_NVP(dim_orig_);
    // Const doesn't work, see sec_boost_serialization_ugliness in the docu
    //ar & boost::serialization::make_nvp("dim_orig_", const_cast<int&>(dim_orig_));
    ar & BOOST_SERIALIZATION_NVP(tau_);
    ar & BOOST_SERIALIZATION_NVP(initial_state_);
    ar & BOOST_SERIALIZATION_NVP(attractor_state_);
    ar & BOOST_SERIALIZATION_NVP(name_);
    ar & BOOST_SERIALIZATION_NVP(integration_method_);
  }

};

}

#include <boost/serialization/assume_abstract.hpp>
BOOST_SERIALIZATION_ASSUME_ABSTRACT(DmpBbo::DynamicalSystem);
 
#include <boost/serialization/export.hpp>
BOOST_CLASS_IMPLEMENTATION(DmpBbo::DynamicalSystem,boost::serialization::object_serializable);

#endif // _DYNAMICALSYSTEM_H_