docp.h

Go to the documentation of this file.

#ifndef GCOP_DOCP_H
#define GCOP_DOCP_H

#include <Eigen/Dense>
#include <vector>
#include <type_traits>
#include <algorithm>
#include <iterator>
#include "system.h"
#include "cost.h"
#include <cmath>
#include "rn.h"

namespace gcop {
  
  using namespace std;
  using namespace Eigen;
 
  template <typename T, 
    int nx = Dynamic, 
    int nu = Dynamic,
    int np = Dynamic> class Docp {
    
    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;  
    
  public:
    Docp(System<T, nx, nu, np> &sys, Cost<T, nx, nu, np> &cost, 
         vector<double> &ts, vector<T> &xs, vector<Vectorcd> &us, 
         Vectormd *p = 0, bool update = true);
    
    virtual ~Docp();

    virtual void Iterate();

    void Update(bool der = true);
    
    System<T, nx, nu, np> &sys;    

    Cost<T, nx, nu, np> &cost;     

    std::vector<double> &ts; 

    std::vector<T> &xs;      

    std::vector<Vectorcd> &us;      
    
    Vectormd *p;               
    
    std::vector<Matrixnd> As;    
    std::vector<Matrixncd> Bs;   
    
    bool debug;  
    
    double eps;  

    double J;    

    int nofevaluations;

  };

  using namespace std;
  using namespace Eigen;
  
  template <typename T, int nx, int nu, int np> 
    Docp<T, nx, nu, np>::Docp(System<T, nx, nu, np> &sys, 
                              Cost<T, nx, nu, np> &cost, 
                              vector<double> &ts, 
                              vector<T> &xs, 
                              vector<Matrix<double, nu, 1> > &us,
                              Matrix<double, np, 1> *p,
                              bool update) : 
    sys(sys), cost(cost), ts(ts), xs(xs), us(us), p(p),
    As(us.size()), Bs(us.size()), J(std::numeric_limits<double>::max()), 
    debug(true), eps(1e-3), nofevaluations(0)
    {
      int N = us.size();
      assert(N > 0);
      assert(ts.size() == N+1);
      assert(xs.size() == N+1);

      if (nx == Dynamic || nu == Dynamic) {
        for (int i = 0; i < N; ++i) {
          As[i].resize(sys.X.n, sys.X.n);
          Bs[i].resize(sys.X.n, sys.U.n);
        }
      }

      if (update) {
        Update();
      }
    }
  
  template <typename T, int nx, int nu, int np> 
    Docp<T, nx, nu, np>::~Docp()
    {
    }
  
  template <typename T, int nx, int nu, int np> 
    void Docp<T, nx, nu, np>::Update(bool der) {

    typedef Matrix<double, nx, 1> Vectornd;
    typedef Matrix<double, nu, 1> Vectorcd;
    
    Vectornd dx;
    Vectorcd du;
    Vectornd dfp;
    Vectornd dfm;
    T xav(xs[0]);
    T xbv(xs[0]);
    
    //    cout << "SIZE=" << ((MbsState*)&xav)->r.size() << endl;
    
    if (nx == Dynamic) {
      dx.resize(sys.X.n);
      dfp.resize(sys.X.n);
      dfm.resize(sys.X.n);
    }
    
    if (nu == Dynamic)
      du.resize(sys.U.n);    

    int N = us.size();

    sys.Reset(xs[0],ts[0]);//Reset the internal state

    for (int k = 0; k < N; ++k) {
      double h = ts[k+1] - ts[k];
      if (der) {
        static const double q = 1093121342312;  // a random number
        As[k](0,0) = q;
        Bs[k](0,0) = q;
        
        const T &xa = xs[k];
        T &xb = xs[k+1];
        const Vectorcd &u = us[k];

        //sys.Step(xb, ts[k], xa, u, h, p, &As[k], &Bs[k], 0);
        sys.Step(xs[k+1], us[k], h, p, &As[k], &Bs[k], 0);

        //        cout << "B=" << endl << Bs[k] << endl;
        

        //        if (fabs(As[k](0,0) - q) < 1e-10) {
        //          autodiff.DF(xb, ts[k], xa, u, h, &As[k], &Bs[k]);
        //        }

        
        // if no jacobians were provided use finite differences
        if (fabs(As[k](0,0) - q) < 1e-10) {
                    
          assert(sys.X.n > 0 && sys.U.n > 0);

          for (int i = 0; i < sys.X.n; ++i) {
            dx.setZero();
            dx[i] = eps;
            
            // xav = xa + dx
            sys.X.Retract(xav, xa, dx);
            
            // reconstruct state using previous time-step
            sys.Rec(xav, h);
            
            // xbv = F(xav)
            sys.Step(xbv, ts[k], xav, u, h, p);
            
            // df = xbv - xb
            sys.X.Lift(dfp, xb, xbv);

            // xav = xa - dx
            sys.X.Retract(xav, xa, -dx);
            
            // reconstruct state using previous time-step
            sys.Rec(xav, h);
            
            // xbv = F(xav)
            sys.Step(xbv, ts[k], xav, u, h, p);
            
            // dfm = xbv - xb
            sys.X.Lift(dfm, xb, xbv);

            As[k].col(i) = (dfp - dfm)/(2*eps);
          }
          sys.Reset(xs[k+1],ts[k+1]);//Reset the internal state
          //cout<<"As["<<k<<"]: "<<endl<<As[k]<<endl;
        }
        
        if (fabs(Bs[k](0,0) - q) < 1e-10) {
          
          for (int i = 0; i < sys.U.n; ++i) {
            du.setZero();
            du[i] = eps;
            
            // xbv = F(xa, u + du)
            sys.Step(xbv, ts[k], xa, u + du, h, p);
            
            // df = xbv - xb
            sys.X.Lift(dfp, xb, xbv);

            // xbv = F(xa, u - du)
            sys.Step(xbv, ts[k], xa, u - du, h, p);
            
            // df = xbv - xb
            sys.X.Lift(dfm, xb, xbv);

            Bs[k].col(i) = (dfp - dfm)/eps;
          }
          sys.Reset(xs[k+1],ts[k+1]);//Reset the internal state
         // cout<<"Bs["<<k<<"]: "<<endl<<Bs[k]<<endl;
        }        
      } else {
        sys.Step(xs[k+1], us[k], h, p);
      }
    }
  } 

  template <typename T, int nx, int nu, int np> 
    void Docp<T, nx, nu, np>::Iterate() {
    cout << "[W] Docp::Iterate: subclasses should implement this!" << endl;
  }
}

#endif