lscost.h

Go to the documentation of this file.

#ifndef GCOP_LSCOST_H
#define GCOP_LSCOST_H

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

namespace gcop {

  using namespace Eigen;
  using namespace std;
  
  template <typename T = VectorXd, 
    int _nx = Dynamic,
    int _nu = Dynamic,
    int _np = Dynamic,
    int _ng = Dynamic,
    typename Tc = T> class LsCost : public Cost<T, _nx, _nu, _np, Tc> {
    
  public:
  
  typedef Matrix<double, _ng, 1> Vectorgd;
  typedef Matrix<double, _ng, _nx> Matrixgxd;
  typedef Matrix<double, _ng, _nu> Matrixgud;
  typedef Matrix<double, _ng, _np> Matrixgpd;
  
  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;
  
  LsCost(System<T, _nx, _nu, _np> &sys, double tf, int ng = 0);
  
  virtual bool Res(Vectorgd &g, 
                   double t, const T &x, const Vectorcd &u, double h,
                   const Vectormd *p = 0,
                   Matrixgxd *dgdx = 0, Matrixgud *dgdu = 0,
                   Matrixgpd *dgdp = 0);

  int ng;       
  Vectorgd g;   
  };
  
  
  template <typename T, int _nx, int _nu, int _np, int _ng, typename Tc> 
    LsCost<T, _nx, _nu, _np, _ng, Tc>::LsCost(System<T, _nx, _nu, _np> &sys, 
                                              double tf, int ng) : 
    Cost<T, _nx, _nu, _np, Tc>(sys, tf), ng(_ng != Dynamic ? _ng : ng)
    {
      assert(ng > 0);
      if (_ng == Dynamic)
        g.resize(ng);
    }
  
  template <typename T, int _nx, int _nu, int _np, int _ng, typename Tc> 
    bool LsCost<T, _nx, _nu, _np, _ng, Tc>::Res(Vectorgd &g, 
                                                double t, const T &x, const Vectorcd &u, double h,
                                                const Vectormd *p,
                                                Matrixgxd *dgdx, Matrixgud *dgdu,
                                                Matrixgpd *dgdp) {
    cout << "[W] LsCost:Res: unimplemented! Subclasses should override." << endl;
    return false;
  }
}

#endif