Dmp.hpp

Go to the documentation of this file.

#ifndef _DMP_H_
#define _DMP_H_

// This must be included before any Eigen header files are included
#include "eigen_realtime/eigen_realtime_check.hpp"

#include "dynamicalsystems/DynamicalSystem.hpp"
#include "functionapproximators/Parameterizable.hpp"

#include "dmpbbo_io/EigenBoostSerialization.hpp"
#include <boost/serialization/assume_abstract.hpp>

#include <boost/random.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/normal_distribution.hpp>

#include <set>

namespace DmpBbo {
  
// forward declaration
class FunctionApproximator;
class SpringDamperSystem;
class Trajectory;

class Dmp : public DynamicalSystem, public Parameterizable
{
public:
  
  enum DmpType { IJSPEERT_2002_MOVEMENT, KULVICIUS_2012_JOINING, COUNTDOWN_2013  };

  enum ForcingTermScaling { NO_SCALING, G_MINUS_Y0_SCALING, AMPLITUDE_SCALING };
  
   Dmp(double tau, Eigen::VectorXd y_init, Eigen::VectorXd y_attr,
     std::vector<FunctionApproximator*> function_approximators,
     double alpha_spring_damper, DynamicalSystem* goal_system,
     DynamicalSystem* phase_system, DynamicalSystem* gating_system, 
     ForcingTermScaling scaling=NO_SCALING);
  
   Dmp(int n_dims_dmp, std::vector<FunctionApproximator*> function_approximators, 
     double alpha_spring_damper, DynamicalSystem* goal_system,
     DynamicalSystem* phase_system, DynamicalSystem* gating_system,
     ForcingTermScaling scaling=NO_SCALING);
    
  Dmp(double tau, Eigen::VectorXd y_init, Eigen::VectorXd y_attr, 
    std::vector<FunctionApproximator*> function_approximators, 
    DmpType dmp_type=KULVICIUS_2012_JOINING,   
    ForcingTermScaling scaling=NO_SCALING);

  
  Dmp(int n_dims_dmp, std::vector<FunctionApproximator*> function_approximators,
    DmpType dmp_type=KULVICIUS_2012_JOINING, ForcingTermScaling scaling=NO_SCALING);      
   
  Dmp(double tau, Eigen::VectorXd y_init, Eigen::VectorXd y_attr, double alpha_spring_damper, DynamicalSystem* goal_system);
  
  ~Dmp(void);
  
  Dmp* clone(void) const;

  
  virtual void integrateStart(Eigen::Ref<Eigen::VectorXd> x, Eigen::Ref<Eigen::VectorXd> xd) const;
  
  void differentialEquation(
   const Eigen::Ref<const Eigen::VectorXd>& x, 
   Eigen::Ref<Eigen::VectorXd> xd) const;
  
  void analyticalSolution(const Eigen::VectorXd& ts, Eigen::MatrixXd& xs, Eigen::MatrixXd& xds, Eigen::MatrixXd& forcing_terms, Eigen::MatrixXd& fa_output) const;
  
  inline void analyticalSolution(const Eigen::VectorXd& ts, Eigen::MatrixXd& xs, Eigen::MatrixXd& xds, Eigen::MatrixXd& forcing_terms) const
  {
    Eigen::MatrixXd fa_output;
    analyticalSolution(ts, xs, xds, forcing_terms, fa_output);
  }

  inline void analyticalSolution(const Eigen::VectorXd& ts, Eigen::MatrixXd& xs, Eigen::MatrixXd& xds) const
  {
    Eigen::MatrixXd forcing_terms, fa_output;
    analyticalSolution(ts, xs, xds, forcing_terms, fa_output);
  }

  virtual void analyticalSolution(const Eigen::VectorXd& ts, Trajectory& trajectory) const
  {
    Eigen::MatrixXd xs,  xds;
    analyticalSolution(ts, xs, xds);
    statesAsTrajectory(ts, xs, xds, trajectory);
  }

  inline void analyticalSolution(const Eigen::VectorXd& ts, Trajectory& trajectory, Eigen::MatrixXd& forcing_terms) const
  {
    Eigen::MatrixXd xs,  xds;
    analyticalSolution(ts, xs, xds, forcing_terms);
    statesAsTrajectory(ts, xs, xds, trajectory);
  }

  
  
  
  virtual void statesAsTrajectory(const Eigen::MatrixXd& x_in, const Eigen::MatrixXd& xd_in, Eigen::MatrixXd& y_out, Eigen::MatrixXd& yd_out, Eigen::MatrixXd& ydd_out) const;
  
  virtual void statesAsTrajectory(const Eigen::VectorXd& ts, const Eigen::MatrixXd& x_in, const Eigen::MatrixXd& xd_in, Trajectory& trajectory) const;
  
  virtual void train(const Trajectory& trajectory);
      
  virtual void train(const Trajectory& trajectory, std::string save_directory, bool overwrite=false);

  virtual void set_tau(double tau);

  virtual void set_initial_state(const Eigen::VectorXd& y_init);
  
  virtual void set_attractor_state(const Eigen::VectorXd& y_attr);
  
  
  void set_damping_coefficient(double damping_coefficient);
  
  void set_spring_constant(double spring_constant);

        std::string toString(void) const;
    
  //virtual bool isTrained(void) const;
  
  void getSelectableParameters(std::set<std::string>& selectable_values_labels) const;
  void setSelectedParameters(const std::set<std::string>& selected_values_labels);

  int getParameterVectorAllSize(void) const;
  void getParameterVectorAll(Eigen::VectorXd& values) const;
  void setParameterVectorAll(const Eigen::VectorXd& values);
  void getParameterVectorMask(const std::set<std::string> selected_values_labels, Eigen::VectorXi& selected_mask) const;

  void computeFunctionApproximatorInputsAndTargets(const Trajectory& trajectory, Eigen::VectorXd& fa_inputs_phase, Eigen::MatrixXd& fa_targets) const;
  
  virtual void computeFunctionApproximatorOutput(
    const Eigen::Ref<const Eigen::MatrixXd>& phase_state, Eigen::MatrixXd& fa_output) const;
  
  void set_perturbation_analytical_solution(double perturbation_standard_deviation);
  
  double get_perturbation_analytical_solution() const 
  {
    return perturbation_standard_deviation_;
  }
  
protected:

  inline FunctionApproximator* function_approximator(int i_dim) const
  {
    assert(i_dim<(int)function_approximators_.size());
    return function_approximators_[i_dim];
  }
   
  
private:
  DynamicalSystem* goal_system_;   
  SpringDamperSystem* spring_system_; // end of group_linear
  
  DynamicalSystem* phase_system_;
  DynamicalSystem* gating_system_;
  
  std::vector<FunctionApproximator*> function_approximators_;
  
  ForcingTermScaling forcing_term_scaling_;
  
  Eigen::VectorXd trajectory_amplitudes_;
  
  double perturbation_standard_deviation_ = 0.0;
 // end of group_nonlinear
  
  mutable Eigen::VectorXd attractor_state_prealloc_;
  
  mutable Eigen::VectorXd initial_state_prealloc_;
  
  mutable Eigen::MatrixXd fa_outputs_one_prealloc_;
  
  mutable Eigen::MatrixXd fa_outputs_prealloc_;
  
  mutable Eigen::MatrixXd fa_output_prealloc_; 
  
  mutable Eigen::VectorXd forcing_term_prealloc_;
  
  mutable Eigen::VectorXd g_minus_y0_prealloc_;
  
  void initSubSystems(double alpha_spring_system, DynamicalSystem* goal_system,
    DynamicalSystem* phase_system, DynamicalSystem* gating_system);
  
  void initSubSystems(DmpType dmp_type);
  
  void initFunctionApproximators(std::vector<FunctionApproximator*> function_approximators);
  
  static boost::mt19937 rng;
  mutable boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > *analytical_solution_perturber_ = NULL;
  
protected:
   Dmp(void) {};

private:
  friend class boost::serialization::access;
  
  template<class Archive>
  void serialize(Archive & ar, const unsigned int version);
  
};

}

#include <boost/serialization/export.hpp>

BOOST_SERIALIZATION_ASSUME_ABSTRACT(DmpBbo::Dmp);
 
BOOST_CLASS_IMPLEMENTATION(DmpBbo::Dmp,boost::serialization::object_serializable);

#endif // _DMP_H_