void QpToImplicit::init() { // Call the base class initializer ImplicitFunctionInternal::init(); // Free variable in the NLP MX u = MX::sym("u", input(iin_).sparsity()); // So that we can pass it on to createParent std::vector<Sparsity> sps; for (int i=0; i<getNumInputs(); ++i) if (i!=iin_) sps.push_back(input(i).sparsity()); // u groups all parameters in an MX std::vector< MX > inputs; MX p = createParent(sps, inputs); // Dummy NLP objective MX nlp_f = 0; // NLP constraints std::vector< MX > args_call(getNumInputs()); args_call[iin_] = u; for (int i=0, i2=0; i<getNumInputs(); ++i) if (i!=iin_) args_call[i] = inputs[i2++]; MX nlp_g = f_.call(args_call).at(iout_); // We're going to use two-argument objective and constraints to allow the use of parameters MXFunction nlp(nlpIn("x", u, "p", p), nlpOut("f", nlp_f, "g", nlp_g)); // Create an NlpSolver instance solver_ = NlpSolver(getOption(solvername()), nlp); if (hasSetOption(optionsname())) solver_.setOption(getOption(optionsname())); solver_.init(); }
NlpSolver::NlpSolver(const std::string& name, const std::string& solver, const MXDict& nlp, const Dict& opts) { // Create an NLP function MX x, p, f, g; for (MXDict::const_iterator i=nlp.begin(); i!=nlp.end(); ++i) { if (i->first=="x") { x = i->second; } else if (i->first=="p") { p = i->second; } else if (i->first=="f") { f = i->second; } else if (i->first=="g") { g = i->second; } else { casadi_error("No such field: \"" + i->first + "\""); } } MXFunction nlpf("nlp", nlpIn("x", x, "p", p), nlpOut("f", f, "g", g)); // Create the solver instance assignNode(NlpSolverInternal::instantiatePlugin(solver, nlpf)); setOption("name", name); setOption(opts); init(); }
RealtimeAPCSCP::RealtimeAPCSCP(Model* model) : RealtimeAPCSCP::Solver(model) { this->nlp = MXFunction("nlp", nlpIn("x", model->getV()), nlpOut("f", model->getf(0), "g", G)); this->dG_f = this->nlp.jacobian("x", "g"); this->dG_fast = this->nlp.derReverse(1); this->choiceH = 0; this->exactA = true; this->n_updateA = 1; this->n_updateH = 1; this->assumedData = std::vector<Matrix<double> >(this->model->getN_F() + 1); this->firstIteration = true; this->name = "APCSCP"; this->hess_gi = std::vector<Function>(G.size()); for (int i = 0; i < G.size(); i++) { hess_gi[i] = MXFunction("constraint_i", nlpIn("x", model->getV()), nlpOut("g", (MX) G[i])).hessian("x", "g"); } hess_fi = nlp.hessian("x", "f"); }
void QpToImplicit::init() { // Call the base class initializer ImplicitFunctionInternal::init(); // Free variable in the NLP MX u = MX::sym("u", input(iin_).sparsity()); // So that we can pass it on to createParent std::vector<MX> inputs; for (int i=0; i<nIn(); ++i) { if (i!=iin_) { stringstream ss; ss << "p" << i; inputs.push_back(MX::sym(ss.str(), input(i).sparsity())); } } MX p = veccat(inputs); // Dummy NLP objective MX nlp_f = 0; // NLP constraints std::vector< MX > args_call(nIn()); args_call[iin_] = u; for (int i=0, i2=0; i<nIn(); ++i) if (i!=iin_) args_call[i] = inputs[i2++]; MX nlp_g = f_(args_call).at(iout_); // We're going to use two-argument objective and constraints to allow the use of parameters MXFunction nlp("nlp", nlpIn("x", u, "p", p), nlpOut("f", nlp_f, "g", nlp_g)); Dict options; if (hasSetOption(optionsname())) options = getOption(optionsname()); // Create an NlpSolver instance solver_ = NlpSolver("nlpsolver", getOption(solvername()), nlp, options); }
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(); }
void DirectCollocationInternal::init(){ // Initialize the base classes OCPSolverInternal::init(); // Free parameters currently not supported casadi_assert_message(np_==0, "Not implemented"); // Interpolation order deg_ = getOption("interpolation_order"); // All collocation time points std::vector<double> tau_root = collocationPoints(deg_,getOption("collocation_scheme")); // 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::zeros(deg_+1,deg_+1); // Coefficients of the continuity equation vector<MX> D(deg_+1); // Coefficients of the collocation equation as DMatrix DMatrix D_num = DMatrix::zeros(deg_+1); // Collocation point SX tau = SX::sym("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 SX 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 Function tfcn = lfcn.tangent(); tfcn.init(); for(int j2=0; j2<deg_+1; ++j2){ tfcn.setInput(tau_root[j2]); tfcn.evaluate(); C[j][j2] = tfcn.output(); C_num(j,j2) = tfcn.output(); } } 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 = MX::sym("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 nlp_ = MXFunction(nlpIn("x",nlp_x), nlpOut("f",nlp_j,"g",vertcat(nlp_g))); // Get the NLP creator function NLPSolverCreator nlp_solver_creator = getOption("nlp_solver"); // Allocate an NLP solver nlp_solver_ = nlp_solver_creator(nlp_); // 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 RealtimeAPCSCP::solve() { //Initialisation (calculate the exact solution for inital point) int t_act = 0; std::cout << "Time: " + to_string(t_act) << std::endl; startIt = std::chrono::system_clock::now(); startSolver = std::chrono::system_clock::now(); //Solve the first problem exactly std::map<std::string, Matrix<double> > arg = make_map("lbx", model->getVMIN(), "ubx", model->getVMAX(), "lbg", this->G_bound, "ubg", this->G_bound, "x0", model->getVINIT()); NlpSolver nlpSolver = NlpSolver("solver", "ipopt", nlp, opts); nlpSolver.setOption("warn_initial_bounds", true); nlpSolver.setOption("eval_errors_fatal", true); std::map<string, Matrix<double>> result_tact = nlpSolver(arg); endSolver = std::chrono::system_clock::now(); // Store data assumedData[t_act] = result_tact["x"]; //Setup new iteration model->storeAndShiftValues(result_tact, t_act); storeStatsIpopt(&nlpSolver); //Define values this->x_act = result_tact["x"]; this->y_act = result_tact["lam_g"]; evaluateOriginalF(t_act,x_act); updateA_act(t_act); updateH_act(t_act); updateM_act(); m_act = mul(transpose(Dg_act - A_act), y_act); firstIteration = false; opts["warm_start_init_point"] = "yes"; endIt = std::chrono::system_clock::now(); printTimingData(t_act); //Iteration for (t_act = 1; t_act < model->getN_F(); t_act++) { std::cout << "Time: " + to_string(t_act) << std::endl; startIt = std::chrono::system_clock::now(); startMS = std::chrono::system_clock::now(); G_sub = mul(A_act, model->getV() - x_act) + model->doMultipleShooting(x_act); endMS = std::chrono::system_clock::now(); f_sub = model->getf(t_act) + mul(transpose(m_act), model->getV() - x_act) + 0.5 * quad_form(model->getV() - x_act, H_act); this->nlp_sub = MXFunction("nlp", nlpIn("x", model->getV()), nlpOut( "f", f_sub, "g", G_sub)); this->nlp = MXFunction("nlp", nlpIn("x", model->getV()), nlpOut("f", model->getf(t_act), "g", G)); // Step2 solve convex subproblem startSolver = std::chrono::system_clock::now(); result_tact = solveConvexSubproblem(); endSolver = std::chrono::system_clock::now(); // Step3 update matrices and retrieve new measurement (step 1) x_act = result_tact["x"]; y_act = result_tact["lam_g"]; model->storeAndShiftValues(result_tact, t_act); // update this->x_act = result_tact["x"]; this->y_act = result_tact["lam_g"]; evaluateOriginalF(t_act, x_act); updateA_act(t_act); updateH_act(t_act); updateM_act(); endIt = std::chrono::system_clock::now(); printTimingData(t_act); } }