gndocpv2.h

Go to the documentation of this file.

#ifndef GCOP_GNDOCP_H
#define GCOP_GNDOCP_H

#include "docp.h"
#include "lqcost.h"
#include "tparam.h"

#include <unsupported/Eigen/NonLinearOptimization>

#ifdef GCOP_GNDOCP_CERES
#include "ceres/ceres.h"
#include "glog/logging.h"
#endif


namespace gcop {

#ifdef GCOP_GNDOCP_CERES
using ceres::DynamicNumericDiffCostFunction;
using ceres::CENTRAL;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solver;
using ceres::Solve;
#endif
  
  using namespace std;
  using namespace Eigen;

 
  template <typename T, 
    int _nx, 
    int _nu,
    int _np,
    int _ng,
    int _ntp> class GnCost;

  template <typename _Functor> class SampleNumericalDiff;
  
  template <typename T, 
    int _nx = Dynamic, 
    int _nu = Dynamic,
    int _np = Dynamic,
    int _ng = Dynamic,
    int _ntp = Dynamic> class GnDocp : 
    public Docp<T, _nx, _nu, _np> {
    
    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, _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, _np, _np> Matrixmd;
    typedef Matrix<double, _nx, _np> Matrixnmd;
    typedef Matrix<double, _np, _nx> Matrixmnd;    
  public:
    GnDocp(System<T, _nx, _nu, _np> &sys, 
           LqCost<T, _nx, _nu, _np, _ng> &cost, Tparam<T, _nx, _nu, _np, _ntp> &tparam,
           vector<double> &ts, vector<T> &xs, vector<Vectorcd> &us, 
           Vectormd *p = 0, bool update = true);
    
    virtual ~GnDocp();

    void Iterate();
    Tparam<T, _nx, _nu, _np, _ntp> &tparam;

    int info;
    double fnorm, covfac;

    int inputs;
    int values;

    double numdiff_stepsize;   
     
    VectorXd s;  
    
    GnCost<T, _nx, _nu, _np, _ng, _ntp> *functor;

    SampleNumericalDiff<GnCost<T, _nx, _nu, _np, _ng, _ntp> > *numDiff;
    LevenbergMarquardt<SampleNumericalDiff<GnCost<T, _nx, _nu, _np, _ng, _ntp> > > *lm;
  };

  
// Generic functor
template<typename _Scalar, int NX=Dynamic, int NY=Dynamic>
struct Functor
{
  typedef _Scalar Scalar;
  enum {
    InputsAtCompileTime = NX,
    ValuesAtCompileTime = NY
  };
  typedef Matrix<Scalar,InputsAtCompileTime,1> InputType;
  typedef Matrix<Scalar,ValuesAtCompileTime,1> ValueType;
  typedef Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType;

  const int m_inputs, m_values;

  Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}
  Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {}

  int inputs() const { return m_inputs; }
  int values() const { return m_values; }

  // you should define that in the subclass :
//  void operator() (const InputType& x, ValueType* v, JacobianType* _j=0) const;
};


 template <typename T, 
   int _nx = Dynamic,
   int _nu = Dynamic,
   int _np = Dynamic,
   int _ng = Dynamic,
   int _ntp = Dynamic>
   struct GnCost : Functor<double> {  

   GnDocp<T, _nx, _nu, _np, _ng, _ntp> *docp;

   GnCost<T, _nx, _nu, _np, _ng, _ntp>(int inputs, int values) : Functor<double>(inputs, values), docp(0) {};
   
   typedef Matrix<double, _nx, 1> Vectornd;
   typedef Matrix<double, _nu, 1> Vectorcd;
   typedef Matrix<double, _ng, 1> Vectorgd;
   
   int operator()(const VectorXd &s, VectorXd &fvec) const
   {
     assert(docp);
     docp->tparam.From(docp->ts, docp->xs, docp->us, s, docp->p);

     //#DEBUG Number of function evaluations:
     //cout<<++(docp->nofevaluations)<<endl;
     ++(docp->nofevaluations);

     //cout<<"Docp->Xsback: "<<(docp->xs).back().transpose()<<endl;//what is this back doing?
     if(docp->p)
     {
       cout<<"docp->p: "<<(docp->p)->transpose()<<endl;
     }
     //cout<<"Xf: "<<docp->xs[(docp->xs).size()-1].transpose()<<endl;
     /*
       for (int i = 0, k = 0; k < docp.us.size(); ++k) {
       memcpy(docp.us[k].data(), s.data() + i, docp.sys.U.n*sizeof(double));
       i += docp.sys.U.n;
      }
      docp.Update(false);
      */
     
     Vectorgd &g = ((LsCost<T, _nx, _nu, _np, _ng>&)docp->cost).g;
     
     int i = 0;
      for (int k = 0; k < docp->us.size(); ++k) {
        const double &t = docp->ts[k];
        double h = docp->ts[k+1] - t;
        const T& x = docp->xs[k];
        const Vectorcd& u = docp->us[k];
        
        ((LqCost<T, _nx, _nu, _np, _ng>&)docp->cost).Res(g, t, x, u, h, docp->p);
        //double cost_debug = ((LqCost<T, _nx, _nu, _np, _ng>&)docp->cost).L(t, x, u, h, docp->p);
        memcpy(fvec.data() + i, g.data(), g.size()*sizeof(double));
        i += g.size();
        /*cout<<"docp->us["<<k<<"]: "<<(docp->us[k].transpose())<<endl;
        cout<<"g["<<k<<"]: "<<g.transpose()<<endl;
        cout<<"gcost["<<k<<"]: "<<0.5*(g.transpose()*g)<<endl;
        cout<<"Lcost["<<k<<"]: "<<cost_debug<<endl;
        // 
        ((LqCost<T, _nx, _nu>&)docp->cost).ResX(rx, t, x, h);
        memcpy(fvec.data() + i, rx.data(), rx.size()*sizeof(double));
        i += rx.size();

        ((LqCost<T, _nx, _nu>&)docp->cost).ResU(ru, t, u, h);
        memcpy(fvec.data() + i, ru.data(), ru.size()*sizeof(double));
        i += ru.size();
        */
      }
      
      ((LqCost<T, _nx, _nu, _np, _ng>&)docp->cost).Res(g, 
                                                       docp->ts.back(), 
                                                       docp->xs.back(), 
                                                       docp->us.back(), 0);
      //cout<<"g[end]: "<<g.transpose()<<endl;
      memcpy(fvec.data() + i, g.data(), g.size()*sizeof(double));
      docp->J = 0.5*(fvec.transpose()*fvec)(0);
      //cout<<"s: "<<s.transpose()<<endl;
      //cout<<"fvec: "<<fvec.transpose()<<endl;
      //cout<<"Cost: "<<(docp->J)<<endl;
      //getchar(); // #DEBUG
      return 0;
    }
  };

//Creating Numerical Difference Class ourselves
template<typename _Functor,
  int _nx = Dynamic,
  int _nu = Dynamic,
  int _np = Dynamic,
  int _ng = Dynamic,
  int _ntp = Dynamic> class SampleNumericalDiff : public _Functor{

  public:
    typedef _Functor Functor;
    typedef typename Functor::Scalar Scalar;
    typedef typename Functor::InputType InputType;
    typedef typename Functor::ValueType ValueType;
    typedef typename Functor::JacobianType JacobianType;
    typedef Matrix<double, _nx, 1> Vectornd;
    typedef Matrix<double, _nu, 1> Vectorcd;
    typedef Matrix<double, _ng, 1> Vectorgd;

    Vectorcd duscale;
    Vectornd dxscale;

    SampleNumericalDiff(Scalar _epsfcn=0.) : Functor(), epsfcn(_epsfcn), normal_dist(0,1) {
      duscale = Vectorcd::Constant(0.02);//Default initialization of scale
      dxscale = Vectornd::Constant(0.02);//Default initialization of scale
    }
    SampleNumericalDiff(const Functor& f, Scalar _epsfcn=0.) : Functor(f), epsfcn(_epsfcn), normal_dist(0,1) {
      randgenerator.seed(370213);
    }

    // forward constructors
    template<typename T0>
      SampleNumericalDiff(const T0& a0) : Functor(a0), epsfcn(0) {}
    template<typename T0, typename T1>
      SampleNumericalDiff(const T0& a0, const T1& a1) : Functor(a0, a1), epsfcn(0) {}
    template<typename T0, typename T1, typename T2>
      SampleNumericalDiff(const T0& a0, const T1& a1, const T2& a2) : Functor(a0, a1, a2), epsfcn(0) {}

    enum {
      InputsAtCompileTime = Functor::InputsAtCompileTime,
      ValuesAtCompileTime = Functor::ValuesAtCompileTime
    };

    //This finds the state and control Jacobians along the trajectory this is accurate
    // In principle this should be shifted to docp.h #TODO #IMPORTANT
    void Linearize(int Ns)
    {
      randgenerator.seed(370213);
      //Local Variables:
      int N = docp->us.size();
      int nu = docp->us[0].size();
      int nx = docp->xs[0].size();
      MatrixXd dusmatrix(nu*N,Ns);
      MatrixXd dxsmatrix(nx*(N+1),Ns);

      //Vectorcd du;
      Vectorcd us1;

      int count = 0;

      //dxsmatrix.block(0,0,nx,Ns).setZero();//Set zero first block
      Vectornd dx;
      T x0_sample;
      Rn<nu> &U = (Rn<nu>&)(docp->sys.U);
      //NonParallelizable code for now
      for(count = 0;count < Ns;count++)
      {
        //Set to initial state perturbed by small amount:
        for(int count1 = 0; count1 < nx; count1++)
        {
          dx(count1) = dxscale(count1)*normal_dist(randgenerator);//Adjust dx_scale 
        }
        dxsmatrix.block<nx,1>(0,count) = dx; 
        //cout<<"dx0: "<<dx.transpose()<<endl;
        //cout<<dx.norm();

        docp->sys.X.Retract(x0_sample, docp->xs[0], dx);

        //xss[0] = x0_sample;
        docp->sys.Reset(x0_sample,docp->ts[0]);
        //No Feedback for now:
        for(int count1 = 0;count1 < N;count1++)
        {
          for(int count_u = 0;count_u < nu; count_u++)
          {
            us1[count_u] = us1[count_u] + duscale[count_u]*normal_dist(randgenerator);
          }
          //The sampled control should be within the control bounds of the system !!!
          if (U.bnd) {
            for (int count_u = 0; count_u < nu; ++count_u)
              if (us1[count_u] < U.lb[count_u]) {
                //cout<<"I hit bound"<<endl;//[DEBUG]
                us1[count_u] = U.lb[count_u];
                //   du[j] = un[j] - u[j];
              } else
                if (us1[count_u] > U.ub[count_u]) {
                  us1[count_u] = U.ub[count_u];
                  //du[j] = un[j] - u[j];
                }
          }

          dusmatrix.block<nu,1>(count1*nu, count) = us1 - docp->us[count1];//Verify that this is zero without randomness #DEBUG

          //cout<<"du, u: "<<du.transpose()<<"\t"<<(this->us[count1]).transpose()<<endl;
          docp->sys.Step2(us1,(docp->ts[count1+1])-(docp->ts[count1]), this->p);

          docp->sys.X.Lift(dx, docp->xs[count1+1], docp->sys.x);//For storing the dx
          dxsmatrix.block<nx,1>((count1+1)*nx,count) = dx; 
        }
      }
      //getchar();
      MatrixXd Abs(nx+nu,nx);
      MatrixXd XUMatrix(nx+nu,Ns);

      MatrixXd Xverifymatrix(nx,Ns);//Verify 

      //Compute As and Bs for every k:
      for(count = 0;count < N;count++)
      {
        XUMatrix<<dxsmatrix.block(count*nx,0,nx,Ns), dusmatrix.block(count*nu,0,nu,Ns);
        Abs = (XUMatrix*XUMatrix.transpose()).ldlt().solve(XUMatrix*dxsmatrix.block((count+1)*nx,0,nx,Ns).transpose());

        docp->As[count] = Abs.block<nx,nx>(0,0).transpose();
        docp->Bs[count] = Abs.block<nu,nx>(nx,0).transpose();

        /******VERIFY**********/
        Xverifymatrix = Abs.transpose()*XUMatrix;
        //cout<<endl<<"dXpredicted: "<<endl<<Xverifymatrix<<endl;
        //cout<<endl<<"True_dX: "<<dxsmatrix.block((count+1)*nx,0,nx,Ns)<<endl;
        cout<<endl<<"Error_predicted: "<<(Xverifymatrix - dxsmatrix.block((count+1)*nx,0,nx,Ns)).squaredNorm()<<endl;
        //cout<<endl<<Abs<<endl;
        //getchar();
      }
    }

    int df(const InputType& _x, JacobianType &jac)
    {
      using std::sqrt;
      using std::abs;
      using std::cout;
      using std::endl;

      //Linearize the system:

      /* Local variables */
      int nfev= 50;//Number of function evaluations can be automated also
      this->Linearize(nfev);// Linearize to find state and ctrl Jacobians
      //Outline:
      //For each parameter pw:
      //Find jac(input_parameter)wrt parameter 
      //input_parameter = stack(res_i) so we have to find jac(res_i)wrt parameter p
      // res_i = [Qsqrt*x_i; Rsqrt*u_i] so jac(res_i) = [Qsqrt*jac(x_i); Rsqrt*jac(u_i)];
      //Use sequential computation for jac(x_i) = A_i*jac(x_i-1) + B_i*jac(u_i-1)


      ValueType val1, val2;
      InputType x = _x;
      // TODO : we should do this only if the size is not already known
      val1.resize(Functor::values());
      val2.resize(Functor::values());
      MatrixXd dymatrix(Functor::values(), nfev);
      MatrixXd dumatrix(n, nfev);

      // initialization
      Functor::operator()(x, val1);//Mean input and value

      // Function Body
      for(int ns = 0; ns < nfev; ++ns)
      {
        for (int j = 0; j < n; ++j) {

          /*h = eps * abs(x[j]);
            if (h == 0.) {
            h = eps;
            }
            h = eps;
            if(bernoulli_dist(randgenerator))
            {
            dumatrix(j, ns) = h;
            }
            else
            {
            dumatrix(j, ns) = -h;
            }
           */
          dumatrix(j, ns) = eps*normal_dist(randgenerator);
        }//Do perturbations to the whole vector
        x = _x + dumatrix.col(ns);
        Functor::operator()(x, val2);//Evaluate at new perturbed vector
        dymatrix.col(ns) = val2 - val1;//dY
        //cout<<"dumatrix["<<ns<<"]: "<<dumatrix.col(ns).transpose()<<endl;
        //getchar();
        //cout<<"dymatrix["<<ns<<"]: "<<dymatrix.col(ns).transpose()<<endl;
        //getchar();
      }
      jac = (dumatrix*dumatrix.transpose()).ldlt().solve(dumatrix*dymatrix.transpose()).transpose();
      //cout<<"jac: "<<endl<<jac<<endl;
      cout<<endl<<"Error_predicted: "<<sqrt((dymatrix - jac*dumatrix).squaredNorm())<<endl;
      getchar();
      return nfev;
    }
  private:
    Scalar epsfcn;

    SampleNumericalDiff& operator=(const SampleNumericalDiff&);

    std::default_random_engine randgenerator; 

    //default constructor gives p = 0.5 benoulli
    //std::bernoulli_distribution bernoulli_dist;     ///< Bernoulli generator for getting perturbations
    std::normal_distribution<double> normal_dist;
};



#ifdef GCOP_GNDOCP_CERES

  // A cost functor that implements the residual r = 10 - x.
  template <typename T, int _nx = Dynamic, int _nu = Dynamic>
    struct GnCost {  
      GnCost<T, _nx, _nu>(GnDocp<T, _nx, _nu> &docp) : docp(docp) {};

      GnDocp<T, _nx, _nu> &docp;

    typedef Matrix<double, _nx, 1> Vectornd;
    typedef Matrix<double, _nu, 1> Vectorcd;

    
    //   bool operator()(const double* const s, double* residual) const {
    bool operator()(double const* const* parameters, double* residuals) const {

      const double *s = parameters[0];
      for (int i = 0, k = 0; k < docp.us.size(); ++k) {
        memcpy(docp.us[k].data(), s + i, docp.sys.U.n*sizeof(double));
        i += docp.sys.U.n;
      }
      docp.Update(false);

      Vectorcd ru;
      Vectornd rx;
      int i = 0;
      for (int k = 0; k < docp.us.size(); ++k) {
        const double &t = docp.ts[k];
        double h = docp.ts[k+1] - t;
        const T& x = docp.xs[k];
        const Vectorcd& u = docp.us[k];

        ((LqCost<T, _nx, _nu>&)docp.cost).ResX(rx, t, x, h);
        memcpy(residuals + i, rx.data(), rx.size()*sizeof(double));
        i += rx.size();

        ((LqCost<T, _nx, _nu>&)docp.cost).ResU(ru, t, u, h);
        memcpy(residuals + i, ru.data(), ru.size()*sizeof(double));
        i += ru.size();
      }
      if (docp.ts.size() > 1) {
        double h = docp.ts.back() - docp.ts[docp.ts.size() - 2];
        ((LqCost<T, _nx, _nu>&)docp.cost).ResX(rx, docp.ts.back(), docp.xs.back(), h);
        memcpy(residuals + i, rx.data(), rx.size()*sizeof(double));
      }
      return true;
    }
  };
#endif

  using namespace std;
  using namespace Eigen;
  
  template <typename T, int _nx, int _nu, int _np, int _ng, int _ntp> 
    GnDocp<T, _nx, _nu, _np, _ng, _ntp>::GnDocp(System<T, _nx, _nu, _np> &sys, 
                                                LqCost<T, _nx, _nu, _np, _ng> &cost,
                                                Tparam<T, _nx, _nu, _np, _ntp> &tparam,
                                                vector<double> &ts, 
                                                vector<T> &xs, 
                                                vector<Matrix<double, _nu, 1> > &us,
                                                Matrix<double, _np, 1> *p,
                                                bool update) : 
    Docp<T, _nx, _nu, _np>(sys, cost, ts, xs, us, p, false), tparam(tparam),
    inputs(tparam.ntp),
    values(cost.ng*xs.size()), s(inputs), 
    functor(0), numDiff(0), lm(0), 
#ifndef USE_SAMPLE_NUMERICAL_DIFF
    numdiff_stepsize(1e-8)
#else
    numdiff_stepsize(1e-4)
#endif
    {
      if(update)
        this->Update(false);//No need of derivatives

      tparam.To(s, this->ts, this->xs, this->us, this->p);

      cout <<"ntp=" <<tparam.ntp << endl;
    }
  
  template <typename T, int _nx, int _nu, int _np, int _ng, int _ntp> 
    GnDocp<T, _nx, _nu, _np, _ng, _ntp>::~GnDocp()
    {
      delete lm;
      delete numDiff;
      delete functor;
    }  
  
  template <typename T, int _nx, int _nu, int _np, int _ng, int _ntp> 
    void GnDocp<T, _nx, _nu, _np, _ng, _ntp>::Iterate() {

    if (!lm) {
      functor = new GnCost<T, _nx, _nu, _np, _ng, _ntp>(inputs, values);
      functor->docp = this;
#ifndef USE_SAMPLE_NUMERICAL_DIFF
      numDiff = new NumericalDiff<GnCost<T, _nx, _nu, _np, _ng, _ntp> >(*functor,numdiff_stepsize);
      lm = new LevenbergMarquardt<NumericalDiff<GnCost<T, _nx, _nu, _np, _ng, _ntp> > >(*numDiff);
#else 
      numDiff = new SampleNumericalDiff<GnCost<T, _nx, _nu, _np, _ng, _ntp> >(*functor,numdiff_stepsize);
      lm = new LevenbergMarquardt<SampleNumericalDiff<GnCost<T, _nx, _nu, _np, _ng, _ntp> > >(*numDiff);
#endif
      lm->parameters.maxfev = 1e6;//Maximum nof evaluations is very high
      info = lm->minimizeInit(s);
      cout <<"info="<<info <<endl;
    }

    /*
    for (int i = 0, k = 0; k < this->us.size(); ++k) {
      memcpy(s.data() + i, this->us[k].data(), this->sys.U.n*sizeof(double));
      i += this->sys.U.n;
    }
    */

    //    lm.parameters.maxfev=10000;
    info = lm->minimizeOneStep(s);
    
    cout <<"info="<<info <<endl;
    // check return values
    // VERIFY_IS_EQUAL(info, 1);
    //   VERIFY_IS_EQUAL(lm.nfev(), 26);
    
#ifdef GCOP_GNDOCP_CERES

    //  google::InitGoogleLogging(argv[0]);

  int npar = this->sys.U.n*this->us.size();
  int nres = ((LqCost<T, _nx, _nu, _np, _ng>&)cost).gn*this->xs.size();

  // The variable to solve for with its initial value. It will be
  // mutated in place by the solver.
  double s[npar];
  for (int i = 0, k = 0; k < this->us.size(); ++k) {
    memcpy(s + i, this->us[k].data(), this->sys.U.n*sizeof(double));
    i += this->sys.U.n;
  }

  //  const double initial_x = x;

  // Build the problem.
  Problem problem;

  //  CostFunction* cost_function =
  //    new NumericDiffCostFunction<CostFunctor, CENTRAL, nres, _npar> (new CostFunctor);
  //  problem.AddResidualBlock(cost_function, NULL, &x);
  
  DynamicNumericDiffCostFunction<GnCost<T,n,c> > *cost_function = new DynamicNumericDiffCostFunction<GnCost<T,n,c> >(new GnCost<T,n,c>(*this));
  cost_function->AddParameterBlock(npar);
  cost_function->SetNumResiduals(nres);

  problem.AddResidualBlock(cost_function, NULL, s);

  // Run the solver!
  Solver::Options options;

  options.linear_solver_type = ceres::DENSE_QR;
  //  options.num_threads = 8;
  options.minimizer_progress_to_stdout = true;
  options.max_num_iterations = 1;
  Solver::Summary summary;
  Solve(options, &problem, &summary);

  std::cout << summary.BriefReport() << "\n";
  //  std::cout << "x : " << initial_x
  #endif
    
  }
}

#endif