cost.h

Go to the documentation of this file.

#ifndef GCOP_COST_H
#define GCOP_COST_H

#include <Eigen/Dense>
#include <iostream>
#include "system.h"

namespace gcop {

  using namespace Eigen;
  using namespace std;
  
  template <typename T, 
    int _nx = Dynamic, 
    int _nu = Dynamic,
    int _np = Dynamic,
    typename Tc = T> class Cost {
  public:
  
  typedef Matrix<double, _nx, 1> Vectornd;
  typedef Matrix<double, _nu, 1> Vectorcd;
  typedef Matrix<double, _np, 1> Vectormd;

  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, Dynamic, 1> Vectormd;
  typedef Matrix<double, _np, _np> Matrixmd;
  typedef Matrix<double, _nx, _np> Matrixnmd;
  typedef Matrix<double, _np, _nx> Matrixmnd;
  
  Cost(System<T, _nx, _nu, _np> &sys, double tf);

  //Cost(System<T, _nx, _nu, _np> &sys, Sensor<double tf);
  
  virtual double L(double t, const T &x, const Vectorcd &u, double h,
                   const Vectormd *p = 0,
                   Vectornd *Lx = 0, Matrixnd* Lxx = 0,
                   Vectorcd *Lu = 0, Matrixcd* Luu = 0,
                   Matrixncd *Lxu = 0,
                   Vectormd *Lp = 0, Matrixmd *Lpp = 0,
                   Matrixmnd *Lpx = 0);  

  virtual bool SetContext(const Tc &c) { return true; };
  
  System<T, _nx, _nu, _np> &sys;  
  double tf;                      
  
};


  template <typename T, int _nx, int _nu, int _np, typename Tc> 
    Cost<T, _nx, _nu, _np, Tc>::Cost(System<T, _nx, _nu, _np> &sys, 
                                     double tf) : sys(sys), tf(tf) {
  }
  
  template <typename T, int _nx, int _nu, int _np, typename Tc> 
    double Cost<T, _nx, _nu, _np, Tc>::L(double t, const T& x, const Vectorcd& u, double h,
                                         const Vectormd *p,
                                         Matrix<double, _nx, 1> *Lx, Matrix<double, _nx, _nx>* Lxx,
                                         Matrix<double, _nu, 1> *Lu, Matrix<double, _nu, _nu>* Luu,
                                         Matrix<double, _nx, _nu> *Lxu,
                                         Vectormd *Lp, Matrixmd *Lpp,
                                         Matrixmnd *Lpx) {
    cout << "[W] Cost:L: unimplemented!" << endl;
    return 0;
  }
}

#endif