ExponentialSystem.cpp

Go to the documentation of this file.

#include <boost/serialization/export.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/xml_iarchive.hpp>
#include <boost/archive/xml_oarchive.hpp>
#include "dynamicalsystems/ExponentialSystem.hpp"

BOOST_CLASS_EXPORT_IMPLEMENT(DmpBbo::ExponentialSystem);

#include <boost/serialization/base_object.hpp>

#include <cmath>
#include <vector>
#include <iostream>
#include <eigen3/Eigen/Core>

#include "dmpbbo_io/EigenBoostSerialization.hpp"
#include "dmpbbo_io/BoostSerializationToString.hpp"


using namespace std;
using namespace Eigen;  

namespace DmpBbo {
    
ExponentialSystem::ExponentialSystem(double tau, Eigen::VectorXd y_init, Eigen::VectorXd y_attr, double alpha, std::string name)
  : DynamicalSystem(1, tau, y_init, y_attr, name),
  alpha_(alpha)
{
  attractor_state_prealloc_ = VectorXd::Zero(dim_orig());
}

ExponentialSystem::~ExponentialSystem(void)
{
}

DynamicalSystem* ExponentialSystem::clone(void) const
{
  return new ExponentialSystem(tau(),initial_state(),attractor_state(),alpha_,name());
}


void ExponentialSystem::differentialEquation(
  const Eigen::Ref<const Eigen::VectorXd>& x, 
  Eigen::Ref<Eigen::VectorXd> xd) const
{
  ENTERING_REAL_TIME_CRITICAL_CODE
  // xd = alpha_*(attractor_state()-x)/tau(); // Non-realtime version now commented out  
  
  attractor_state(attractor_state_prealloc_);
  xd.noalias() = alpha_*(attractor_state_prealloc_-x)/tau();
  
  EXITING_REAL_TIME_CRITICAL_CODE
}

void ExponentialSystem::analyticalSolution(const VectorXd& ts, MatrixXd& xs, MatrixXd& xds) const
{
  int T = ts.size();
  assert(T>0);

  // Usually, we expect xs and xds to be of size T X dim(), so we resize to that. However, if the
  // input matrices were of size dim() X T, we return the matrices of that size by doing a 
  // transposeInPlace at the end. That way, the user can also request dim() X T sized matrices.
  bool caller_expects_transposed = (xs.rows()==dim() && xs.cols()==T);

  // Prepare output arguments to be of right size (Eigen does nothing if already the right size)
  xs.resize(T,dim());
  xds.resize(T,dim());
  
  VectorXd val_range = initial_state() - attractor_state();
  
  VectorXd exp_term  = -alpha_*ts/tau();
  exp_term = exp_term.array().exp().transpose();
  VectorXd pos_scale =                   exp_term;
  VectorXd vel_scale = -(alpha_/tau()) * exp_term;
  
  xs = val_range.transpose().replicate(T,1).array() * pos_scale.replicate(1,dim()).array();
  xs += attractor_state().transpose().replicate(T,1);
  xds = val_range.transpose().replicate(T,1).array() * vel_scale.replicate(1,dim()).array();
  
  if (caller_expects_transposed)
  {
    xs.transposeInPlace();
    xds.transposeInPlace();
  }
}

template<class Archive>
void ExponentialSystem::serialize(Archive & ar, const unsigned int version)
{
  // serialize base class information
  ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(DynamicalSystem);

  ar & BOOST_SERIALIZATION_NVP(alpha_);
}

string ExponentialSystem::toString(void) const
{
  RETURN_STRING_FROM_BOOST_SERIALIZATION_XML("ExponentialSystem");
}

}