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(); }
void DirectMultipleShootingInternal::init(){ // Initialize the base classes OCPSolverInternal::init(); // Create an integrator instance integratorCreator integrator_creator = getOption("integrator"); integrator_ = integrator_creator(ffcn_,Function()); 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_*(nk_+1) + // local 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 (states at time i=1) // nu x 1 (controls in interval i=1) // ------ // ..... // ------ // nx x 1 (states at time i=nk) MX V = MX::sym("V",NV); // Global parameters MX P = V(Slice(0,np_)); // offset in the variable vector int v_offset=np_; // Disretized variables for each shooting node vector<MX> X(nk_+1), U(nk_); for(int k=0; k<=nk_; ++k){ // interior nodes // Local state X[k] = V[Slice(v_offset,v_offset+nx_)]; v_offset += nx_; // Variables below do not appear at the end point if(k==nk_) break; // Local control U[k] = V[Slice(v_offset,v_offset+nu_)]; v_offset += nu_; } // Make sure that the size of the variable vector is consistent with the number of variables that we have referenced casadi_assert(v_offset==NV); // Input to the parallel integrator evaluation vector<vector<MX> > int_in(nk_); for(int k=0; k<nk_; ++k){ int_in[k].resize(INTEGRATOR_NUM_IN); int_in[k][INTEGRATOR_P] = vertcat(P,U[k]); int_in[k][INTEGRATOR_X0] = X[k]; } // Input to the parallel function evaluation vector<vector<MX> > fcn_in(nk_); for(int k=0; k<nk_; ++k){ fcn_in[k].resize(DAE_NUM_IN); fcn_in[k][DAE_T] = (k*tf_)/nk_; fcn_in[k][DAE_P] = vertcat(P,U.at(k)); fcn_in[k][DAE_X] = X[k]; } // Options for the parallelizer Dictionary paropt; // Transmit parallelization mode if(hasSetOption("parallelization")) paropt["parallelization"] = getOption("parallelization"); // Evaluate function in parallel vector<vector<MX> > pI_out = integrator_.callParallel(int_in,paropt); // Evaluate path constraints in parallel vector<vector<MX> > pC_out; if(path_constraints) pC_out = cfcn_.callParallel(fcn_in,paropt); //Constraint function vector<MX> gg(2*nk_); // Collect the outputs for(int k=0; k<nk_; ++k){ //append continuity constraints gg[2*k] = pI_out[k][INTEGRATOR_XF] - X[k+1]; // append the path constraints if(path_constraints) gg[2*k+1] = pC_out[k][0]; } // Terminal constraints MX g = vertcat(gg); // Objective function MX f; if (mfcn_.getNumInputs()==1) { f = mfcn_(X.back()).front(); } else { vector<MX> mfcn_argin(MAYER_NUM_IN); mfcn_argin[MAYER_X] = X.back(); mfcn_argin[MAYER_P] = P; f = mfcn_.call(mfcn_argin).front(); } // NLP nlp_ = MXFunction(nlpIn("x",V),nlpOut("f",f,"g",g)); nlp_.setOption("ad_mode","forward"); nlp_.init(); // Get the NLP creator function NLPSolverCreator nlp_solver_creator = getOption("nlp_solver"); // Allocate an NLP solver nlp_solver_ = nlp_solver_creator(nlp_); // Pass user 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(); }