sensorcost.h

Go to the documentation of this file.

#ifndef GCOP_SENSORCOST_H
#define GCOP_SENSORCOST_H

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

namespace gcop {

  using namespace Eigen;
  using namespace std;
  
   //T1 can be the input manifold of sensor which need not be the same as the input of system since we are sharing the same sensor between multiple systems
   //typename T1 = T 

  template <typename T = VectorXd, 
    int _nx = Dynamic, 
    int _nu = Dynamic,
    int _np = Dynamic,
    typename Tz = VectorXd,
    int _nz = Dynamic> class SensorCost {
  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;
  
  typedef Matrix<double, _nz, 1> Vectorrd;
  typedef Matrix<double, _nz, _nz> Matrixrd;
  typedef Matrix<double, _nz, _nx> Matrixrnd;
  typedef Matrix<double, _nz, _nu> Matrixrcd;
  typedef Matrix<double, _nz, _np> Matrixrmd;

  SensorCost(System<T, _nx, _nu, _np> &sys, Manifold<Tz, _nz>  &Z);

  virtual double L(double t, const Tz &z, const Vectornd &w,
                   const Vectormd &p, double h, 
                   Vectornd *Lw = 0, Matrixnd* Lww = 0,
                   Vectormd *Lp = 0, Matrixmd* Lpp = 0,
                   Vectorrd *Lz = 0, Matrixrd* Lzz = 0,
                   Matrixmnd *Lpw = 0, Matrixrnd* Lzw = 0, Matrixrmd* Lzp = 0);  
  virtual double Lp(const Vectormd &p,
                   Vectormd *Lp = 0, Matrixmd *Lpp = 0);

  System<T, _nx, _nu, _np> &sys;  
  Manifold<Tz, _nz> &Z;  
};


  template <typename T, int _nx, int _nu, int _np, typename Tz, int _nz> 
    SensorCost<T, _nx, _nu, _np, Tz, _nz>::SensorCost(System<T, _nx, _nu, _np> &sys, 
                                 Manifold<Tz, _nz> &Z) : sys(sys), 
                                                         Z(Z) {
  }  

  template <typename T, int _nx, int _nu, int _np, typename Tz, int _nz> 
  double SensorCost<T, _nx, _nu, _np, Tz, _nz>::L(double t, const Tz &z, 
                                             const Vectornd &w,
                                             const Vectormd &p, double h,
                                             Vectornd *Lw, Matrixnd* Lww,
                                             Vectormd *Lp, Matrixmd* Lpp,
                                             Vectorrd *Lz, Matrixrd* Lzz,
                                             Matrixmnd *Lpw, Matrixrnd* Lzw, Matrixrmd* Lzp)
  {
    cout << "[W] Cost:L: unimplemented!" << endl;
    return 0;
  }

  template <typename T, int _nx, int _nu, int _np, typename Tz, int _nz> 
  double SensorCost<T, _nx, _nu, _np, Tz, _nz>::Lp(const Vectormd &p,
                   Vectormd *Lp, Matrixmd *Lpp)
  {
    return 0;
  }
}

#endif