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 Diagsplit::evalAdj(const std::vector<pv_MX>& adjSeed, const std::vector<pv_MX>& adjSens) { int nadj = adjSens.size(); int nx = offset_.size()-1; // Get offsets vector<int> offset1; offset1.reserve(offset_.size()); offset1.push_back(0); vector<int> offset2; offset2.reserve(offset_.size()); offset2.push_back(0); for (std::vector<Sparsity>::const_iterator it=output_sparsity_.begin(); it!=output_sparsity_.end(); ++it) { offset1.push_back(offset1.back() + it->size1()); offset2.push_back(offset2.back() + it->size2()); } for (int d=0; d<nadj; ++d) { if (adjSens[d][0]!=0) { vector<MX> v; for (int i=0; i<nx; ++i) { MX* x_i = adjSeed[d][i]; if (x_i!=0) { v.push_back(*x_i); *x_i = MX(); } else { v.push_back(MX(output_sparsity_[i].shape())); } } adjSens[d][0]->addToSum(diagcat(v)); } } }
std::vector<Sparsity> LrDpleInternal::getSparsity(const std::map<std::string, std::vector<Sparsity> >& st, const std::vector< std::vector<int> > &Hs_) { // Chop-up the arguments std::vector<Sparsity> As, Vs, Cs, Hs; if (st.count("a")) As = st.at("a"); if (st.count("v")) Vs = st.at("v"); if (st.count("c")) Cs = st.at("c"); if (st.count("h")) Hs = st.at("h"); bool with_H = !Hs.empty(); int K = As.size(); Sparsity A; if (K==1) { A = As[0]; } else { Sparsity AL = diagcat(vector_slice(As, range(As.size()-1))); Sparsity AL2 = horzcat(AL, Sparsity(AL.size1(), As[0].size2())); Sparsity AT = horzcat(Sparsity(As[0].size1(), AL.size2()), As.back()); A = vertcat(AT, AL2); } Sparsity V = diagcat(Vs.back(), diagcat(vector_slice(Vs, range(Vs.size()-1)))); Sparsity C; if (!Cs.empty()) { C = diagcat(Cs.back(), diagcat(vector_slice(Cs, range(Cs.size()-1)))); } Sparsity H; std::vector<int> Hs_agg; std::vector<int> Hi(1, 0); if (Hs_.size()>0 && with_H) { H = diagcat(Hs.back(), diagcat(vector_slice(Hs, range(Hs.size()-1)))); casadi_assert(K==Hs_.size()); for (int k=0;k<K;++k) { Hs_agg.insert(Hs_agg.end(), Hs_[k].begin(), Hs_[k].end()); int sum=0; for (int i=0;i<Hs_[k].size();++i) { sum+= Hs_[k][i]; } Hi.push_back(Hi.back()+sum); } } Sparsity res = LrDleInternal::getSparsity(make_map("a", A, "v", V, "c", C, "h", H), Hs_agg); if (with_H) { return diagsplit(res, Hi); } else { return diagsplit(res, As[0].size2()); } }
void Diagcat::evaluateMX(const MXPtrV& input, MXPtrV& output, const MXPtrVV& fwdSeed, MXPtrVV& fwdSens, const MXPtrVV& adjSeed, MXPtrVV& adjSens, bool output_given) { int nfwd = fwdSens.size(); int nadj = adjSeed.size(); // Non-differentiated output if (!output_given) { *output[0] = diagcat(getVector(input)); } // Forward sensitivities for (int d = 0; d<nfwd; ++d) { *fwdSens[d][0] = diagcat(getVector(fwdSeed[d])); } // Quick return? if (nadj==0) return; // Get offsets for each row and column vector<int> offset1(ndep()+1, 0); vector<int> offset2(ndep()+1, 0); for (int i=0; i<ndep(); ++i) { int ncol = dep(i).sparsity().size2(); int nrow = dep(i).sparsity().size1(); offset2[i+1] = offset2[i] + ncol; offset1[i+1] = offset1[i] + nrow; } // Adjoint sensitivities for (int d=0; d<nadj; ++d) { MX& aseed = *adjSeed[d][0]; vector<MX> s = diagsplit(aseed, offset1, offset2); aseed = MX(); for (int i=0; i<ndep(); ++i) { adjSens[d][i]->addToSum(s[i]); } } }
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(); }
void StabilizedQpToQp::init() { // Initialize the base classes StabilizedQpSolverInternal::init(); // Form augmented QP Sparsity H_sparsity_qp = diagcat(st_[QP_STRUCT_H], Sparsity::diag(nc_)); Sparsity A_sparsity_qp = horzcat(st_[QP_STRUCT_A], Sparsity::diag(nc_)); std::string qp_solver_name = getOption("qp_solver"); qp_solver_ = QpSolver(qp_solver_name, qpStruct("h", H_sparsity_qp, "a", A_sparsity_qp)); // Pass options if provided if (hasSetOption("qp_solver_options")) { Dictionary qp_solver_options = getOption("qp_solver_options"); qp_solver_.setOption(qp_solver_options); } // Initialize the QP solver qp_solver_.init(); }
void Diagsplit::evalAdj(const std::vector<std::vector<MX> >& aseed, std::vector<std::vector<MX> >& asens) { int nadj = asens.size(); // Get offsets vector<int> offset1; offset1.reserve(offset_.size()); offset1.push_back(0); vector<int> offset2; offset2.reserve(offset_.size()); offset2.push_back(0); for (std::vector<Sparsity>::const_iterator it=output_sparsity_.begin(); it!=output_sparsity_.end(); ++it) { offset1.push_back(offset1.back() + it->size1()); offset2.push_back(offset2.back() + it->size2()); } for (int d=0; d<nadj; ++d) { asens[d][0] += diagcat(aseed[d]); } }
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 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 LrDpleInternal::init() { const_dim_ = getOption("const_dim"); pos_def_ = getOption("pos_def"); error_unstable_ = getOption("error_unstable"); eps_unstable_ = getOption("eps_unstable"); Hs_ = getOption("Hs"); casadi_assert(const_dim_); A_ = st_[LR_Dple_STRUCT_A]; casadi_assert(A_.size()>0); int n = A_[0].size1(); V_ = st_[LR_Dple_STRUCT_V]; C_ = st_[LR_Dple_STRUCT_C]; H_ = st_[LR_Dple_STRUCT_H]; with_H_ = true; if (H_.empty()) { with_H_ = false; } with_C_ = true; if (C_.empty()) { with_C_ = false; } if (with_H_) { casadi_assert_message(A_.size()==H_.size(), "A and H arguments must be of same length, but got " << A_.size() << " and " << H_.size() << "."); casadi_assert_message(H_.size()==Hs_.size(), "H and Hs arguments must be of same length, but got " << H_.size() << " and " << Hs_.size() << "."); Hsi_.resize(1, 0); for (int k=0;k<H_.size();++k) { // Default hs: [H.size2()] if (Hs_[k].size()==0) { Hs_[k].push_back(H_[k].size2()); } // Assert that sum of Hs entries match up to H.size2() double sum = 0; for (int i=0;i<Hs_[k].size();++i) { sum += Hs_[k][i]; } Hsi_.push_back(Hsi_.back()+sum); casadi_assert_message(H_[k].size2()==sum, "Number of columns in H @ k = " << k << " (" << H_[k].size2() << ")," << "must match sum(Hs[k]): " << sum << "."); } } // Dimension sanity checks casadi_assert_message(A_.size()==V_.size(), "A and V arguments must be of same length, but got " << A_.size() << " and " << V_.size() << "."); K_ = A_.size(); int m = with_C_? V_[0].size1(): n; for (int k=0;k<K_;++k) { casadi_assert_message(V_[k].issymmetric(), "V_i must be symmetric but got " << V_[k].dimString() << " for i = " << k << "."); if (with_C_) { casadi_assert_message(n==C_[k].size1(), "Number of rows in C (" << C_[k].size1() << ") must match dimension of square A @ k = " << k << " (" << n << ")" << "."); casadi_assert_message(m==C_[k].size2(), "Number of columns in C (" << C_[k].size2() << ") must match dimension of symmetric V @ k = " << k << " (" << m << ")" << "."); } else { casadi_assert_message(A_[k].size1()==V_[k].size1(), "First dimension of A (" << A_[k].size1() << ") must match dimension of symmetric V @ k = " << k << " (" << V_[k].size1() << ")" << "."); } } if (const_dim_) { int n = A_[0].size1(); for (int k=1;k<K_;++k) { casadi_assert_message(A_[k].size1()==n, "You have set const_dim option, but found " "an A_i with dimension ( " << A_[k].dimString() << " ) deviating from n = " << n << " at i = " << k << "."); } } Hss_.resize(0); if (Hs_.size()>0) { for (int k=0;k<K_;++k) { Hss_.insert(Hss_.end(), Hs_[k].begin(), Hs_[k].end()); } } // Allocate inputs ibuf_.resize(LR_DPLE_NUM_IN); if (const_dim_) { input(LR_DPLE_A) = DMatrix::zeros(horzcat(A_)); if (with_C_) { input(LR_DPLE_C) = DMatrix::zeros(horzcat(C_)); } input(LR_DPLE_V) = DMatrix::zeros(horzcat(V_)); if (with_H_) { input(LR_DPLE_H) = DMatrix::zeros(horzcat(H_)); } } else { input(LR_DPLE_A) = DMatrix::zeros(diagcat(A_)); } /**for (int i=0;i<nrhs_;++i) { if (const_dim_) { input(1+i) = DMatrix::zeros(horzcat(V_)); } else { input(1+i) = DMatrix::zeros(diagcat(V_)); } }*/ // Allocate outputs std::map<std::string, std::vector<Sparsity> > tmp; tmp["a"] = st_[LR_Dple_STRUCT_A]; tmp["v"] = st_[LR_Dple_STRUCT_V]; tmp["c"] = st_[LR_Dple_STRUCT_C]; tmp["h"] = st_[LR_Dple_STRUCT_H]; std::vector<Sparsity> P = getSparsity(tmp, Hs_); obuf_.resize(nrhs_*LR_DPLE_NUM_OUT); for (int i=0;i<nrhs_;++i) { if (const_dim_) { output(i) = DMatrix::zeros(horzcat(P)); } else { output(i) = DMatrix::zeros(diagcat(P)); } } FunctionInternal::init(); }
void DpleInternal::init() { const_dim_ = getOption("const_dim"); pos_def_ = getOption("pos_def"); error_unstable_ = getOption("error_unstable"); eps_unstable_ = getOption("eps_unstable"); A_ = st_[Dple_STRUCT_A]; V_ = st_[Dple_STRUCT_V]; // Dimension sanity checks casadi_assert_message(A_.size()==V_.size(), "A and V arguments must be of same length, but got " << A_.size() << " and " << V_.size() << "."); K_ = A_.size(); for (int k=0;k<K_;++k) { casadi_assert_message(V_[k].issymmetric(), "V_i must be symmetric but got " << V_[k].dimString() << " for i = " << k << "."); casadi_assert_message(A_[k].size1()==V_[k].size1(), "First dimension of A (" << A_[k].size1() << ") must match dimension of symmetric V_i (" << V_[k].size1() << ")" << " for i = " << k << "."); } if (const_dim_) { int n = A_[0].size1(); for (int k=1;k<K_;++k) { casadi_assert_message(A_[k].size1()==n, "You have set const_dim option, but found " "an A_i with dimension ( " << A_[k].dimString() << " ) deviating from n = " << n << " at i = " << k << "."); } } // Allocate inputs ibuf_.resize(1+nrhs_); if (const_dim_) { input(0) = DMatrix::zeros(horzcat(A_)); } else { input(0) = DMatrix::zeros(diagcat(A_)); } for (int i=0;i<nrhs_;++i) { if (const_dim_) { input(1+i) = DMatrix::zeros(horzcat(V_)); } else { input(1+i) = DMatrix::zeros(diagcat(V_)); } } // Allocate outputs std::map<std::string, std::vector<Sparsity> > tmp; tmp["a"] = A_; tmp["v"] = V_; std::vector<Sparsity> P = LrDpleInternal::getSparsity(tmp); obuf_.resize(nrhs_); for (int i=0;i<nrhs_;++i) { if (const_dim_) { output(i) = DMatrix::zeros(horzcat(P)); } else { output(i) = DMatrix::zeros(diagcat(P)); } } FunctionInternal::init(); }