controllertparam.h

Go to the documentation of this file.

// This file is part of libgcop, a library for Geometric Control, Optimization, and Planning (GCOP)
//
// Copyright (C) 2004-2014 Marin Kobilarov <marin(at)jhu.edu>
//
// This Source Code Form is subject to the terms of the Mozilla
// Public License v. 2.0. If a copy of the MPL was not distributed
// with this file, You can obtain one at http://mozilla.org/MPL/2.0/.

#ifndef GCOP_CONTROLLERTPARAM_H
#define GCOP_CONTROLLERTPARAM_H

#include "tparam.h"
#include "controller.h"
#include "constraint.h"
#include <assert.h>
#include "utils.h"

namespace gcop {
  
  using namespace std;
  using namespace Eigen;
  
  template <typename Tx, 
    int nx = Dynamic, 
    int nu = Dynamic,
    int np = Dynamic,
    int _ntp = Dynamic,
    typename Tc = VectorXd> class ControllerTparam : public Tparam<Tx, nx, nu, np, _ntp> {
    
    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, _ntp, 1> Vectorntpd;
    typedef Matrix<double, _ntp, _ntp> Matrixntpd;
    
  public:
    ControllerTparam(System<Tx, nx, nu, np> &sys, 
                     Controller<Tx, Vectorcd, Vectorntpd, Tc> &ctrl,
                     int ntp = _ntp,
                     Constraint<Tx, nx, nu, np, 1> *con = 0);
    
    bool To(Vectorntpd &s, 
            const vector<double> &ts, 
            const vector<Tx> &xs, 
            const vector<Vectorcd> &us,
            const Vectormd *p = 0);
    
    bool From(vector<double> &ts, 
              vector<Tx> &xs, 
              vector<Vectorcd> &us,
              const Vectorntpd &s,
              Vectormd *p = 0);

    virtual bool SetContext(const Tc& c);
    
    Controller<Tx, Vectorcd, Vectorntpd, Tc> &ctrl; 
    Constraint<Tx, nx, nu, np, 1> *con;         
 
    bool stoch;       
    Vectornd w;       
  };
  
  template <typename Tx, int nx, int nu, int np, int _ntp, typename Tc> 
    ControllerTparam<Tx, nx, nu, np, _ntp, Tc>::ControllerTparam(System<Tx, nx, nu, np> &sys, Controller<Tx, Vectorcd, Vectorntpd, Tc> &ctrl, int ntp, Constraint<Tx, nx, nu, np, 1> *con) :  Tparam<Tx, nx, nu, np, _ntp>(sys, ntp), ctrl(ctrl), con(con), stoch(true) {
    assert(ntp > 0);
    if (nx == Dynamic)
      w.resize(sys.X.n);
  }

  template <typename Tx, int nx, int nu, int np, int _ntp, typename Tc> 
    bool ControllerTparam<Tx, nx, nu, np, _ntp, Tc>::To(Vectorntpd &s,
                                                        const vector<double> &ts, 
                                                        const vector<Tx> &xs, 
                                                        const vector<Vectorcd> &us,
                                                        const Vectormd *p) {
    cout << "[W] ControllerTparam::To: calling this is useless for this type of paremetrization!" << endl;    
    return false;
  }  
  
  template <typename Tx, int nx, int nu, int np, int _ntp, typename Tc> 
    bool ControllerTparam<Tx, nx, nu, np, _ntp, Tc>::SetContext(const Tc &c) {
    return ctrl.SetContext(c);
  }

  template <typename Tx, int nx, int nu, int np, int _ntp, typename Tc> 
    bool ControllerTparam<Tx, nx, nu, np, _ntp, Tc>::From(vector<double> &ts, 
                                                          vector<Tx> &xs, 
                                                          vector<Vectorcd> &us,
                                                          const Vectorntpd &s,
                                                          Vectormd *p) {
    ctrl.SetParams(s);
    Matrix<double, 1, 1> g;
    
    // sample noise term, assume constant along path
    for (int i = 0; i < us.size(); ++i) {
      ctrl.Set(us[i], ts[i], xs[i]);
      
      if (stoch) {
        for (int j=0; j < w.size(); ++j)
          w[j] = random_normal();
        
        this->sys.Step(xs[i+1], ts[i], xs[i], us[i], ts[i+1] - ts[i], w, p);      
      } else {
        this->sys.Step(xs[i+1], ts[i], xs[i], us[i], ts[i+1] - ts[i], p);
      }
      
      if (con) { // check for feasibility
        (*con)(g, ts[i+1], xs[i+1], us[i], p);
        if (g[0] > 0) {  // if infeasible then set all remaining states to current state
          for (int j = i+2; j < xs.size(); ++j) {
            xs[j] = xs[i+1];         
            us[j-1].setZero();
          }
          return false;
        }
      }
    }
    return true;
  }
}

#endif