void DirectCollocationInternal::init(){
  // Initialize the base classes
  OCPSolverInternal::init();
  
  // Free parameters currently not supported
  casadi_assert_message(np_==0, "Not implemented");

  // Legendre collocation points
  double legendre_points[][6] = {
    {0},
    {0,0.500000},
    {0,0.211325,0.788675},
    {0,0.112702,0.500000,0.887298},
    {0,0.069432,0.330009,0.669991,0.930568},
    {0,0.046910,0.230765,0.500000,0.769235,0.953090}};

  // Radau collocation points
  double radau_points[][6] = {
    {0},
    {0,1.000000},
    {0,0.333333,1.000000},
    {0,0.155051,0.644949,1.000000},
    {0,0.088588,0.409467,0.787659,1.000000},
    {0,0.057104,0.276843,0.583590,0.860240,1.000000}};

  // Read options
  bool use_radau;
  if(getOption("collocation_scheme")=="radau"){
    use_radau = true;
  } else if(getOption("collocation_scheme")=="legendre"){
    use_radau = false;
  }

  // Interpolation order
  deg_ = getOption("interpolation_order");

  // All collocation time points
  double* tau_root = use_radau ? radau_points[deg_] : legendre_points[deg_];

  // Size of the finite elements
  double h = tf_/nk_;

  // Coefficients of the collocation equation
  vector<vector<MX> > C(deg_+1,vector<MX>(deg_+1));

  // Coefficients of the collocation equation as DMatrix
  DMatrix C_num = DMatrix(deg_+1,deg_+1,0);

  // Coefficients of the continuity equation
  vector<MX> D(deg_+1);

  // Coefficients of the collocation equation as DMatrix
  DMatrix D_num = DMatrix(deg_+1,1,0);

  // Collocation point
  SXMatrix tau = ssym("tau");

  // For all collocation points
  for(int j=0; j<deg_+1; ++j){
    // Construct Lagrange polynomials to get the polynomial basis at the collocation point
    SXMatrix L = 1;
    for(int j2=0; j2<deg_+1; ++j2){
      if(j2 != j){
        L *= (tau-tau_root[j2])/(tau_root[j]-tau_root[j2]);
      }
    }

    SXFunction lfcn(tau,L);
    lfcn.init();

    // Evaluate the polynomial at the final time to get the coefficients of the continuity equation
    lfcn.setInput(1.0);
    lfcn.evaluate();
    D[j] = lfcn.output();
    D_num(j) = lfcn.output();

    // Evaluate the time derivative of the polynomial at all collocation points to get the coefficients of the continuity equation
    for(int j2=0; j2<deg_+1; ++j2){
      lfcn.setInput(tau_root[j2]);
      lfcn.setFwdSeed(1.0);
      lfcn.evaluate(1,0);
      C[j][j2] = lfcn.fwdSens();
      C_num(j,j2) = lfcn.fwdSens();
    }
  }

  C_num(std::vector<int>(1,0),ALL) = 0;
  C_num(0,0)   = 1;

  // All collocation time points
  vector<vector<double> > T(nk_);
  for(int k=0; k<nk_; ++k){
	  T[k].resize(deg_+1);
	  for(int j=0; j<=deg_; ++j){
		  T[k][j] = h*(k + tau_root[j]);
	  }
  }

  // Total number of variables
  int nlp_nx = 0;
  nlp_nx += nk_*(deg_+1)*nx_;   // Collocated states
  nlp_nx += nk_*nu_;            // Parametrized controls
  nlp_nx += nx_;               	// Final state

  // NLP variable vector
  MX nlp_x = msym("x",nlp_nx);
  int offset = 0;

  // Get collocated states and parametrized control
  vector<vector<MX> > X(nk_+1);
  vector<MX> U(nk_);
  for(int k=0; k<nk_; ++k){
    // Collocated states
	X[k].resize(deg_+1);
    for(int j=0; j<=deg_; ++j){
        // Get the expression for the state vector
        X[k][j] = nlp_x[Slice(offset,offset+nx_)];
        offset += nx_;
    }

    // Parametrized controls
    U[k] = nlp_x[Slice(offset,offset+nu_)];
    offset += nu_;
  }

  // State at end time
  X[nk_].resize(1);
  X[nk_][0] = nlp_x[Slice(offset,offset+nx_)];
  offset += nx_;
  casadi_assert(offset==nlp_nx);

  // Constraint function for the NLP
  vector<MX> nlp_g;

  // Objective function
  MX nlp_j = 0;

  // For all finite elements
  for(int k=0; k<nk_; ++k){

    // For all collocation points
    for(int j=1; j<=deg_; ++j){

        // Get an expression for the state derivative at the collocation point
        MX xp_jk = 0;
        for(int r=0; r<=deg_; ++r){
            xp_jk += C[r][j]*X[k][r];
        }

        // Add collocation equations to the NLP
        MX fk = ffcn_.call(daeIn("x",X[k][j],"p",U[k]))[DAE_ODE];
        nlp_g.push_back(h*fk - xp_jk);
    }

    // Get an expression for the state at the end of the finite element
    MX xf_k = 0;
    for(int r=0; r<=deg_; ++r){
        xf_k += D[r]*X[k][r];
    }

    // Add continuity equation to NLP
    nlp_g.push_back(X[k+1][0] - xf_k);

    // Add path constraints
    if(nh_>0){
      MX pk = cfcn_.call(daeIn("x",X[k+1][0],"p",U[k])).at(0);
      nlp_g.push_back(pk);
    }

    // Add integral objective function term
	//    [Jk] = lfcn.call([X[k+1,0], U[k]])
	//    nlp_j += Jk
  }

  // Add end cost
  MX Jk = mfcn_.call(mayerIn("x",X[nk_][0])).at(0);
  nlp_j += Jk;

  // Objective function of the NLP
  F_ = MXFunction(nlp_x, nlp_j);

  // Nonlinear constraint function
  G_ = MXFunction(nlp_x, vertcat(nlp_g));

  // Get the NLP creator function
  NLPSolverCreator nlp_solver_creator = getOption("nlp_solver");
  
  // Allocate an NLP solver
  nlp_solver_ = nlp_solver_creator(F_,G_,FX(),FX());
  
  // Pass options
  if(hasSetOption("nlp_solver_options")){
    const Dictionary& nlp_solver_options = getOption("nlp_solver_options");
    nlp_solver_.setOption(nlp_solver_options);
  }
  
  // Initialize the solver
  nlp_solver_.init();
}
Example #2
0
 template<> inline
 MX GenericMatrix<MX>::sym(const std::string& name, const CRSSparsity& sp){ return msym(name,sp);}
void DirectSingleShootingInternal::init(){
  // Initialize the base classes
  OCPSolverInternal::init();

  // Create an integrator instance
  integratorCreator integrator_creator = getOption("integrator");
  integrator_ = integrator_creator(ffcn_,FX());
  if(hasSetOption("integrator_options")){
    integrator_.setOption(getOption("integrator_options"));
  }

  // Set t0 and tf
  integrator_.setOption("t0",0);
  integrator_.setOption("tf",tf_/nk_);
  integrator_.init();
  
  // Path constraints present?
  bool path_constraints = nh_>0;
  
  // Count the total number of NLP variables
  int NV = np_ + // global parameters
           nx_ + // initial state
           nu_*nk_; // local control
           
  // Declare variable vector for the NLP
  // The structure is as follows:
  // np x 1  (parameters)
  // ------
  // nx x 1  (states at time i=0)
  // ------
  // nu x 1  (controls in interval i=0)
  // .....
  // nx x 1  (controls in interval i=nk-1)
  
  MX V = msym("V",NV);
  int offset = 0;

  // Global parameters
  MX P = V[Slice(0,np_)];
  offset += np_;

  // Initial state
  MX X0 = V[Slice(offset,offset+nx_)];
  offset += nx_;
  
  // Control for each shooting interval
  vector<MX> U(nk_);
  for(int k=0; k<nk_; ++k){ // interior nodes
    U[k] = V[range(offset,offset+nu_)];
    offset += nu_;
  }
  
  // Make sure that the size of the variable vector is consistent with the number of variables that we have referenced
  casadi_assert(offset==NV);

  // Current state
  MX X = X0;

  // Objective
  MX nlp_j = 0;

  // Constraints
  vector<MX> nlp_g;
  nlp_g.reserve(nk_*(path_constraints ? 2 : 1));
  
  // For all shooting nodes
  for(int k=0; k<nk_; ++k){
    // Integrate
    vector<MX> int_out = integrator_.call(integratorIn("x0",X,"p",vertcat(P,U[k])));

    // Store expression for state trajectory
    X = int_out[INTEGRATOR_XF];
    
    // Add constraints on the state
    nlp_g.push_back(X);

    // Add path constraints
    if(path_constraints){
      vector<MX> cfcn_out = cfcn_.call(daeIn("x",X,"p",U[k])); // TODO: Change signature of cfcn_: remove algebraic variable, add control
      nlp_g.push_back(cfcn_out.at(0));
    }
  }

  // Terminal constraints
  G_ = MXFunction(V,vertcat(nlp_g));
  G_.setOption("name","nlp_g");
  G_.init();
  
  // Objective function
  MX jk = mfcn_.call(mayerIn("x",X,"p",P)).at(0);
  nlp_j += jk;
  F_ = MXFunction(V,nlp_j);
  F_.setOption("name","nlp_j");
  
  // Get the NLP creator function
  NLPSolverCreator nlp_solver_creator = getOption("nlp_solver");
  
  // Allocate an NLP solver
  nlp_solver_ = nlp_solver_creator(F_,G_,FX(),FX());
  
  // Pass options
  if(hasSetOption("nlp_solver_options")){
    const Dictionary& nlp_solver_options = getOption("nlp_solver_options");
    nlp_solver_.setOption(nlp_solver_options);
  }
  
  // Initialize the solver
  nlp_solver_.init();
}