void LrDpleToDple::init() { // Initialize the base classes LrDpleInternal::init(); MX As = MX::sym("As", input(LR_DPLE_A).sparsity()); MX Vs = MX::sym("Vs", input(LR_DPLE_V).sparsity()); MX Cs = MX::sym("Cs", input(LR_DPLE_C).sparsity()); MX Hs = MX::sym("Hs", input(LR_DPLE_H).sparsity()); int n_ = A_[0].size1(); // Chop-up the arguments std::vector<MX> As_ = horzsplit(As, n_); std::vector<MX> Vs_ = horzsplit(Vs, V_[0].size2()); std::vector<MX> Cs_ = horzsplit(Cs, V_[0].size2()); std::vector<MX> Hss_ = horzsplit(Hs, Hsi_); std::vector<MX> V_(Vs_.size()); for (int k=0;k<V_.size();++k) { V_[k] = mul(Cs_[k], mul(Vs_[k], Cs_[k].T())); } std::vector<Sparsity> Vsp(Vs_.size()); for (int k=0;k<V_.size();++k) { Vsp[k] = V_[k].sparsity(); } // Solver options Dict options; if (hasSetOption(optionsname())) { options = getOption(optionsname()); } // Create an dplesolver instance std::map<std::string, std::vector<Sparsity> > tmp; tmp["a"] = A_; tmp["v"] = Vsp; solver_ = DpleSolver("solver", getOption(solvername()), tmp, options); MX P = solver_(make_map("a", horzcat(As_), "v", horzcat(V_))).at("p"); std::vector<MX> Ps_ = horzsplit(P, n_); std::vector<MX> HPH(K_); for (int k=0;k<K_;++k) { std::vector<MX> hph = horzsplit(Hss_[k], cumsum0(Hs_[k])); for (int kk=0;kk<hph.size();++kk) { hph[kk] = mul(hph[kk].T(), mul(Ps_[k], hph[kk])); } HPH[k] = diagcat(hph); } f_ = MXFunction(name_, lrdpleIn("a", As, "v", Vs, "c", Cs, "h", Hs), lrdpleOut("y", horzcat(HPH))); Wrapper<LrDpleToDple>::checkDimensions(); }
void CondensingIndefDpleInternal::init() { // Initialize the base classes DpleInternal::init(); casadi_assert_message(!pos_def_, "pos_def option set to True: Solver only handles the indefinite case."); casadi_assert_message(const_dim_, "const_dim option set to False: Solver only handles the True case."); n_ = A_[0].size1(); MX As = MX::sym("A", horzcat(A_)); MX Vs = MX::sym("V", horzcat(V_)); std::vector< MX > Vss = horzsplit(Vs, n_); std::vector< MX > Ass = horzsplit(As, n_); for (int k=0;k<K_;++k) { Vss[k] = (Vss[k]+Vss[k].T())/2; } MX R = MX::zeros(n_, n_); for (int k=0;k<K_;++k) { R = mul(mul(Ass[k], R), Ass[k].T()) + Vss[k]; } std::vector< MX > Assr(K_); std::reverse_copy(Ass.begin(), Ass.end(), Assr.begin()); MX Ap = mul(Assr); // Create an dlesolver instance solver_ = DleSolver(getOption(solvername()), dleStruct("a", Ap.sparsity(), "v", R.sparsity())); solver_.setOption(getOption(optionsname())); // Initialize the NLP solver solver_.init(); std::vector<MX> Pr = solver_.call(dpleIn("a", Ap, "v", R)); std::vector<MX> Ps(K_); Ps[0] = Pr[0]; for (int k=0;k<K_-1;++k) { Ps[k+1] = mul(mul(Ass[k], Ps[k]), Ass[k].T()) + Vss[k]; } f_ = MXFunction(dpleIn("a", As, "v", Vs), dpleOut("p", horzcat(Ps))); f_.init(); Wrapper::checkDimensions(); }
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"); }
Function SwitchInternal ::getDerReverse(const std::string& name, int nadj, Dict& opts) { // Derivative of each case vector<Function> der(f_.size()); for (int k=0; k<f_.size(); ++k) { if (!f_[k].isNull()) der[k] = f_[k].derReverse(nadj); } // Default case Function der_def; if (!f_def_.isNull()) der_def = f_def_.derReverse(nadj); // New Switch for derivatives stringstream ss; ss << "adj" << nadj << "_" << name_; Switch sw(ss.str(), der, der_def); // Construct wrapper inputs and arguments for calling sw vector<MX> arg = symbolicInput(); vector<MX> res = symbolicOutput(); vector<vector<MX> > seed = symbolicAdjSeed(nadj, res); vector<MX> w_in = arg; w_in.insert(w_in.end(), res.begin(), res.end()); // Arguments for calling sw being constructed vector<MX> v; v.push_back(arg.at(0)); // index for (int d=0; d<nadj; ++d) { // Add to wrapper input w_in.insert(w_in.end(), seed[d].begin(), seed[d].end()); // Add to sw argument vector v.insert(v.end(), arg.begin()+1, arg.end()); v.insert(v.end(), res.begin(), res.end()); v.insert(v.end(), seed[d].begin(), seed[d].end()); } // Construct wrapper outputs casadi_assert(v.size()==sw.nIn()); v = sw(v); vector<MX> w_out; MX ind_sens = MX(1, 1); // no dependency on index vector<MX>::const_iterator v_it = v.begin(), v_it_next; for (int d=0; d<nadj; ++d) { w_out.push_back(ind_sens); v_it_next = v_it + (nIn()-1); w_out.insert(w_out.end(), v_it, v_it_next); v_it = v_it_next; } // Create wrapper return MXFunction(name, w_in, w_out, opts); }
void SimpleIndefDpleInternal::init() { DpleInternal::init(); casadi_assert_message(!pos_def_, "pos_def option set to True: Solver only handles the indefinite case."); casadi_assert_message(const_dim_, "const_dim option set to False: Solver only handles the True case."); n_ = A_[0].size1(); MX As = MX::sym("A", n_, K_*n_); MX Vs = MX::sym("V", n_, K_*n_); std::vector< MX > Vss = horzsplit(Vs, n_); std::vector< MX > Ass = horzsplit(As, n_); for (int k=0;k<K_;++k) { Vss[k]=(Vss[k]+Vss[k].T())/2; } std::vector< MX > AA_list(K_); for (int k=0;k<K_;++k) { AA_list[k] = kron(Ass[k], Ass[k]); } MX AA = blkdiag(AA_list); MX A_total = DMatrix::eye(n_*n_*K_) - vertcat(AA(range(K_*n_*n_-n_*n_, K_*n_*n_), range(K_*n_*n_)), AA(range(K_*n_*n_-n_*n_), range(K_*n_*n_))); std::vector<MX> Vss_shift; Vss_shift.push_back(Vss.back()); Vss_shift.insert(Vss_shift.end(), Vss.begin(), Vss.begin()+K_-1); MX Pf = solve(A_total, vec(horzcat(Vss_shift)), getOption("linear_solver")); MX P = reshape(Pf, n_, K_*n_); std::vector<MX> v_in; v_in.push_back(As); v_in.push_back(Vs); f_ = MXFunction(v_in, P); f_.setInputScheme(SCHEME_DPLEInput); f_.setOutputScheme(SCHEME_DPLEOutput); f_.init(); }
FX ParallelizerInternal::getDerivative(int nfwd, int nadj){ // Generate derivative expressions vector<FX> der_funcs(funcs_.size()); for(int i=0; i<funcs_.size(); ++i){ der_funcs[i] = funcs_[i].derivative(nfwd,nadj); } // Create a new parallelizer for the derivatives Parallelizer par(der_funcs); // Set options and initialize par.setOption(dictionary()); par.init(); // Create a function call to the parallelizer vector<MX> par_arg = par.symbolicInput(); vector<MX> par_res = par.call(par_arg); // Get the offsets: copy to allow being used as a counter std::vector<int> par_inind = par->inind_; std::vector<int> par_outind = par->outind_; // Arguments and results of the return function vector<MX> ret_arg, ret_res; ret_arg.reserve(par_arg.size()); ret_res.reserve(par_res.size()); // Loop over all nondifferentiated inputs/outputs and forward seeds/sensitivities for(int dir=-1; dir<nfwd; ++dir){ for(int i=0; i<funcs_.size(); ++i){ for(int j=inind_[i]; j<inind_[i+1]; ++j) ret_arg.push_back(par_arg[par_inind[i]++]); for(int j=outind_[i]; j<outind_[i+1]; ++j) ret_res.push_back(par_res[par_outind[i]++]); } } // Loop over adjoint seeds/sensitivities for(int dir=0; dir<nadj; ++dir){ for(int i=0; i<funcs_.size(); ++i){ for(int j=outind_[i]; j<outind_[i+1]; ++j) ret_arg.push_back(par_arg[par_inind[i]++]); for(int j=inind_[i]; j<inind_[i+1]; ++j) ret_res.push_back(par_res[par_outind[i]++]); } } // Assemble the return function return MXFunction(ret_arg,ret_res); }
void FixedSmithLrDleInternal::init() { iter_ = getOption("iter"); LrDleInternal::init(); casadi_assert_message(!pos_def_, "pos_def option set to True: Solver only handles the indefinite case."); MX H = MX::sym("H", H_); MX A = MX::sym("A", A_); MX C = MX::sym("C", C_); MX V = MX::sym("V", V_); MX Vs = (V+V.T())/2; MX D = with_C_ ? C : DMatrix::eye(A_.size1()); std::vector<MX> HPH(Hs_.size(), 0); std::vector<MX> Hs = with_H_? horzsplit(H, Hi_) : std::vector<MX>(); MX out = 0; for (int i=0;i<iter_;++i) { if (with_H_) { for (int k=0;k<Hs.size();++k) { MX v = mul(D.T(), Hs[k]); HPH[k]+= mul(v.T(), mul(Vs, v)); } } else { out += mul(D, mul(Vs, D.T())); } D = mul(A, D); } std::vector<MX> dle_in(LR_DLE_NUM_IN); dle_in[LR_DLE_A] = A; dle_in[LR_DLE_V] = V; if (with_C_) dle_in[LR_DLE_C] = C; if (with_H_) dle_in[LR_DLE_H] = H; f_ = MXFunction(dle_in, lrdleOut("y", with_H_? diagcat(HPH): out)); f_.init(); Wrapper<FixedSmithLrDleInternal>::checkDimensions(); }
Function SwitchInternal ::getDerForward(const std::string& name, int nfwd, Dict& opts) { // Derivative of each case vector<Function> der(f_.size()); for (int k=0; k<f_.size(); ++k) { if (!f_[k].isNull()) der[k] = f_[k].derForward(nfwd); } // Default case Function der_def; if (!f_def_.isNull()) der_def = f_def_.derForward(nfwd); // New Switch for derivatives stringstream ss; ss << "fwd" << nfwd << "_" << name_; Switch sw(ss.str(), der, der_def); // Construct wrapper inputs and arguments for calling sw vector<MX> arg = symbolicInput(); vector<MX> res = symbolicOutput(); vector<vector<MX> > seed = symbolicFwdSeed(nfwd, arg); // Wrapper input being constructed vector<MX> w_in = arg; w_in.insert(w_in.end(), res.begin(), res.end()); // Arguments for calling sw being constructed vector<MX> v; v.insert(v.end(), arg.begin(), arg.end()); v.insert(v.end(), res.begin(), res.end()); for (int d=0; d<nfwd; ++d) { // ignore seed for ind seed[d][0] = MX::sym(seed[d][0].getName(), Sparsity::scalar(false)); // Add to wrapper input w_in.insert(w_in.end(), seed[d].begin(), seed[d].end()); // Add to sw argument vector v.insert(v.end(), seed[d].begin()+1, seed[d].end()); } // Create wrapper casadi_assert(v.size()==sw.nIn()); return MXFunction(name, w_in, sw(v), opts); }
void SimpleIndefDleInternal::init() { DleInternal::init(); casadi_assert_message(!pos_def_, "pos_def option set to True: Solver only handles the indefinite case."); n_ = A_.size1(); MX As = MX::sym("A", A_); MX Vs = MX::sym("V", V_); MX Vss = (Vs+Vs.T())/2; MX A_total = DMatrix::eye(n_*n_) - kron(As, As); MX Pf = solve(A_total, vec(Vss), getOption("linear_solver")); MX P = reshape(Pf, n_, n_); f_ = MXFunction(dleIn("a", As, "v", Vs), dleOut("p", MX(P(output().sparsity())))); f_.init(); casadi_assert(getNumOutputs()==f_.getNumOutputs()); for (int i=0;i<getNumInputs();++i) { casadi_assert_message(input(i).sparsity()==f_.input(i).sparsity(), "Sparsity mismatch for input " << i << ":" << input(i).dimString() << " <-> " << f_.input(i).dimString() << "."); } for (int i=0;i<getNumOutputs();++i) { casadi_assert_message(output(i).sparsity()==f_.output(i).sparsity(), "Sparsity mismatch for output " << i << ":" << output(i).dimString() << " <-> " << f_.output(i).dimString() << "."); } }
MXFunction vec (const FX &a_) { FX a = a_; // Pass null if input is null if (a.isNull()) return MXFunction(); // Get the MX inputs, only used for shape const std::vector<MX> &symbolicInputMX = a.symbolicInput(); // Have a vector with MX that have the shape of vec(symbolicInputMX ) std::vector<MX> symbolicInputMX_vec(a.getNumInputs()); // Make vector valued MX's out of them std::vector<MX> symbolicInputMX_vec_reshape(a.getNumInputs()); // Apply the vec-transformation to the inputs for (int i=0;i<symbolicInputMX.size();++i) { std::stringstream s; s << "X_flat_" << i; symbolicInputMX_vec[i] = MX(s.str(),vec(symbolicInputMX[i].sparsity())); symbolicInputMX_vec_reshape[i] = trans(reshape(symbolicInputMX_vec[i],trans(symbolicInputMX[i].sparsity()))); } // Call the original function with the vecced inputs std::vector<MX> symbolicOutputMX = a.call(symbolicInputMX_vec_reshape); // Apply the vec-transformation to the outputs for (int i=0;i<symbolicOutputMX.size();++i) symbolicOutputMX[i] = vec(symbolicOutputMX[i]); // Make a new function with the vecced input/outputs MXFunction ret(symbolicInputMX_vec,symbolicOutputMX); // Initialize it if a was if (a.isInit()) ret.init(); return ret; }
Function implicitRK(Function& f, const std::string& impl, const Dictionary& impl_options, const MX& tf, int order, const std::string& scheme, int ne) { casadi_assert_message(ne>=1, "Parameter ne (number of elements must be at least 1), " "but got " << ne << "."); casadi_assert_message(order==4, "Only RK order 4 is supported now."); casadi_assert_message(f.getNumInputs()==DAE_NUM_IN && f.getNumOutputs()==DAE_NUM_OUT, "Supplied function must adhere to dae scheme."); casadi_assert_message(f.output(DAE_QUAD).isEmpty(), "Supplied function cannot have quadrature states."); // Obtain collocation points std::vector<double> tau_root = collocationPoints(order, "legendre"); // Retrieve collocation interpolating matrices std::vector < std::vector <double> > C; std::vector < double > D; collocationInterpolators(tau_root, C, D); // Retrieve problem dimensions int nx = f.input(DAE_X).size(); int nz = f.input(DAE_Z).size(); int np = f.input(DAE_P).size(); //Variables for one finite element MX X = MX::sym("X", nx); MX P = MX::sym("P", np); MX V = MX::sym("V", order*(nx+nz)); // Unknowns MX X0 = X; // Components of the unknowns that correspond to states at collocation points std::vector<MX> Xc;Xc.reserve(order); Xc.push_back(X0); // Components of the unknowns that correspond to algebraic states at collocation points std::vector<MX> Zc;Zc.reserve(order); // Splitting the unknowns std::vector<int> splitPositions = range(0, order*nx, nx); if (nz>0) { std::vector<int> Zc_pos = range(order*nx, order*nx+(order+1)*nz, nz); splitPositions.insert(splitPositions.end(), Zc_pos.begin(), Zc_pos.end()); } else { splitPositions.push_back(order*nx); } std::vector<MX> Vs = vertsplit(V, splitPositions); // Extracting unknowns from Z for (int i=0;i<order;++i) { Xc.push_back(X0+Vs[i]); } if (nz>0) { for (int i=0;i<order;++i) { Zc.push_back(Vs[order+i]); } } // Get the collocation Equations (that define V) std::vector<MX> V_eq; // Local start time MX t0_l=MX::sym("t0"); MX h = MX::sym("h"); for (int j=1;j<order+1;++j) { // Expression for the state derivative at the collocation point MX xp_j = 0; for (int r=0;r<order+1;++r) { xp_j+= C[j][r]*Xc[r]; } // Append collocation equations & algebraic constraints std::vector<MX> f_out; MX t_l = t0_l+tau_root[j]*h; if (nz>0) { f_out = f.call(daeIn("t", t_l, "x", Xc[j], "p", P, "z", Zc[j-1])); } else { f_out = f.call(daeIn("t", t_l, "x", Xc[j], "p", P)); } V_eq.push_back(h*f_out[DAE_ODE]-xp_j); V_eq.push_back(f_out[DAE_ALG]); } // Root-finding function, implicitly defines V as a function of X0 and P std::vector<MX> vfcn_inputs; vfcn_inputs.push_back(V); vfcn_inputs.push_back(X); vfcn_inputs.push_back(P); vfcn_inputs.push_back(t0_l); vfcn_inputs.push_back(h); Function vfcn = MXFunction(vfcn_inputs, vertcat(V_eq)); vfcn.init(); try { // Attempt to convert to SXFunction to decrease overhead vfcn = SXFunction(vfcn); vfcn.init(); } catch(CasadiException & e) { // } // Create a implicit function instance to solve the system of equations ImplicitFunction ifcn(impl, vfcn, Function(), LinearSolver()); ifcn.setOption(impl_options); ifcn.init(); // Get an expression for the state at the end of the finite element std::vector<MX> ifcn_call_in(5); ifcn_call_in[0] = MX::zeros(V.sparsity()); std::copy(vfcn_inputs.begin()+1, vfcn_inputs.end(), ifcn_call_in.begin()+1); std::vector<MX> ifcn_call_out = ifcn.call(ifcn_call_in, true); Vs = vertsplit(ifcn_call_out[0], splitPositions); MX XF = 0; for (int i=0;i<order+1;++i) { XF += D[i]*(i==0? X : X + Vs[i-1]); } // Get the discrete time dynamics ifcn_call_in.erase(ifcn_call_in.begin()); MXFunction F = MXFunction(ifcn_call_in, XF); F.init(); // Loop over all finite elements MX h_ = tf/ne; MX t0_ = 0; for (int i=0;i<ne;++i) { std::vector<MX> F_in; F_in.push_back(X); F_in.push_back(P); F_in.push_back(t0_); F_in.push_back(h_); t0_+= h_; std::vector<MX> F_out = F.call(F_in); X = F_out[0]; } // Create a ruturn function with Integrator signature MXFunction ret = MXFunction(integratorIn("x0", X0, "p", P), integratorOut("xf", X)); ret.init(); return ret; }
void NLPSolverInternal::init(){ // Read options verbose_ = getOption("verbose"); gauss_newton_ = getOption("gauss_newton"); // Initialize the functions casadi_assert_message(!F_.isNull(),"No objective function"); if(!F_.isInit()){ F_.init(); log("Objective function initialized"); } if(!G_.isNull() && !G_.isInit()){ G_.init(); log("Constraint function initialized"); } // Get dimensions n_ = F_.input(0).numel(); m_ = G_.isNull() ? 0 : G_.output(0).numel(); parametric_ = getOption("parametric"); if (parametric_) { casadi_assert_message(F_.getNumInputs()==2, "Wrong number of input arguments to F for parametric NLP. Must be 2, but got " << F_.getNumInputs()); } else { casadi_assert_message(F_.getNumInputs()==1, "Wrong number of input arguments to F for non-parametric NLP. Must be 1, but got " << F_.getNumInputs() << " instead. Do you perhaps intend to use fixed parameters? Then use the 'parametric' option."); } // Basic sanity checks casadi_assert_message(F_.getNumInputs()==1 || F_.getNumInputs()==2, "Wrong number of input arguments to F. Must be 1 or 2"); if (F_.getNumInputs()==2) parametric_=true; casadi_assert_message(getOption("ignore_check_vec") || gauss_newton_ || F_.input().size2()==1, "To avoid confusion, the input argument to F must be vector. You supplied " << F_.input().dimString() << endl << " We suggest you make the following changes:" << endl << " - F is an SXFunction: SXFunction([X],[rhs]) -> SXFunction([vec(X)],[rhs])" << endl << " or F - -> F = vec(F) " << " - F is an MXFunction: MXFunction([X],[rhs]) -> " << endl << " X_vec = MX(\"X\",vec(X.sparsity())) " << endl << " F_vec = MXFunction([X_flat],[F.call([X_flat.reshape(X.sparsity())])[0]]) " << endl << " or F - -> F = vec(F) " << " You may ignore this warning by setting the 'ignore_check_vec' option to true." << endl ); casadi_assert_message(F_.getNumOutputs()>=1, "Wrong number of output arguments to F"); casadi_assert_message(gauss_newton_ || F_.output().scalar(), "Output argument of F not scalar."); casadi_assert_message(F_.output().dense(), "Output argument of F not dense."); casadi_assert_message(F_.input().dense(), "Input argument of F must be dense. You supplied " << F_.input().dimString()); if(!G_.isNull()) { if (parametric_) { casadi_assert_message(G_.getNumInputs()==2, "Wrong number of input arguments to G for parametric NLP. Must be 2, but got " << G_.getNumInputs()); } else { casadi_assert_message(G_.getNumInputs()==1, "Wrong number of input arguments to G for non-parametric NLP. Must be 1, but got " << G_.getNumInputs() << " instead. Do you perhaps intend to use fixed parameters? Then use the 'parametric' option."); } casadi_assert_message(G_.getNumOutputs()>=1, "Wrong number of output arguments to G"); casadi_assert_message(G_.input().numel()==n_, "Inconsistent dimensions"); casadi_assert_message(G_.input().sparsity()==F_.input().sparsity(), "F and G input dimension must match. F " << F_.input().dimString() << ". G " << G_.input().dimString()); } // Find out if we are to expand the objective function in terms of scalar operations bool expand_f = getOption("expand_f"); if(expand_f){ log("Expanding objective function"); // Cast to MXFunction MXFunction F_mx = shared_cast<MXFunction>(F_); if(F_mx.isNull()){ casadi_warning("Cannot expand objective function as it is not an MXFunction"); } else { // Take use the input scheme of G if possible (it might be an SXFunction) vector<SXMatrix> inputv; if(!G_.isNull() && F_.getNumInputs()==G_.getNumInputs()){ inputv = G_.symbolicInputSX(); } else { inputv = F_.symbolicInputSX(); } // Try to expand the MXFunction F_ = F_mx.expand(inputv); F_.setOption("number_of_fwd_dir",F_mx.getOption("number_of_fwd_dir")); F_.setOption("number_of_adj_dir",F_mx.getOption("number_of_adj_dir")); F_.init(); } } // Find out if we are to expand the constraint function in terms of scalar operations bool expand_g = getOption("expand_g"); if(expand_g){ log("Expanding constraint function"); // Cast to MXFunction MXFunction G_mx = shared_cast<MXFunction>(G_); if(G_mx.isNull()){ casadi_warning("Cannot expand constraint function as it is not an MXFunction"); } else { // Take use the input scheme of F if possible (it might be an SXFunction) vector<SXMatrix> inputv; if(F_.getNumInputs()==G_.getNumInputs()){ inputv = F_.symbolicInputSX(); } else { inputv = G_.symbolicInputSX(); } // Try to expand the MXFunction G_ = G_mx.expand(inputv); G_.setOption("number_of_fwd_dir",G_mx.getOption("number_of_fwd_dir")); G_.setOption("number_of_adj_dir",G_mx.getOption("number_of_adj_dir")); G_.init(); } } // Find out if we are to expand the constraint function in terms of scalar operations bool generate_hessian = getOption("generate_hessian"); if(generate_hessian && H_.isNull()){ casadi_assert_message(!gauss_newton_,"Automatic generation of Gauss-Newton Hessian not yet supported"); log("generating hessian"); // Simple if unconstrained if(G_.isNull()){ // Create Hessian of the objective function FX HF = F_.hessian(); HF.init(); // Symbolic inputs of HF vector<MX> HF_in = F_.symbolicInput(); // Lagrange multipliers MX lam("lam",0); // Objective function scaling MX sigma("sigma"); // Inputs of the Hessian function vector<MX> H_in = HF_in; H_in.insert(H_in.begin()+1, lam); H_in.insert(H_in.begin()+2, sigma); // Get an expression for the Hessian of F MX hf = HF.call(HF_in).at(0); // Create the scaled Hessian function H_ = MXFunction(H_in, sigma*hf); log("Unconstrained Hessian function generated"); } else { // G_.isNull() // Check if the functions are SXFunctions SXFunction F_sx = shared_cast<SXFunction>(F_); SXFunction G_sx = shared_cast<SXFunction>(G_); // Efficient if both functions are SXFunction if(!F_sx.isNull() && !G_sx.isNull()){ // Expression for f and g SXMatrix f = F_sx.outputSX(); SXMatrix g = G_sx.outputSX(); // Numeric hessian bool f_num_hess = F_sx.getOption("numeric_hessian"); bool g_num_hess = G_sx.getOption("numeric_hessian"); // Number of derivative directions int f_num_fwd = F_sx.getOption("number_of_fwd_dir"); int g_num_fwd = G_sx.getOption("number_of_fwd_dir"); int f_num_adj = F_sx.getOption("number_of_adj_dir"); int g_num_adj = G_sx.getOption("number_of_adj_dir"); // Substitute symbolic variables in f if different input variables from g if(!isEqual(F_sx.inputSX(),G_sx.inputSX())){ f = substitute(f,F_sx.inputSX(),G_sx.inputSX()); } // Lagrange multipliers SXMatrix lam = ssym("lambda",g.size1()); // Objective function scaling SXMatrix sigma = ssym("sigma"); // Lagrangian function vector<SXMatrix> lfcn_in(parametric_? 4: 3); lfcn_in[0] = G_sx.inputSX(); lfcn_in[1] = lam; lfcn_in[2] = sigma; if (parametric_) lfcn_in[3] = G_sx.inputSX(1); SXFunction lfcn(lfcn_in, sigma*f + inner_prod(lam,g)); lfcn.setOption("verbose",getOption("verbose")); lfcn.setOption("numeric_hessian",f_num_hess || g_num_hess); lfcn.setOption("number_of_fwd_dir",std::min(f_num_fwd,g_num_fwd)); lfcn.setOption("number_of_adj_dir",std::min(f_num_adj,g_num_adj)); lfcn.init(); // Hessian of the Lagrangian H_ = static_cast<FX&>(lfcn).hessian(); H_.setOption("verbose",getOption("verbose")); log("SX Hessian function generated"); } else { // !F_sx.isNull() && !G_sx.isNull() // Check if the functions are SXFunctions MXFunction F_mx = shared_cast<MXFunction>(F_); MXFunction G_mx = shared_cast<MXFunction>(G_); // If they are, check if the arguments are the same if(!F_mx.isNull() && !G_mx.isNull() && isEqual(F_mx.inputMX(),G_mx.inputMX())){ casadi_warning("Exact Hessian calculation for MX is still experimental"); // Expression for f and g MX f = F_mx.outputMX(); MX g = G_mx.outputMX(); // Lagrange multipliers MX lam("lam",g.size1()); // Objective function scaling MX sigma("sigma"); // Inputs of the Lagrangian function vector<MX> lfcn_in(parametric_? 4:3); lfcn_in[0] = G_mx.inputMX(); lfcn_in[1] = lam; lfcn_in[2] = sigma; if (parametric_) lfcn_in[3] = G_mx.inputMX(1); // Lagrangian function MXFunction lfcn(lfcn_in,sigma*f+ inner_prod(lam,g)); lfcn.init(); log("SX Lagrangian function generated"); /* cout << "countNodes(lfcn.outputMX()) = " << countNodes(lfcn.outputMX()) << endl;*/ bool adjoint_mode = true; if(adjoint_mode){ // Gradient of the lagrangian MX gL = lfcn.grad(); log("MX Lagrangian gradient generated"); MXFunction glfcn(lfcn_in,gL); glfcn.init(); log("MX Lagrangian gradient function initialized"); // cout << "countNodes(glfcn.outputMX()) = " << countNodes(glfcn.outputMX()) << endl; // Get Hessian sparsity CRSSparsity H_sp = glfcn.jacSparsity(); log("MX Lagrangian Hessian sparsity determined"); // Uni-directional coloring (note, the hessian is symmetric) CRSSparsity coloring = H_sp.unidirectionalColoring(H_sp); log("MX Lagrangian Hessian coloring determined"); // Number of colors needed is the number of rows int nfwd_glfcn = coloring.size1(); log("MX Lagrangian gradient function number of sensitivity directions determined"); glfcn.setOption("number_of_fwd_dir",nfwd_glfcn); glfcn.updateNumSens(); log("MX Lagrangian gradient function number of sensitivity directions updated"); // Hessian of the Lagrangian H_ = glfcn.jacobian(); } else { // Hessian of the Lagrangian H_ = lfcn.hessian(); } log("MX Lagrangian Hessian function generated"); } else { casadi_assert_message(0, "Automatic calculation of exact Hessian currently only for F and G both SXFunction or MXFunction "); } } // !F_sx.isNull() && !G_sx.isNull() } // G_.isNull() } // generate_hessian && H_.isNull() if(!H_.isNull() && !H_.isInit()) { H_.init(); log("Hessian function initialized"); } // Create a Jacobian if it does not already exists bool generate_jacobian = getOption("generate_jacobian"); if(generate_jacobian && !G_.isNull() && J_.isNull()){ log("Generating Jacobian"); J_ = G_.jacobian(); // Use live variables if SXFunction if(!shared_cast<SXFunction>(J_).isNull()){ J_.setOption("live_variables",true); } log("Jacobian function generated"); } if(!J_.isNull() && !J_.isInit()){ J_.init(); log("Jacobian function initialized"); } if(!H_.isNull()) { if (parametric_) { casadi_assert_message(H_.getNumInputs()>=2, "Wrong number of input arguments to H for parametric NLP. Must be at least 2, but got " << G_.getNumInputs()); } else { casadi_assert_message(H_.getNumInputs()>=1, "Wrong number of input arguments to H for non-parametric NLP. Must be at least 1, but got " << G_.getNumInputs() << " instead. Do you perhaps intend to use fixed parameters? Then use the 'parametric' option."); } casadi_assert_message(H_.getNumOutputs()>=1, "Wrong number of output arguments to H"); casadi_assert_message(H_.input(0).numel()==n_,"Inconsistent dimensions"); casadi_assert_message(H_.output().size1()==n_,"Inconsistent dimensions"); casadi_assert_message(H_.output().size2()==n_,"Inconsistent dimensions"); } if(!J_.isNull()){ if (parametric_) { casadi_assert_message(J_.getNumInputs()==2, "Wrong number of input arguments to J for parametric NLP. Must be at least 2, but got " << G_.getNumInputs()); } else { casadi_assert_message(J_.getNumInputs()==1, "Wrong number of input arguments to J for non-parametric NLP. Must be at least 1, but got " << G_.getNumInputs() << " instead. Do you perhaps intend to use fixed parameters? Then use the 'parametric' option."); } casadi_assert_message(J_.getNumOutputs()>=1, "Wrong number of output arguments to J"); casadi_assert_message(J_.input().numel()==n_,"Inconsistent dimensions"); casadi_assert_message(J_.output().size2()==n_,"Inconsistent dimensions"); } if (parametric_) { sp_p = F_->input(1).sparsity(); if (!G_.isNull()) casadi_assert_message(sp_p == G_->input(G_->getNumInputs()-1).sparsity(),"Parametric NLP has inconsistent parameter dimensions. F has got " << sp_p.dimString() << " as dimensions, while G has got " << G_->input(G_->getNumInputs()-1).dimString()); if (!H_.isNull()) casadi_assert_message(sp_p == H_->input(H_->getNumInputs()-1).sparsity(),"Parametric NLP has inconsistent parameter dimensions. F has got " << sp_p.dimString() << " as dimensions, while H has got " << H_->input(H_->getNumInputs()-1).dimString()); if (!J_.isNull()) casadi_assert_message(sp_p == J_->input(J_->getNumInputs()-1).sparsity(),"Parametric NLP has inconsistent parameter dimensions. F has got " << sp_p.dimString() << " as dimensions, while J has got " << J_->input(J_->getNumInputs()-1).dimString()); } // Infinity double inf = numeric_limits<double>::infinity(); // Allocate space for inputs input_.resize(NLP_NUM_IN - (parametric_? 0 : 1)); input(NLP_X_INIT) = DMatrix(n_,1,0); input(NLP_LBX) = DMatrix(n_,1,-inf); input(NLP_UBX) = DMatrix(n_,1, inf); input(NLP_LBG) = DMatrix(m_,1,-inf); input(NLP_UBG) = DMatrix(m_,1, inf); input(NLP_LAMBDA_INIT) = DMatrix(m_,1,0); if (parametric_) input(NLP_P) = DMatrix(sp_p,0); // Allocate space for outputs output_.resize(NLP_NUM_OUT); output(NLP_X_OPT) = DMatrix(n_,1,0); output(NLP_COST) = DMatrix(1,1,0); output(NLP_LAMBDA_X) = DMatrix(n_,1,0); output(NLP_LAMBDA_G) = DMatrix(m_,1,0); output(NLP_G) = DMatrix(m_,1,0); if (hasSetOption("iteration_callback")) { callback_ = getOption("iteration_callback"); if (!callback_.isNull()) { if (!callback_.isInit()) callback_.init(); casadi_assert_message(callback_.getNumOutputs()==1, "Callback function should have one output, a scalar that indicates wether to break. 0 = continue"); casadi_assert_message(callback_.output(0).size()==1, "Callback function should have one output, a scalar that indicates wether to break. 0 = continue"); casadi_assert_message(callback_.getNumInputs()==NLP_NUM_OUT, "Callback function should have the output scheme of NLPSolver as input scheme. i.e. " <<NLP_NUM_OUT << " inputs instead of the " << callback_.getNumInputs() << " you provided." ); for (int i=0;i<NLP_NUM_OUT;i++) { casadi_assert_message(callback_.input(i).sparsity()==output(i).sparsity(), "Callback function should have the output scheme of NLPSolver as input scheme. " << "Input #" << i << " (" << getSchemeEntryEnumName(SCHEME_NLPOutput,i) << " aka '" << getSchemeEntryName(SCHEME_NLPOutput,i) << "') was found to be " << callback_.input(i).dimString() << " instead of expected " << output(i).dimString() << "." ); callback_.input(i).setAll(0); } } } callback_step_ = getOption("iteration_callback_step"); // Call the initialization method of the base class FXInternal::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); } }
void GslInternal::init(){ // Init ODE rhs function and quadrature functions f_.init(); casadi_assert(f_.getNumInputs()==DAE_NUM_IN); casadi_assert(f_.getNumOutputs()==DAE_NUM_OUT); if(!q_.isNull()){ q_.init(); casadi_assert(q_.getNumInputs()==DAE_NUM_IN); casadi_assert(q_.getNumOutputs()==DAE_NUM_OUT); } // Number of states int nx = f_.output(INTEGRATOR_XF).numel(); // Add quadratures, if any if(!q_.isNull()) nx += q_.output().numel(); // Number of parameters int np = f_.input(DAE_P).numel(); setDimensions(nx,np); // If time was not specified, initialise it. if (f_.input(DAE_T).numel()==0) { std::vector<MX> in1(DAE_NUM_IN); in1[DAE_T] = MX("T"); in1[DAE_Y] = MX("Y",f_.input(DAE_Y).size1(),f_.input(DAE_Y).size2()); in1[DAE_YDOT] = MX("YDOT",f_.input(DAE_YDOT).size1(),f_.input(DAE_YDOT).size2()); in1[DAE_P] = MX("P",f_.input(DAE_P).size1(),f_.input(DAE_P).size2()); std::vector<MX> in2(in1); in2[DAE_T] = MX(); f_ = MXFunction(in1,f_.call(in2)); f_.init(); } // We only allow for 0-D time casadi_assert_message(f_.input(DAE_T).numel()==1, "IntegratorInternal: time must be zero-dimensional, not (" << f_.input(DAE_T).size1() << 'x' << f_.input(DAE_T).size2() << ")"); // ODE right hand side must be a dense matrix casadi_assert_message(f_.output(DAE_RES).dense(),"ODE right hand side must be dense: reformulate the problem"); // States and RHS should match casadi_assert_message(f_.output(DAE_RES).size()==f_.input(DAE_Y).size(), "IntegratorInternal: rhs of ODE is (" << f_.output(DAE_RES).size1() << 'x' << f_.output(DAE_RES).size2() << ") - " << f_.output(DAE_RES).size() << " non-zeros" << std::endl << " ODE state matrix is (" << f_.input(DAE_Y).size1() << 'x' << f_.input(DAE_Y).size2() << ") - " << f_.input(DAE_Y).size() << " non-zeros" << std::endl << "Mismatch between number of non-zeros" ); IntegratorInternal::init(); jac_f_ = Jacobian(f_,DAE_Y,DAE_RES); dt_f_ = Jacobian(f_,DAE_T,DAE_RES); jac_f_.init(); dt_f_.init(); // define the type of routine for making steps: type_ptr = gsl_odeiv_step_rkf45; // some other possibilities (see GSL manual): // = gsl_odeiv_step_rk4; // = gsl_odeiv_step_rkck; // = gsl_odeiv_step_rk8pd; // = gsl_odeiv_step_rk4imp; // = gsl_odeiv_step_bsimp; // = gsl_odeiv_step_gear1; // = gsl_odeiv_step_gear2; step_ptr = gsl_odeiv_step_alloc (type_ptr, nx); control_ptr = gsl_odeiv_control_y_new (abstol_, reltol_); evolve_ptr = gsl_odeiv_evolve_alloc (nx); my_system.function = rhs_wrapper; // the right-hand-side functions dy[i]/dt my_system.jacobian = jac_wrapper; // the Jacobian df[i]/dy[j] my_system.dimension = nx; // number of diffeq's my_system.params = this; // parameters to pass to rhs and jacobian is_init = true; }
void SQICInternal::init() { // Call the init method of the base class QpSolverInternal::init(); if (is_init_) sqicDestroy(); inf_ = 1.0e+20; // Allocate data structures for SQIC bl_.resize(n_+nc_+1, 0); bu_.resize(n_+nc_+1, 0); x_.resize(n_+nc_+1, 0); hs_.resize(n_+nc_+1, 0); hEtype_.resize(n_+nc_+1, 0); pi_.resize(nc_+1, 0); rc_.resize(n_+nc_+1, 0); locH_ = st_[QP_STRUCT_H].colind(); indH_ = st_[QP_STRUCT_H].row(); // Fortran indices are one-based for (int i=0;i<indH_.size();++i) indH_[i]+=1; for (int i=0;i<locH_.size();++i) locH_[i]+=1; // Sparsity of augmented linear constraint matrix Sparsity A_ = vertcat(st_[QP_STRUCT_A], Sparsity::dense(1, n_)); locA_ = A_.colind(); indA_ = A_.row(); // Fortran indices are one-based for (int i=0;i<indA_.size();++i) indA_[i]+=1; for (int i=0;i<locA_.size();++i) locA_[i]+=1; // helper functions for augmented linear constraint matrix MX a = MX::sym("A", st_[QP_STRUCT_A]); MX g = MX::sym("g", n_); std::vector<MX> ins; ins.push_back(a); ins.push_back(g); formatA_ = MXFunction(ins, vertcat(a, g.T())); formatA_.init(); // Set objective row of augmented linear constraints bu_[n_+nc_] = inf_; bl_[n_+nc_] = -inf_; is_init_ = true; int n = n_; int m = nc_+1; int nnzA=formatA_.output().size(); int nnzH=input(QP_SOLVER_H).size(); std::fill(hEtype_.begin()+n_, hEtype_.end(), 3); sqic(&m , &n, &nnzA, &indA_[0], &locA_[0], &formatA_.output().data()[0], &bl_[0], &bu_[0], &hEtype_[0], &hs_[0], &x_[0], &pi_[0], &rc_[0], &nnzH, &indH_[0], &locH_[0], &input(QP_SOLVER_H).data()[0]); }
void CollocationIntegratorInternal::setupFG() { // Interpolation order deg_ = getOption("interpolation_order"); // All collocation time points std::vector<long double> tau_root = collocationPointsL(deg_, getOption("collocation_scheme")); // Coefficients of the collocation equation vector<vector<double> > C(deg_+1, vector<double>(deg_+1, 0)); // Coefficients of the continuity equation vector<double> D(deg_+1, 0); // Coefficients of the quadratures vector<double> B(deg_+1, 0); // For all collocation points for (int j=0; j<deg_+1; ++j) { // Construct Lagrange polynomials to get the polynomial basis at the collocation point Polynomial p = 1; for (int r=0; r<deg_+1; ++r) { if (r!=j) { p *= Polynomial(-tau_root[r], 1)/(tau_root[j]-tau_root[r]); } } // Evaluate the polynomial at the final time to get the // coefficients of the continuity equation D[j] = zeroIfSmall(p(1.0L)); // Evaluate the time derivative of the polynomial at all collocation points to // get the coefficients of the continuity equation Polynomial dp = p.derivative(); for (int r=0; r<deg_+1; ++r) { C[j][r] = zeroIfSmall(dp(tau_root[r])); } // Integrate polynomial to get the coefficients of the quadratures Polynomial ip = p.anti_derivative(); B[j] = zeroIfSmall(ip(1.0L)); } // Symbolic inputs MX x0 = MX::sym("x0", f_.input(DAE_X).sparsity()); MX p = MX::sym("p", f_.input(DAE_P).sparsity()); MX t = MX::sym("t", f_.input(DAE_T).sparsity()); // Implicitly defined variables (z and x) MX v = MX::sym("v", deg_*(nx_+nz_)); vector<int> v_offset(1, 0); for (int d=0; d<deg_; ++d) { v_offset.push_back(v_offset.back()+nx_); v_offset.push_back(v_offset.back()+nz_); } vector<MX> vv = vertsplit(v, v_offset); vector<MX>::const_iterator vv_it = vv.begin(); // Collocated states vector<MX> x(deg_+1), z(deg_+1); for (int d=1; d<=deg_; ++d) { x[d] = reshape(*vv_it++, this->x0().shape()); z[d] = reshape(*vv_it++, this->z0().shape()); } casadi_assert(vv_it==vv.end()); // Collocation time points vector<MX> tt(deg_+1); for (int d=0; d<=deg_; ++d) { tt[d] = t + h_*tau_root[d]; } // Equations that implicitly define v vector<MX> eq; // Quadratures MX qf = MX::zeros(f_.output(DAE_QUAD).sparsity()); // End state MX xf = D[0]*x0; // For all collocation points for (int j=1; j<deg_+1; ++j) { //for (int j=deg_; j>=1; --j) { // Evaluate the DAE vector<MX> f_arg(DAE_NUM_IN); f_arg[DAE_T] = tt[j]; f_arg[DAE_P] = p; f_arg[DAE_X] = x[j]; f_arg[DAE_Z] = z[j]; vector<MX> f_res = f_.call(f_arg); // Get an expression for the state derivative at the collocation point MX xp_j = C[0][j] * x0; for (int r=1; r<deg_+1; ++r) { xp_j += C[r][j] * x[r]; } // Add collocation equation eq.push_back(vec(h_*f_res[DAE_ODE] - xp_j)); // Add the algebraic conditions eq.push_back(vec(f_res[DAE_ALG])); // Add contribution to the final state xf += D[j]*x[j]; // Add contribution to quadratures qf += (B[j]*h_)*f_res[DAE_QUAD]; } // Form forward discrete time dynamics vector<MX> F_in(DAE_NUM_IN); F_in[DAE_T] = t; F_in[DAE_X] = x0; F_in[DAE_P] = p; F_in[DAE_Z] = v; vector<MX> F_out(DAE_NUM_OUT); F_out[DAE_ODE] = xf; F_out[DAE_ALG] = vertcat(eq); F_out[DAE_QUAD] = qf; F_ = MXFunction(F_in, F_out); F_.init(); // Backwards dynamics // NOTE: The following is derived so that it will give the exact adjoint // sensitivities whenever g is the reverse mode derivative of f. if (!g_.isNull()) { // Symbolic inputs MX rx0 = MX::sym("x0", g_.input(RDAE_RX).sparsity()); MX rp = MX::sym("p", g_.input(RDAE_RP).sparsity()); // Implicitly defined variables (rz and rx) MX rv = MX::sym("v", deg_*(nrx_+nrz_)); vector<int> rv_offset(1, 0); for (int d=0; d<deg_; ++d) { rv_offset.push_back(rv_offset.back()+nrx_); rv_offset.push_back(rv_offset.back()+nrz_); } vector<MX> rvv = vertsplit(rv, rv_offset); vector<MX>::const_iterator rvv_it = rvv.begin(); // Collocated states vector<MX> rx(deg_+1), rz(deg_+1); for (int d=1; d<=deg_; ++d) { rx[d] = reshape(*rvv_it++, this->rx0().shape()); rz[d] = reshape(*rvv_it++, this->rz0().shape()); } casadi_assert(rvv_it==rvv.end()); // Equations that implicitly define v eq.clear(); // Quadratures MX rqf = MX::zeros(g_.output(RDAE_QUAD).sparsity()); // End state MX rxf = D[0]*rx0; // For all collocation points for (int j=1; j<deg_+1; ++j) { // Evaluate the backward DAE vector<MX> g_arg(RDAE_NUM_IN); g_arg[RDAE_T] = tt[j]; g_arg[RDAE_P] = p; g_arg[RDAE_X] = x[j]; g_arg[RDAE_Z] = z[j]; g_arg[RDAE_RX] = rx[j]; g_arg[RDAE_RZ] = rz[j]; g_arg[RDAE_RP] = rp; vector<MX> g_res = g_.call(g_arg); // Get an expression for the state derivative at the collocation point MX rxp_j = -D[j]*rx0; for (int r=1; r<deg_+1; ++r) { rxp_j += (B[r]*C[j][r]) * rx[r]; } // Add collocation equation eq.push_back(vec(h_*B[j]*g_res[RDAE_ODE] - rxp_j)); // Add the algebraic conditions eq.push_back(vec(g_res[RDAE_ALG])); // Add contribution to the final state rxf += -B[j]*C[0][j]*rx[j]; // Add contribution to quadratures rqf += h_*B[j]*g_res[RDAE_QUAD]; } // Form backward discrete time dynamics vector<MX> G_in(RDAE_NUM_IN); G_in[RDAE_T] = t; G_in[RDAE_X] = x0; G_in[RDAE_P] = p; G_in[RDAE_Z] = v; G_in[RDAE_RX] = rx0; G_in[RDAE_RP] = rp; G_in[RDAE_RZ] = rv; vector<MX> G_out(RDAE_NUM_OUT); G_out[RDAE_ODE] = rxf; G_out[RDAE_ALG] = vertcat(eq); G_out[RDAE_QUAD] = rqf; G_ = MXFunction(G_in, G_out); G_.init(); } }
Function NlpSolver::joinFG(Function F, Function G) { if (G.isNull()) { // unconstrained if (is_a<SXFunction>(F)) { SXFunction F_sx = shared_cast<SXFunction>(F); vector<SX> nlp_in = F_sx.inputExpr(); nlp_in.resize(NL_NUM_IN); vector<SX> nlp_out(NL_NUM_OUT); nlp_out[NL_F] = F_sx.outputExpr(0); return SXFunction(nlp_in, nlp_out); } else if (is_a<MXFunction>(F)) { MXFunction F_mx = shared_cast<MXFunction>(F); vector<MX> nlp_in = F_mx.inputExpr(); nlp_in.resize(NL_NUM_IN); vector<MX> nlp_out(NL_NUM_OUT); nlp_out[NL_F] = F_mx.outputExpr(0); return MXFunction(nlp_in, nlp_out); } else { vector<MX> F_in = F.symbolicInput(); vector<MX> nlp_in(NL_NUM_IN); nlp_in[NL_X] = F_in.at(0); if (F_in.size()>1) nlp_in[NL_P] = F_in.at(1); vector<MX> nlp_out(NL_NUM_OUT); nlp_out[NL_F] = F(F_in).front(); return MXFunction(nlp_in, nlp_out); } } else if (F.isNull()) { // feasibility problem if (is_a<SXFunction>(G)) { SXFunction G_sx = shared_cast<SXFunction>(G); vector<SX> nlp_in = G_sx.inputExpr(); nlp_in.resize(NL_NUM_IN); vector<SX> nlp_out(NL_NUM_OUT); nlp_out[NL_G] = G_sx.outputExpr(0); return SXFunction(nlp_in, nlp_out); } else if (is_a<MXFunction>(G)) { MXFunction G_mx = shared_cast<MXFunction>(F); vector<MX> nlp_in = G_mx.inputExpr(); nlp_in.resize(NL_NUM_IN); vector<MX> nlp_out(NL_NUM_OUT); nlp_out[NL_G] = G_mx.outputExpr(0); nlp_out.resize(NL_NUM_OUT); return MXFunction(nlp_in, nlp_out); } else { vector<MX> G_in = G.symbolicInput(); vector<MX> nlp_in(NL_NUM_IN); nlp_in[NL_X] = G_in.at(0); if (G_in.size()>1) nlp_in[NL_P] = G_in.at(1); vector<MX> nlp_out(NL_NUM_OUT); nlp_out[NL_G] = G(G_in).at(0); return MXFunction(nlp_in, nlp_out); } } else { // Standard (constrained) NLP // SXFunction if both functions are SXFunction if (is_a<SXFunction>(F) && is_a<SXFunction>(G)) { vector<SX> nlp_in(NL_NUM_IN), nlp_out(NL_NUM_OUT); SXFunction F_sx = shared_cast<SXFunction>(F); SXFunction G_sx = shared_cast<SXFunction>(G); nlp_in[NL_X] = G_sx.inputExpr(0); if (G_sx.getNumInputs()>1) { nlp_in[NL_P] = G_sx.inputExpr(1); } else { nlp_in[NL_P] = SX::sym("p", 1, 0); } // Expression for f and g nlp_out[NL_G] = G_sx.outputExpr(0); nlp_out[NL_F] = substitute(F_sx.outputExpr(), F_sx.inputExpr(), G_sx.inputExpr()).front(); return SXFunction(nlp_in, nlp_out); } else { // MXFunction otherwise vector<MX> nlp_in(NL_NUM_IN), nlp_out(NL_NUM_OUT); // Try to cast into MXFunction MXFunction F_mx = shared_cast<MXFunction>(F); MXFunction G_mx = shared_cast<MXFunction>(G); // Convert to MX if cast failed and make sure that they // use the same expressions if cast was successful if (!G_mx.isNull()) { nlp_in[NL_X] = G_mx.inputExpr(0); if (G_mx.getNumInputs()>1) { nlp_in[NL_P] = G_mx.inputExpr(1); } else { nlp_in[NL_P] = MX::sym("p", 1, 0); } nlp_out[NL_G] = G_mx.outputExpr(0); if (!F_mx.isNull()) { // Both are MXFunction, make sure they use the same variables nlp_out[NL_F] = substitute(F_mx.outputExpr(), F_mx.inputExpr(), G_mx.inputExpr()).front(); } else { // G_ but not F_ MXFunction nlp_out[NL_F] = F(G_mx.inputExpr()).front(); } } else { if (!F_mx.isNull()) { // F but not G MXFunction nlp_in[NL_X] = F_mx.inputExpr(0); if (F_mx.getNumInputs()>1) { nlp_in[NL_P] = F_mx.inputExpr(1); } else { nlp_in[NL_P] = MX::sym("p", 1, 0); } nlp_out[NL_F] = F_mx.outputExpr(0); nlp_out[NL_G] = G(F_mx.inputExpr()).front(); } else { // None of them MXFunction vector<MX> FG_in = G.symbolicInput(); nlp_in[NL_X] = FG_in.at(0); if (FG_in.size()>1) nlp_in[NL_P] = FG_in.at(1); nlp_out[NL_G] = G(FG_in).front(); nlp_out[NL_F] = F(FG_in).front(); } } return MXFunction(nlp_in, nlp_out); } // SXFunction/MXFunction } // constrained/unconstrained }
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 SDPSDQPInternal::init() { // Initialize the base classes SdqpSolverInternal::init(); cholesky_ = LinearSolver("csparsecholesky", st_[SDQP_STRUCT_H]); cholesky_.init(); MX g_socp = MX::sym("x", cholesky_.getFactorizationSparsity(true)); MX h_socp = MX::sym("h", n_); MX f_socp = sqrt(inner_prod(h_socp, h_socp)); MX en_socp = 0.5/f_socp; MX f_sdqp = MX::sym("f", input(SDQP_SOLVER_F).sparsity()); MX g_sdqp = MX::sym("g", input(SDQP_SOLVER_G).sparsity()); std::vector<MX> fi(n_+1); MX znp = MX::sparse(n_+1, n_+1); for (int k=0;k<n_;++k) { MX gk = vertcat(g_socp(ALL, k), DMatrix::sparse(1, 1)); MX fk = -blockcat(znp, gk, gk.T(), DMatrix::sparse(1, 1)); // TODO(Joel): replace with ALL fi.push_back(blkdiag(f_sdqp(ALL, Slice(f_sdqp.size1()*k, f_sdqp.size1()*(k+1))), fk)); } MX fin = en_socp*DMatrix::eye(n_+2); fin(n_, n_+1) = en_socp; fin(n_+1, n_) = en_socp; fi.push_back(blkdiag(DMatrix::sparse(f_sdqp.size1(), f_sdqp.size1()), -fin)); MX h0 = vertcat(h_socp, DMatrix::sparse(1, 1)); MX g = blockcat(f_socp*DMatrix::eye(n_+1), h0, h0.T(), f_socp); g = blkdiag(g_sdqp, g); IOScheme mappingIn("g_socp", "h_socp", "f_sdqp", "g_sdqp"); IOScheme mappingOut("f", "g"); mapping_ = MXFunction(mappingIn("g_socp", g_socp, "h_socp", h_socp, "f_sdqp", f_sdqp, "g_sdqp", g_sdqp), mappingOut("f", horzcat(fi), "g", g)); mapping_.init(); // Create an sdpsolver instance std::string sdpsolver_name = getOption("sdp_solver"); sdpsolver_ = SdpSolver(sdpsolver_name, sdpStruct("g", mapping_.output("g").sparsity(), "f", mapping_.output("f").sparsity(), "a", horzcat(input(SDQP_SOLVER_A).sparsity(), Sparsity::sparse(nc_, 1)))); if (hasSetOption("sdp_solver_options")) { sdpsolver_.setOption(getOption("sdp_solver_options")); } // Initialize the SDP solver sdpsolver_.init(); sdpsolver_.input(SDP_SOLVER_C).at(n_)=1; // Output arguments setNumOutputs(SDQP_SOLVER_NUM_OUT); output(SDQP_SOLVER_X) = DMatrix::zeros(n_, 1); std::vector<int> r = range(input(SDQP_SOLVER_G).size1()); output(SDQP_SOLVER_P) = sdpsolver_.output(SDP_SOLVER_P).isEmpty() ? DMatrix() : sdpsolver_.output(SDP_SOLVER_P)(r, r); output(SDQP_SOLVER_DUAL) = sdpsolver_.output(SDP_SOLVER_DUAL).isEmpty() ? DMatrix() : sdpsolver_.output(SDP_SOLVER_DUAL)(r, r); output(SDQP_SOLVER_COST) = 0.0; output(SDQP_SOLVER_DUAL_COST) = 0.0; output(SDQP_SOLVER_LAM_X) = DMatrix::zeros(n_, 1); output(SDQP_SOLVER_LAM_A) = DMatrix::zeros(nc_, 1); }
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"); // 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(); }
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 SimpleIndefDleInternal::init() { DleInternal::init(); casadi_assert_message(!pos_def_, "pos_def option set to True: Solver only handles the indefinite case."); n_ = A_.size1(); MX As = MX::sym("A", A_); MX Vs = MX::sym("V", V_); MX Cs = MX::sym("C", C_); MX Hs = MX::sym("H", H_); MX Vss = (Vs+Vs.T())/2; if (with_C_) Vss = mul(mul(Cs, Vss), Cs.T()); MX A_total = DMatrix::eye(n_*n_) - kron(As,As); // Should be treated by solve node MX Pf = solve(A_total, vec(Vss), getOption("linear_solver")); std::vector<MX> v_in; v_in.push_back(As); v_in.push_back(Vs); v_in.push_back(Cs); MX P = reshape(Pf,n_,n_); std::vector<MX> HPH; if (with_H_) { std::vector<MX> H = horzsplit(Hs,Hi_); for (int k=0;k<H.size();++k) { HPH.push_back(mul(H[k].T(),mul(P,H[k]))); } } std::vector<MX> dle_in(DLE_NUM_IN); dle_in[DLE_A] = As; dle_in[DLE_V] = Vs; if (with_C_) dle_in[DLE_C] = Cs; if (with_H_) dle_in[DLE_H] = Hs; f_ = MXFunction(dle_in,dleOut("p",with_H_? diagcat(HPH) : P(output().sparsity()))); f_.init(); casadi_assert(nOut()==f_.nOut()); for (int i=0;i<nIn();++i) { casadi_assert_message(input(i).sparsity()==f_.input(i).sparsity(), "Sparsity mismatch for input " << i << ":" << input(i).dimString() << " <-> " << f_.input(i).dimString() << "."); } for (int i=0;i<nOut();++i) { casadi_assert_message(output(i).sparsity()==f_.output(i).sparsity(), "Sparsity mismatch for output " << i << ":" << output(i).dimString() << " <-> " << f_.output(i).dimString() << "."); } }
void SdqpToSdp::init() { // Initialize the base classes SdqpSolverInternal::init(); cholesky_ = LinearSolver("cholesky", "csparsecholesky", st_[SDQP_STRUCT_H]); MX g_socp = MX::sym("x", cholesky_.getFactorizationSparsity(true)); MX h_socp = MX::sym("h", n_); MX f_socp = sqrt(inner_prod(h_socp, h_socp)); MX en_socp = 0.5/f_socp; MX f_sdqp = MX::sym("f", input(SDQP_SOLVER_F).sparsity()); MX g_sdqp = MX::sym("g", input(SDQP_SOLVER_G).sparsity()); std::vector<MX> fi(n_+1); MX znp = MX(n_+1, n_+1); for (int k=0;k<n_;++k) { MX gk = vertcat(g_socp(ALL, k), MX(1, 1)); MX fk = -blockcat(znp, gk, gk.T(), MX(1, 1)); // TODO(Joel): replace with ALL fi.push_back(diagcat(f_sdqp(ALL, Slice(f_sdqp.size1()*k, f_sdqp.size1()*(k+1))), fk)); } MX fin = en_socp*DMatrix::eye(n_+2); fin(n_, n_+1) = en_socp; fin(n_+1, n_) = en_socp; fi.push_back(diagcat(DMatrix(f_sdqp.size1(), f_sdqp.size1()), -fin)); MX h0 = vertcat(h_socp, DMatrix(1, 1)); MX g = blockcat(f_socp*DMatrix::eye(n_+1), h0, h0.T(), f_socp); g = diagcat(g_sdqp, g); Dict opts; opts["input_scheme"] = IOScheme("g_socp", "h_socp", "f_sdqp", "g_sdqp"); opts["output_scheme"] = IOScheme("f", "g"); mapping_ = MXFunction("mapping", make_vector(g_socp, h_socp, f_sdqp, g_sdqp), make_vector(horzcat(fi), g), opts); Dict options; if (hasSetOption(optionsname())) options = getOption(optionsname()); // Create an SdpSolver instance solver_ = SdpSolver("sdpsolver", getOption(solvername()), make_map("g", mapping_.output("g").sparsity(), "f", mapping_.output("f").sparsity(), "a", horzcat(input(SDQP_SOLVER_A).sparsity(), Sparsity(nc_, 1))), options); solver_.input(SDP_SOLVER_C).at(n_)=1; // Output arguments obuf_.resize(SDQP_SOLVER_NUM_OUT); output(SDQP_SOLVER_X) = DMatrix::zeros(n_, 1); std::vector<int> r = range(input(SDQP_SOLVER_G).size1()); output(SDQP_SOLVER_P) = solver_.output(SDP_SOLVER_P).isempty() ? DMatrix() : solver_.output(SDP_SOLVER_P)(r, r); output(SDQP_SOLVER_DUAL) = solver_.output(SDP_SOLVER_DUAL).isempty() ? DMatrix() : solver_.output(SDP_SOLVER_DUAL)(r, r); output(SDQP_SOLVER_COST) = 0.0; output(SDQP_SOLVER_DUAL_COST) = 0.0; output(SDQP_SOLVER_LAM_X) = DMatrix::zeros(n_, 1); output(SDQP_SOLVER_LAM_A) = DMatrix::zeros(nc_, 1); }
void LiftingLrDpleInternal::init() { form_ = getOptionEnumValue("form"); // Initialize the base classes LrDpleInternal::init(); casadi_assert_message(!pos_def_, "pos_def option set to True: Solver only handles the indefinite case."); casadi_assert_message(const_dim_, "const_dim option set to False: Solver only handles the True case."); // We will construct an MXFunction to facilitate the calculation of derivatives MX As = MX::sym("As", input(LR_DLE_A).sparsity()); MX Vs = MX::sym("Vs", input(LR_DLE_V).sparsity()); MX Cs = MX::sym("Cs", input(LR_DLE_C).sparsity()); MX Hs = MX::sym("Hs", input(LR_DLE_H).sparsity()); n_ = A_[0].size1(); // Chop-up the arguments std::vector<MX> As_ = horzsplit(As, n_); std::vector<MX> Vs_ = horzsplit(Vs, V_[0].size2()); std::vector<MX> Cs_ = horzsplit(Cs, V_[0].size2()); std::vector<MX> Hs_; if (with_H_) { Hs_ = horzsplit(Hs, Hsi_); } MX A; if (K_==1) { A = As; } else { if (form_==0) { MX AL = diagcat(vector_slice(As_, range(As_.size()-1))); MX AL2 = horzcat(AL, MX::sparse(AL.size1(), As_[0].size2())); MX AT = horzcat(MX::sparse(As_[0].size1(), AL.size2()), As_.back()); A = vertcat(AT, AL2); } else { MX AL = diagcat(reverse(vector_slice(As_, range(As_.size()-1)))); MX AL2 = horzcat(MX::sparse(AL.size1(), As_[0].size2()), AL); MX AT = horzcat(As_.back(), MX::sparse(As_[0].size1(), AL.size2())); A = vertcat(AL2, AT); } } MX V; MX C; MX H; if (form_==0) { V = diagcat(Vs_.back(), diagcat(vector_slice(Vs_, range(Vs_.size()-1)))); if (with_C_) { C = diagcat(Cs_.back(), diagcat(vector_slice(Cs_, range(Cs_.size()-1)))); } } else { V = diagcat(diagcat(reverse(vector_slice(Vs_, range(Vs_.size()-1)))), Vs_.back()); if (with_C_) { C = diagcat(diagcat(reverse(vector_slice(Cs_, range(Cs_.size()-1)))), Cs_.back()); } } if (with_H_) { H = diagcat(form_==0? Hs_ : reverse(Hs_)); } // Create an LrDleSolver instance solver_ = LrDleSolver(getOption(solvername()), lrdleStruct("a", A.sparsity(), "v", V.sparsity(), "c", C.sparsity(), "h", H.sparsity())); solver_.setOption("Hs", Hss_); if (hasSetOption(optionsname())) solver_.setOption(getOption(optionsname())); solver_.init(); std::vector<MX> v_in(LR_DPLE_NUM_IN); v_in[LR_DLE_A] = As; v_in[LR_DLE_V] = Vs; if (with_C_) { v_in[LR_DLE_C] = Cs; } if (with_H_) { v_in[LR_DLE_H] = Hs; } std::vector<MX> Pr = solver_.call(lrdpleIn("a", A, "v", V, "c", C, "h", H)); MX Pf = Pr[0]; std::vector<MX> Ps = with_H_ ? diagsplit(Pf, Hsi_) : diagsplit(Pf, n_); if (form_==1) { Ps = reverse(Ps); } f_ = MXFunction(v_in, dpleOut("p", horzcat(Ps))); f_.setInputScheme(SCHEME_LR_DPLEInput); f_.setOutputScheme(SCHEME_LR_DPLEOutput); f_.init(); Wrapper::checkDimensions(); }
void CollocationIntegratorInternal::init(){ // Call the base class init IntegratorInternal::init(); // 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; } // Hotstart? hotstart_ = getOption("hotstart"); // Number of finite elements int nk = getOption("number_of_finite_elements"); // Interpolation order int deg = getOption("interpolation_order"); // Assume explicit ODE bool explicit_ode = f_.input(DAE_XDOT).size()==0; // All collocation time points double* tau_root = use_radau ? radau_points[deg] : legendre_points[deg]; // Size of the finite elements double h = (tf_-t0_)/nk; // MX version of the same MX h_mx = h; // Coefficients of the collocation equation vector<vector<MX> > C(deg+1,vector<MX>(deg+1)); // Coefficients of the continuity equation vector<MX> D(deg+1); // 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(); // 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(); } } // Initial state MX X0("X0",nx_); // Parameters MX P("P",np_); // Backward state MX RX0("RX0",nrx_); // Backward parameters MX RP("RP",nrp_); // Collocated differential states and algebraic variables int nX = (nk*(deg+1)+1)*(nx_+nrx_); int nZ = nk*deg*(nz_+nrz_); // Unknowns MX V("V",nX+nZ); int offset = 0; // Get collocated states, algebraic variables and times vector<vector<MX> > X(nk+1); vector<vector<MX> > RX(nk+1); vector<vector<MX> > Z(nk); vector<vector<MX> > RZ(nk); coll_time_.resize(nk+1); for(int k=0; k<nk+1; ++k){ // Number of time points int nj = k==nk ? 1 : deg+1; // Allocate differential states expressions at the time points X[k].resize(nj); RX[k].resize(nj); coll_time_[k].resize(nj); // Allocate algebraic variable expressions at the collocation points if(k!=nk){ Z[k].resize(nj-1); RZ[k].resize(nj-1); } // For all time points for(int j=0; j<nj; ++j){ // Get expressions for the differential state X[k][j] = V[range(offset,offset+nx_)]; offset += nx_; RX[k][j] = V[range(offset,offset+nrx_)]; offset += nrx_; // Get the local time coll_time_[k][j] = h*(k + tau_root[j]); // Get expressions for the algebraic variables if(j>0){ Z[k][j-1] = V[range(offset,offset+nz_)]; offset += nz_; RZ[k][j-1] = V[range(offset,offset+nrz_)]; offset += nrz_; } } } // Check offset for consistency casadi_assert(offset==V.size()); // Constraints vector<MX> g; g.reserve(2*(nk+1)); // Quadrature expressions MX QF = MX::zeros(nq_); MX RQF = MX::zeros(nrq_); // Counter int jk = 0; // Add initial condition g.push_back(X[0][0]-X0); // For all finite elements for(int k=0; k<nk; ++k, ++jk){ // For all collocation points for(int j=1; j<deg+1; ++j, ++jk){ // Get the time MX tkj = coll_time_[k][j]; // Get an expression for the state derivative at the collocation point MX xp_jk = 0; for(int j2=0; j2<deg+1; ++j2){ xp_jk += C[j2][j]*X[k][j2]; } // Add collocation equations to the NLP vector<MX> f_in(DAE_NUM_IN); f_in[DAE_T] = tkj; f_in[DAE_P] = P; f_in[DAE_X] = X[k][j]; f_in[DAE_Z] = Z[k][j-1]; vector<MX> f_out; if(explicit_ode){ // Assume equation of the form ydot = f(t,y,p) f_out = f_.call(f_in); g.push_back(h_mx*f_out[DAE_ODE] - xp_jk); } else { // Assume equation of the form 0 = f(t,y,ydot,p) f_in[DAE_XDOT] = xp_jk/h_mx; f_out = f_.call(f_in); g.push_back(f_out[DAE_ODE]); } // Add the algebraic conditions if(nz_>0){ g.push_back(f_out[DAE_ALG]); } // Add the quadrature if(nq_>0){ QF += D[j]*h_mx*f_out[DAE_QUAD]; } // Now for the backward problem if(nrx_>0){ // Get an expression for the state derivative at the collocation point MX rxp_jk = 0; for(int j2=0; j2<deg+1; ++j2){ rxp_jk += C[j2][j]*RX[k][j2]; } // Add collocation equations to the NLP vector<MX> g_in(RDAE_NUM_IN); g_in[RDAE_T] = tkj; g_in[RDAE_X] = X[k][j]; g_in[RDAE_Z] = Z[k][j-1]; g_in[RDAE_P] = P; g_in[RDAE_RP] = RP; g_in[RDAE_RX] = RX[k][j]; g_in[RDAE_RZ] = RZ[k][j-1]; vector<MX> g_out; if(explicit_ode){ // Assume equation of the form xdot = f(t,x,p) g_out = g_.call(g_in); g.push_back(h_mx*g_out[RDAE_ODE] - rxp_jk); } else { // Assume equation of the form 0 = f(t,x,xdot,p) g_in[RDAE_XDOT] = xp_jk/h_mx; g_in[RDAE_RXDOT] = rxp_jk/h_mx; g_out = g_.call(g_in); g.push_back(g_out[RDAE_ODE]); } // Add the algebraic conditions if(nrz_>0){ g.push_back(g_out[RDAE_ALG]); } // Add the backward quadrature if(nrq_>0){ RQF += D[j]*h_mx*g_out[RDAE_QUAD]; } } } // Get an expression for the state at the end of the finite element MX xf_k = 0; for(int j=0; j<deg+1; ++j){ xf_k += D[j]*X[k][j]; } // Add continuity equation to NLP g.push_back(X[k+1][0] - xf_k); if(nrx_>0){ // Get an expression for the state at the end of the finite element MX rxf_k = 0; for(int j=0; j<deg+1; ++j){ rxf_k += D[j]*RX[k][j]; } // Add continuity equation to NLP g.push_back(RX[k+1][0] - rxf_k); } } // Add initial condition for the backward integration if(nrx_>0){ g.push_back(RX[nk][0]-RX0); } // Constraint expression MX gv = vertcat(g); // Make sure that the dimension is consistent with the number of unknowns casadi_assert_message(gv.size()==V.size(),"Implicit function unknowns and equations do not match"); // Nonlinear constraint function input vector<MX> gfcn_in(1+INTEGRATOR_NUM_IN); gfcn_in[0] = V; gfcn_in[1+INTEGRATOR_X0] = X0; gfcn_in[1+INTEGRATOR_P] = P; gfcn_in[1+INTEGRATOR_RX0] = RX0; gfcn_in[1+INTEGRATOR_RP] = RP; vector<MX> gfcn_out(1+INTEGRATOR_NUM_OUT); gfcn_out[0] = gv; gfcn_out[1+INTEGRATOR_XF] = X[nk][0]; gfcn_out[1+INTEGRATOR_QF] = QF; gfcn_out[1+INTEGRATOR_RXF] = RX[0][0]; gfcn_out[1+INTEGRATOR_RQF] = RQF; // Nonlinear constraint function FX gfcn = MXFunction(gfcn_in,gfcn_out); // Expand f? bool expand_f = getOption("expand_f"); if(expand_f){ gfcn.init(); gfcn = SXFunction(shared_cast<MXFunction>(gfcn)); } // Get the NLP creator function implicitFunctionCreator implicit_function_creator = getOption("implicit_solver"); // Allocate an NLP solver implicit_solver_ = implicit_function_creator(gfcn); // Pass options if(hasSetOption("implicit_solver_options")){ const Dictionary& implicit_solver_options = getOption("implicit_solver_options"); implicit_solver_.setOption(implicit_solver_options); } // Initialize the solver implicit_solver_.init(); if(hasSetOption("startup_integrator")){ // Create the linear solver integratorCreator startup_integrator_creator = getOption("startup_integrator"); // Allocate an NLP solver startup_integrator_ = startup_integrator_creator(f_,g_); // Pass options startup_integrator_.setOption("number_of_fwd_dir",0); // not needed startup_integrator_.setOption("number_of_adj_dir",0); // not needed startup_integrator_.setOption("t0",coll_time_.front().front()); startup_integrator_.setOption("tf",coll_time_.back().back()); if(hasSetOption("startup_integrator_options")){ const Dictionary& startup_integrator_options = getOption("startup_integrator_options"); startup_integrator_.setOption(startup_integrator_options); } // Initialize the startup integrator startup_integrator_.init(); } // Mark the system not yet integrated integrated_once_ = false; }