trajectory.h

Go to the documentation of this file.

#ifndef GCOP_TRAJECTORY_H
#define GCOP_TRAJECTORY_H

#include <iostream>
#include <cstring>
#include <map>
#include "system.h"

namespace gcop {

  template <typename T, 
    int nx = Dynamic, 
    int nu = Dynamic,
    int np = Dynamic,
    int _ns = Dynamic> 
    class Trajectory {
  public:

    typedef Matrix<double, nx, 1> Vectornd;
    typedef Matrix<double, nu, 1> Vectorcd;
    typedef Matrix<double, nx, nx> Matrixnd;
    typedef Matrix<double, nx, nu> Matrixncd;
    typedef Matrix<double, nu, nx> Matrixcnd;
    typedef Matrix<double, nu, nu> Matrixcd;  

    typedef Matrix<double, np, 1> Vectormd;

    typedef Matrix<double, _ns, 1> Vectorsd;


  Trajectory(const System<T, nx, nu, np>& sys, int ns = 0, int N = 0);
  
  //  Trajectory(const Trajectory<T, nx, nu, np, _ns> &traj);
  
  //  Trajectory& operator=(const Trajectory<T, nx, nu, np, _ns> &traj);
  
  
  //  Trajectory(const System<T, nx, nu, np>& sys, std::istream& istr);
  
  
  virtual ~Trajectory();
  
  //  virtual Trajectory<T, nx, nu, np, _ns>* Clone() const;
  
  virtual bool Update(); 
  
  
  //  void Resize(int sn);
  
  //    void Reverse();
  
  
  // void Init(int sn,
  //    const State *si = 0,
  //    const State *sf = 0);
  
  
    //    void Append(const State &s);
  
  
  
  //  void Clear(); 
  
  //  void SetTime(double t0, double h);
  
  
  //    void Get(State &s) const;
  virtual bool Get(T &x, Vectorcd &u, double t) const;
  
    
    int Get(double t) const;


  // bool Get(Trajectory<T, nx, nu, np, _ns> &traj, double ti, double tf) const;
    
    
  // void Refine(double a = .5);
    
    // void Resample(int sn);


  //    void Modify(int mn, const int *mi, const double *mu);


  //    bool IsValidTime(double t) const;
    

    const System<T, nx, nu, np> &sys;    
  
  int ns; 

    Vectorsd s;           

    vector<double> ts;    
    vector<T> xs;        
    vector<Vectorcd> us;  
    Vectormd p;           
    
  //    bool ext;             ///< extended formulation (used for optimization purposes -- this flag is internally set when one requires variations in the exteneded stapce: space + time)

  //    double hmin;          ///< minimum timestep size (used internally for checking trajectory consistency) it is 1e-10 by default
    
  //    virtual void Log(const char *logName, int di = 1);

  //    virtual void Log(std::ofstream& os, int di = 1);

  //    static void Log(std::ofstream& os, int fw, double a);       


    
  private:
  //    friend std::ostream& operator<<(std::ostream &os, const Trajectory<T, nx, nu, np, _ns> &traj);
  //    friend std::istream& operator>>(std::istream &is, Trajectory &traj);
  };

  template <typename T, int nx, int nu, int np, int _ns> 
    Trajectory<T, nx, nu, np, _ntp>::Trajectory(System<T, nx, nu, np> &sys, int ns) : 
    sys(sys), ns(_ns != Dynamic ? _ns : ns), 
    ts(this->ns + 1), xs(this->ns + 1), us(this->ns), p(sys.P.n) {
    if (_ns == Dynamic)
      s.resize(this->ns);
  }
  

  template <typename T, int nx, int nu, int np, int _ns> 
    bool Trajectory<T, nx, nu, np, _ns>::Get(T &s, 
                                             const T &sa, 
                                             const T &sb, 
                                             double a) const
{
  Vectornd dx;
  sys.X.Lift(dx, sa, sb);
  sys.X.Retract(s, sa, a*dx);
}



  template <typename T, int nx, int nu, int np, int _ns> 
    void Trajectory<T, nx, nu, np, _ns>::Update() {
    assert(ns == us.size()*sys.U.n);
    for (int i = 0; i < us.size(); ++i) {
      memcpy(s.data() + i*sys.U.n, us[i].data(), sys.U.n*sizeof(double));
    }
    // assume parameter p does not play a role
  }
  
  template <typename T, int nx, int nu, int np, int _ns> 
    bool Trajectory<T, nx, nu, np, _ns>::Get(T &x, Vectorcd &u, double t) const {
    int N = us.size();
    if (!N)
      return;
    
    // find the time slot
    if ( (t < ts[0] - hmin) || (t > ts[N] + hmin)) {
      cerr << "[W] dcop::Trajectory::Get:\t time t= " << t << " out of bounds [" << ts[0] << "," << ts[N] << "]!" << endl;
      return;
    }
    
    
    // assume constant timestep  
    double h = ts[1] - ts[0];
    
    if (h < hmin) {
      cerr << "[W] dgc::Trajectory::GetState:\t time step is very small h=" << h << "!" << endl;    
    }
    
    
    if (fabs(t - ts[0]) < hmin) {
      x = xs[0];
      u = us[0];
      return;
    }
    
    if (fabs(t - ts[N]) < hmin) {
      x = xs[N];
      u = us[N-1];
      return;
    }


    double i = (t - ts[0])/h;
    int i0 = (int)i;
    
    //  cout << "t=" << s.t << " ti=" << states[0]->t << " tf=" << states[sn]->t << " h=" << h << endl;
    //  cout << " sn=" << sn << " i=" << i << " i0=" << i0 << endl;
    
    assert(i0 >= 0 && i0 < N);
    
    //  cout << "i0=" << i0 << " i=" << i <<  " sn=" << sn << endl;
    
    double a = i - i0;
    //    u = (1 - a)*us[i0] + a*us[i0 + 1]; 
    u = us[i0];
    Get(x, xs[i0], xs[i0+1], a);
  }
  
  
  template <typename T, int nx, int nu, int np, int _ns> 
    int Trajectory<T, nx, nu, np, _ns>::Get(double t) const {
    int N = us.size();
    // find the time slot
    if ( (t < ts[0] - hmin) || (t > ts[N] + hmin)) {
      cerr << "[W] gcop::Trajectory::Get:\t time t= " << t << " out of bounds [" << ts[0] << "," << ts[N] << "]!" << endl;
      return -1;
    }
    
    // assume constant timestep  
    double h = ts[1] - ts[0];
    
    if (h < hmin) {
      cerr << "[W] gcop::Trajectory::GetState:\t time step is very small h=" << h << "!" << endl;
    }
    
    if (fabs(t - ts[0]) < hmin)
      return 0;
    
    if (fabs(t - ts[N]) < hmin)
      return N-1;
    
    return (int)((t - ts[0])/h);    
  }
}

#endif