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(); }
std::map<string, Matrix<double> > RealtimeAPCSCP::solveConvexSubproblem() { 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()); if (!firstIteration) { arg.insert(std::pair<std::string, Matrix<double> >("lam_g0", model->getLAM_G())); arg.insert(std::pair<std::string, Matrix<double> >("lam_x0", model->getLAM_X())); } NlpSolver nlpSolver = NlpSolver("solver", "ipopt", nlp_sub, opts); nlpSolver.setOption("warn_initial_bounds", true); nlpSolver.setOption("eval_errors_fatal", true); std::map<string, Matrix<double> > result = nlpSolver(arg); storeStatsIpopt(&nlpSolver); return result; }
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); }
NlpSolver DirectCollocation::getNlpSolver() const { return isNull() ? NlpSolver(): (*this)->nlp_solver_; }
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); } }
void DirectMultipleShootingInternal::init() { // Initialize the base classes OCPSolverInternal::init(); // Create an integrator instance std::string integrator_name = getOption("integrator"); integrator_ = Integrator(integrator_name, 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 std::string nlp_solver_name = getOption("nlp_solver"); // Allocate an NLP solver nlp_solver_ = NlpSolver(nlp_solver_name, 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(); }