bool BonminInterface:: get_starting_point(BonminMemory* m, bool init_x, double* x, bool init_z, double* z_L, double* z_U, bool init_lambda, double* lambda) const { try { // Initialize primal variables if (init_x) { casadi_copy(m->x, nx_, x); } // Initialize dual variables (simple bounds) if (init_z) { for (casadi_int i=0; i<nx_; ++i) { z_L[i] = max(0., -m->lam_x[i]); z_U[i] = max(0., m->lam_x[i]); } } // Initialize dual variables (nonlinear bounds) if (init_lambda) { casadi_copy(m->lam_g, ng_, lambda); } return true; } catch(exception& ex) { uerr() << "get_starting_point failed: " << ex.what() << endl; return false; } }
void IdasInterface::retreat(IntegratorMemory* mem, double t, double* rx, double* rz, double* rq) const { auto m = to_mem(mem); // Integrate, unless already at desired time if (t<m->t) { THROWING(IDASolveB, m->mem, t, IDA_NORMAL); THROWING(IDAGetB, m->mem, m->whichB, &m->t, m->rxz, m->rxzdot); if (nrq_>0) { THROWING(IDAGetQuadB, m->mem, m->whichB, &m->t, m->rq); } } // Save outputs casadi_copy(NV_DATA_S(m->rxz), nrx_, rx); casadi_copy(NV_DATA_S(m->rxz)+nrx_, nrz_, rz); casadi_copy(NV_DATA_S(m->rq), nrq_, rq); // Get stats IDAMem IDA_mem = IDAMem(m->mem); IDAadjMem IDAADJ_mem = IDA_mem->ida_adj_mem; IDABMem IDAB_mem = IDAADJ_mem->IDAB_mem; THROWING(IDAGetIntegratorStats, IDAB_mem->IDA_mem, &m->nstepsB, &m->nfevalsB, &m->nlinsetupsB, &m->netfailsB, &m->qlastB, &m->qcurB, &m->hinusedB, &m->hlastB, &m->hcurB, &m->tcurB); }
bool BonminInterface:: get_bounds_info(BonminMemory* m, double* x_l, double* x_u, double* g_l, double* g_u) const { try { casadi_copy(m->lbx, nx_, x_l); casadi_copy(m->ubx, nx_, x_u); casadi_copy(m->lbg, ng_, g_l); casadi_copy(m->ubg, ng_, g_u); return true; } catch(exception& ex) { uerr() << "get_bounds_info failed: " << ex.what() << endl; return false; } }
void BonminInterface:: finalize_solution(BonminMemory* m, TMINLP::SolverReturn status, const double* x, double obj_value) const { try { // Get primal solution casadi_copy(x, nx_, m->x); // Get optimal cost m->f = obj_value; // Dual solution not calculated casadi_fill(m->lam_x, nx_, nan); casadi_fill(m->lam_g, ng_, nan); // Get the constraints casadi_fill(m->gk, ng_, nan); // Get statistics m->iter_count = 0; // Interpret return code m->return_status = return_status_string(status); m->success = status==Bonmin::TMINLP::SUCCESS; } catch(exception& ex) { uerr() << "finalize_solution failed: " << ex.what() << endl; } }
void casadi_bfgs(const casadi_int* sp_h, T1* h, const T1* dx, const T1* glag, const T1* glag_old, T1* w) { // Local variables casadi_int nx; T1 *yk, *qk, dxBkdx, omega, theta, phi; // Dimension nx = sp_h[0]; // Work vectors yk = w; w += nx; qk = w; w += nx; // yk = glag - glag_old casadi_copy(glag, nx, yk); casadi_axpy(nx, -1., glag_old, yk); // qk = H*dx casadi_fill(qk, nx, 0.); casadi_mv(h, sp_h, dx, qk, 0); // Calculating theta dxBkdx = casadi_dot(nx, dx, qk); // C-REPLACE "if_else" "casadi_if_else" omega = if_else(casadi_dot(nx, yk, dx) < 0.2 * casadi_dot(nx, dx, qk), 0.8 * dxBkdx / (dxBkdx - casadi_dot(nx, dx, yk)), 1); // yk = omega * yk + (1 - omega) * qk; casadi_scal(nx, omega, yk); casadi_axpy(nx, 1 - omega, qk, yk); theta = 1. / casadi_dot(nx, dx, yk); phi = 1. / casadi_dot(nx, qk, dx); // Update H casadi_rank1(h, sp_h, theta, yk, yk); casadi_rank1(h, sp_h, -phi, qk, qk); }
void BonminInterface:: finalize_solution(BonminMemory* m, const double* x, double obj_value) const { try { // Get primal solution casadi_copy(x, nx_, m->xk); // Get optimal cost m->fk = obj_value; // Get dual solution (simple bounds) if (m->lam_xk) { for (int i=0; i<nx_; ++i) { m->lam_xk[i] = casadi::nan; } } // Get dual solution (nonlinear bounds) casadi_fill(m->lam_gk, ng_, nan); // Get the constraints casadi_fill(m->gk, ng_, nan); // Get statistics m->iter_count = 0; } catch(exception& ex) { userOut<true, PL_WARN>() << "finalize_solution failed: " << ex.what() << endl; } }
void SundialsInterface::resetB(IntegratorMemory* mem, double t, const double* rx, const double* rz, const double* rp) const { auto m = static_cast<SundialsMemory*>(mem); // Update time m->t = t; // Set parameters casadi_copy(rp, nrp_, m->rp); // Get the backward state casadi_copy(rx, nrx_, NV_DATA_S(m->rxz)); // Reset summation states N_VConst(0., m->rq); }
void SundialsInterface::reset(IntegratorMemory* mem, double t, const double* x, const double* z, const double* p) const { auto m = static_cast<SundialsMemory*>(mem); // Update time m->t = t; // Set parameters casadi_copy(p, np_, m->p); // Set the state casadi_copy(x, nx_, NV_DATA_S(m->xz)); casadi_copy(z, nz_, NV_DATA_S(m->xz)+nx_); // Reset summation states N_VConst(0., m->q); }
void Callback::eval(const double** arg, double** res, int* iw, double* w, int mem) { // Allocate input matrices int n_in = this->n_in(); std::vector<DM> argv(n_in); for (int i=0; i<n_in; ++i) { argv[i] = DM(sparsity_in(i)); casadi_copy(arg[i], argv[i].nnz(), argv[i].ptr()); } // Evaluate std::vector<DM> resv = eval(argv); // Get the outputs int n_out = this->n_out(); for (int i=0; i<n_out; ++i) { casadi_copy(resv[i].ptr(), resv[i].nnz(), res[i]); } }
void IdasInterface:: advance(IntegratorMemory* mem, double t, double* x, double* z, double* q) const { auto m = to_mem(mem); casadi_assert_message(t>=grid_.front(), "IdasInterface::integrate(" << t << "): " "Cannot integrate to a time earlier than t0 (" << grid_.front() << ")"); casadi_assert_message(t<=grid_.back() || !stop_at_end_, "IdasInterface::integrate(" << t << "): " "Cannot integrate past a time later than tf (" << grid_.back() << ") " "unless stop_at_end is set to False."); // Integrate, unless already at desired time double ttol = 1e-9; // tolerance if (fabs(m->t-t)>=ttol) { // Integrate forward ... if (nrx_>0) { // ... with taping THROWING(IDASolveF, m->mem, t, &m->t, m->xz, m->xzdot, IDA_NORMAL, &m->ncheck); } else { // ... without taping THROWING(IDASolve, m->mem, t, &m->t, m->xz, m->xzdot, IDA_NORMAL); } // Get quadratures if (nq_>0) { double tret; THROWING(IDAGetQuad, m->mem, &tret, m->q); } } // Set function outputs casadi_copy(NV_DATA_S(m->xz), nx_, x); casadi_copy(NV_DATA_S(m->xz)+nx_, nz_, z); casadi_copy(NV_DATA_S(m->q), nq_, q); // Get stats THROWING(IDAGetIntegratorStats, m->mem, &m->nsteps, &m->nfevals, &m->nlinsetups, &m->netfails, &m->qlast, &m->qcur, &m->hinused, &m->hlast, &m->hcur, &m->tcur); THROWING(IDAGetNonlinSolvStats, m->mem, &m->nniters, &m->nncfails); }
/* casadi_impl_ode_jac_xdot:(i0[3],i1[3],i2[4])->(o0[3x3]) */ static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, void* mem) { casadi_real *rr, *ss; casadi_real *w0=w+3, *w1=w+6, *w2=w+9; /* #0: @0 = zeros(3x3,3nz) */ casadi_fill(w0, 3, 0.); /* #1: @1 = ones(3x1) */ casadi_fill(w1, 3, 1.); /* #2: (@0[:3] = @1) */ for (rr=w0+0, ss=w1; rr!=w0+3; rr+=1) *rr = *ss++; /* #3: @1 = @0' */ casadi_trans(w0,casadi_s0, w1, casadi_s0, iw); /* #4: @2 = dense(@1) */ casadi_project(w1, casadi_s0, w2, casadi_s1, w); /* #5: output[0][0] = @2 */ casadi_copy(w2, 9, res[0]); return 0; }
bool BonminInterface:: intermediate_callback(BonminMemory* m, const double* x, const double* z_L, const double* z_U, const double* g, const double* lambda, double obj_value, int iter, double inf_pr, double inf_du, double mu, double d_norm, double regularization_size, double alpha_du, double alpha_pr, int ls_trials, bool full_callback) const { m->n_iter += 1; try { if (verbose_) casadi_message("intermediate_callback started"); m->inf_pr.push_back(inf_pr); m->inf_du.push_back(inf_du); m->mu.push_back(mu); m->d_norm.push_back(d_norm); m->regularization_size.push_back(regularization_size); m->alpha_pr.push_back(alpha_pr); m->alpha_du.push_back(alpha_du); m->ls_trials.push_back(ls_trials); m->obj.push_back(obj_value); if (!fcallback_.is_null()) { m->fstats.at("callback_fun").tic(); if (full_callback) { casadi_copy(x, nx_, m->x); for (casadi_int i=0; i<nx_; ++i) { m->lam_x[i] = z_U[i]-z_L[i]; } casadi_copy(lambda, ng_, m->lam_g); casadi_copy(g, ng_, m->gk); } else { if (iter==0) { uerr() << "Warning: intermediate_callback is disfunctional in your installation. " "You will only be able to use stats(). " "See https://github.com/casadi/casadi/wiki/enableBonminCallback to enable it." << endl; } } // Inputs fill_n(m->arg, fcallback_.n_in(), nullptr); if (full_callback) { // The values used below are meaningless // when not doing a full_callback m->arg[NLPSOL_X] = x; m->arg[NLPSOL_F] = &obj_value; m->arg[NLPSOL_G] = g; m->arg[NLPSOL_LAM_P] = nullptr; m->arg[NLPSOL_LAM_X] = m->lam_x; m->arg[NLPSOL_LAM_G] = m->lam_g; } // Outputs fill_n(m->res, fcallback_.n_out(), nullptr); double ret_double; m->res[0] = &ret_double; fcallback_(m->arg, m->res, m->iw, m->w, 0); int ret = static_cast<int>(ret_double); m->fstats.at("callback_fun").toc(); return !ret; } else { return 1; } } catch(KeyboardInterruptException& ex) { return 0; } catch(exception& ex) { if (iteration_callback_ignore_errors_) { uerr() << "intermediate_callback: " << ex.what() << endl; return 1; } return 0; } }
int BonminInterface::solve(void* mem) const { auto m = static_cast<BonminMemory*>(mem); // Reset statistics m->inf_pr.clear(); m->inf_du.clear(); m->mu.clear(); m->d_norm.clear(); m->regularization_size.clear(); m->alpha_pr.clear(); m->alpha_du.clear(); m->obj.clear(); m->ls_trials.clear(); // Reset number of iterations m->n_iter = 0; // MINLP instance SmartPtr<BonminUserClass> tminlp = new BonminUserClass(*this, m); BonMinMessageHandler mh; // Start an BONMIN application BonminSetup bonmin(&mh); SmartPtr<OptionsList> options = new OptionsList(); SmartPtr<Journalist> journalist= new Journalist(); SmartPtr<Bonmin::RegisteredOptions> roptions = new Bonmin::RegisteredOptions(); { // Direct output through casadi::uout() StreamJournal* jrnl_raw = new StreamJournal("console", J_ITERSUMMARY); jrnl_raw->SetOutputStream(&casadi::uout()); jrnl_raw->SetPrintLevel(J_DBG, J_NONE); SmartPtr<Journal> jrnl = jrnl_raw; journalist->AddJournal(jrnl); } options->SetJournalist(journalist); options->SetRegisteredOptions(roptions); bonmin.setOptionsAndJournalist(roptions, options, journalist); bonmin.registerOptions(); // Get all options available in BONMIN auto regops = bonmin.roptions()->RegisteredOptionsList(); // Pass all the options to BONMIN for (auto&& op : opts_) { // Find the option auto regops_it = regops.find(op.first); if (regops_it==regops.end()) { casadi_error("No such BONMIN option: " + op.first); } // Get the type Ipopt::RegisteredOptionType ipopt_type = regops_it->second->Type(); // Pass to BONMIN bool ret; switch (ipopt_type) { case Ipopt::OT_Number: ret = bonmin.options()->SetNumericValue(op.first, op.second.to_double(), false); break; case Ipopt::OT_Integer: ret = bonmin.options()->SetIntegerValue(op.first, op.second.to_int(), false); break; case Ipopt::OT_String: ret = bonmin.options()->SetStringValue(op.first, op.second.to_string(), false); break; case Ipopt::OT_Unknown: default: casadi_warning("Cannot handle option \"" + op.first + "\", ignored"); continue; } if (!ret) casadi_error("Invalid options were detected by BONMIN."); } // Initialize bonmin.initialize(GetRawPtr(tminlp)); if (true) { // Branch-and-bound try { Bab bb; bb(bonmin); } catch (CoinError& e) { casadi_error("CoinError occured: " + to_str(e)); } } // Save results to outputs casadi_copy(m->gk, ng_, m->g); return 0; }
void BonminInterface::solve(void* mem) const { auto m = static_cast<BonminMemory*>(mem); // Check the provided inputs checkInputs(mem); // Reset statistics m->inf_pr.clear(); m->inf_du.clear(); m->mu.clear(); m->d_norm.clear(); m->regularization_size.clear(); m->alpha_pr.clear(); m->alpha_du.clear(); m->obj.clear(); m->ls_trials.clear(); // Reset number of iterations m->n_iter = 0; // Statistics for (auto&& s : m->fstats) s.second.reset(); // MINLP instance SmartPtr<BonminUserClass> tminlp = new BonminUserClass(*this, m); BonMinMessageHandler mh; // Start an BONMIN application BonminSetup bonmin(&mh); SmartPtr<OptionsList> options = new OptionsList(); SmartPtr<Journalist> journalist= new Journalist(); SmartPtr<Bonmin::RegisteredOptions> roptions = new Bonmin::RegisteredOptions(); { // Direct output through casadi::userOut() StreamJournal* jrnl_raw = new StreamJournal("console", J_ITERSUMMARY); jrnl_raw->SetOutputStream(&casadi::userOut()); jrnl_raw->SetPrintLevel(J_DBG, J_NONE); SmartPtr<Journal> jrnl = jrnl_raw; journalist->AddJournal(jrnl); } options->SetJournalist(journalist); options->SetRegisteredOptions(roptions); bonmin.setOptionsAndJournalist(roptions, options, journalist); bonmin.registerOptions(); // Initialize bonmin.initialize(GetRawPtr(tminlp)); m->fstats.at("mainloop").tic(); if (true) { // Branch-and-bound Bab bb; bb(bonmin); } //m->return_status = return_status_string(status); m->fstats.at("mainloop").toc(); // Save results to outputs casadi_copy(&m->fk, 1, m->f); casadi_copy(m->xk, nx_, m->x); casadi_copy(m->lam_gk, ng_, m->lam_g); casadi_copy(m->lam_xk, nx_, m->lam_x); casadi_copy(m->gk, ng_, m->g); }
void SnoptInterface::solve(void* mem) const { auto m = static_cast<SnoptMemory*>(mem); // Check the provided inputs checkInputs(mem); m->fstats.at("mainloop").tic(); // Memory object snProblem prob; // Evaluate gradF and jacG at initial value const double** arg = m->arg; *arg++ = m->x0; *arg++ = m->p; double** res = m->res; *res++ = 0; *res++ = m->jac_gk; calc_function(m, "nlp_jac_g"); res = m->res; *res++ = 0; *res++ = m->jac_fk; calc_function(m, "nlp_jac_f"); // perform the mapping: // populate A_data_ (the nonzeros of A) // with numbers pulled from jacG and gradF for (int k = 0; k < A_structure_.nnz(); ++k) { int i = A_structure_.nonzeros()[k]; if (i == 0) { m->A_data[k] = 0; } else if (i > 0) { m->A_data[k] = m->jac_gk[i-1]; } else { m->A_data[k] = m->jac_fk[-i-1]; } } int n = nx_; int nea = A_structure_.nnz(); double ObjAdd = 0; casadi_assert(m_ > 0); casadi_assert(n > 0); casadi_assert(nea > 0); casadi_assert(A_structure_.nnz() == nea); // Pointer magic, courtesy of Greg casadi_assert_message(!jac_f_fcn_.is_null(), "blaasssshc"); // Outputs //double Obj = 0; // TODO(Greg): get this from snopt // snInit must be called first. // 9, 6 are print and summary unit numbers (for Fortran). // 6 == standard out int iprint = 9; int isumm = 6; std::string outname = name_ + ".out"; snInit(&prob, const_cast<char*>(name_.c_str()), const_cast<char*>(outname.c_str()), iprint, isumm); // Set the problem size and other data. // This will allocate arrays inside snProblem struct. setProblemSize(&prob, m_, nx_, nea, nnCon_, nnJac_, nnObj_); setObjective(&prob, iObj_, ObjAdd); setUserfun(&prob, userfunPtr); // user data prob.leniu = 1; prob.iu = &m->memind; // Pass bounds casadi_copy(m->lbx, nx_, prob.bl); casadi_copy(m->ubx, nx_, prob.bu); casadi_copy(m->lbg, ng_, prob.bl + nx_); casadi_copy(m->ubg, ng_, prob.bu + nx_); // Initialize states and slack casadi_fill(prob.hs, ng_ + nx_, 0); casadi_copy(m->x0, nx_, prob.x); casadi_fill(prob.x + nx_, ng_, 0.); // Initialize multipliers casadi_copy(m->lam_g0, ng_, prob.pi); // Set up Jacobian matrix casadi_copy(A_structure_.colind(), A_structure_.size2()+1, prob.locJ); casadi_copy(A_structure_.row(), A_structure_.nnz(), prob.indJ); casadi_copy(get_ptr(m->A_data), A_structure_.nnz(), prob.valJ); for (auto&& op : opts_) { // Replace underscores with spaces std::string opname = op.first; std::replace(opname.begin(), opname.end(), '_', ' '); // Try integer if (op.second.can_cast_to(OT_INT)) { casadi_assert(opname.size() <= 55); int flag = setIntParameter(&prob, const_cast<char*>(opname.c_str()), op.second.to_int()); if (flag==0) continue; } // Try double if (op.second.can_cast_to(OT_DOUBLE)) { casadi_assert(opname.size() <= 55); int flag = setRealParameter(&prob, const_cast<char*>(opname.c_str()), op.second.to_double()); if (flag==0) continue; } // try string if (op.second.can_cast_to(OT_STRING)) { std::string buffer = opname + " " + op.second.to_string(); casadi_assert(buffer.size() <= 72); int flag = setParameter(&prob, const_cast<char*>(buffer.c_str())); if (flag==0) continue; } // Error if reached this point casadi_error("SNOPT error setting option \"" + opname + "\""); } m->fstats.at("mainloop").toc(); // Run SNOPT int info = solveC(&prob, Cold_, &m->fk); casadi_assert_message(99 != info, "snopt problem set up improperly"); // Negate rc to match CasADi's definition casadi_scal(nx_ + ng_, -1., prob.rc); // Get primal solution casadi_copy(prob.x, nx_, m->x); // Get dual solution casadi_copy(prob.rc, nx_, m->lam_x); casadi_copy(prob.rc+nx_, ng_, m->lam_g); // Copy optimal cost to output if (m->f) *m->f = m->fk; // Copy optimal constraint values to output casadi_copy(m->gk, ng_, m->g); // Free memory deleteSNOPT(&prob); }
void Newton::solve(void* mem) const { auto m = static_cast<NewtonMemory*>(mem); // Get the initial guess casadi_copy(m->iarg[iin_], n_, m->x); // Perform the Newton iterations m->iter=0; bool success = true; while (true) { // Break if maximum number of iterations already reached if (m->iter >= max_iter_) { log("eval", "Max. iterations reached."); m->return_status = "max_iteration_reached"; success = false; break; } // Start a new iteration m->iter++; // Use x to evaluate J copy_n(m->iarg, n_in(), m->arg); m->arg[iin_] = m->x; m->res[0] = m->jac; copy_n(m->ires, n_out(), m->res+1); m->res[1+iout_] = m->f; calc_function(m, "jac_f_z"); // Check convergence double abstol = 0; if (abstol_ != numeric_limits<double>::infinity()) { for (int i=0; i<n_; ++i) { abstol = max(abstol, fabs(m->f[i])); } if (abstol <= abstol_) { casadi_msg("Converged to acceptable tolerance - abstol: " << abstol_); break; } } // Factorize the linear solver with J linsol_.factorize(m->jac); linsol_.solve(m->f, 1, false); // Check convergence again double abstolStep=0; if (numeric_limits<double>::infinity() != abstolStep_) { for (int i=0; i<n_; ++i) { abstolStep = max(abstolStep, fabs(m->f[i])); } if (abstolStep <= abstolStep_) { casadi_msg("Converged to acceptable tolerance - abstolStep: " << abstolStep_); break; } } if (print_iteration_) { // Only print iteration header once in a while if (m->iter % 10==0) { printIteration(userOut()); } // Print iteration information printIteration(userOut(), m->iter, abstol, abstolStep); } // Update Xk+1 = Xk - J^(-1) F casadi_axpy(n_, -1., m->f, m->x); } // Get the solution casadi_copy(m->x, n_, m->ires[iout_]); // Store the iteration count if (success) m->return_status = "success"; casadi_msg("Newton::solveNonLinear():end after " << m->iter << " steps"); }
int IdasInterface::psolveB(double t, N_Vector xz, N_Vector xzdot, N_Vector xzB, N_Vector xzdotB, N_Vector resvalB, N_Vector rvecB, N_Vector zvecB, double cjB, double deltaB, void *user_data, N_Vector tmpB) { try { auto m = to_mem(user_data); auto& s = m->self; // Get right-hand sides in m->v1, ordered by sensitivity directions double* vx = NV_DATA_S(rvecB); double* vz = vx + s.nrx_; double* v_it = m->v1; for (int d=0; d<=s.ns_; ++d) { casadi_copy(vx + d*s.nrx1_, s.nrx1_, v_it); v_it += s.nrx1_; casadi_copy(vz + d*s.nrz1_, s.nrz1_, v_it); v_it += s.nrz1_; } // Solve for undifferentiated right-hand-side, save to output s.linsolB_.solve(m->v1, 1); vx = NV_DATA_S(zvecB); // possibly different from rvecB vz = vx + s.nrx_; casadi_copy(m->v1, s.nrx1_, vx); casadi_copy(m->v1 + s.nrx1_, s.nrz1_, vz); // Sensitivity equations if (s.ns_>0) { // Second order correction if (s.second_order_correction_) { // The outputs will double as seeds for jtimesB casadi_fill(vx + s.nrx1_, s.nrx_ - s.nrx1_, 0.); casadi_fill(vz + s.nrz1_, s.nrz_ - s.nrz1_, 0.); // Get second-order-correction, save to m->v2 m->arg[0] = &t; // t m->arg[1] = NV_DATA_S(xz); // x m->arg[2] = NV_DATA_S(xz)+s.nx_; // z m->arg[3] = m->p; // p m->arg[4] = NV_DATA_S(xzB); // rx m->arg[5] = NV_DATA_S(xzB)+s.nrx_; // rz m->arg[6] = m->rp; // rp m->arg[7] = vx; // fwd:rx m->arg[8] = vz; // fwd:rz m->res[0] = m->v2; // fwd:rode m->res[1] = m->v2 + s.nrx_; // fwd:ralg s.calc_function(m, "jtimesB"); // Subtract m->v2 (reordered) from m->v1 v_it = m->v1 + s.nrx1_ + s.nrz1_; for (int d=1; d<=s.ns_; ++d) { casadi_axpy(s.nrx1_, -1., m->v2 + d*s.nrx1_, v_it); v_it += s.nrx1_; casadi_axpy(s.nrz1_, -1., m->v2 + s.nrx_ + d*s.nrz1_, v_it); v_it += s.nrz1_; } } // Solve for sensitivity right-hand-sides s.linsolB_.solve(m->v1 + s.nrx1_ + s.nrz1_, s.ns_); // Save to output, reordered v_it = m->v1 + s.nrx1_ + s.nrz1_; for (int d=1; d<=s.ns_; ++d) { casadi_copy(v_it, s.nrx1_, vx + d*s.nrx1_); v_it += s.nrx1_; casadi_copy(v_it, s.nrz1_, vz + d*s.nrz1_); v_it += s.nrz1_; } } return 0; } catch(int flag) { // recoverable error return flag; } catch(exception& e) { // non-recoverable error userOut<true, PL_WARN>() << "psolveB failed: " << e.what() << endl; return -1; } }
int WorhpInterface::solve(void* mem) const { auto m = static_cast<WorhpMemory*>(mem); if (m->lbg && m->ubg) { for (casadi_int i=0; i<ng_; ++i) { casadi_assert(!(m->lbg[i]==-inf && m->ubg[i] == inf), "WorhpInterface::evaluate: Worhp cannot handle the case when both " "LBG and UBG are infinite." "You have that case at non-zero " + str(i)+ "." "Reformulate your problem eliminating the corresponding constraint."); } } // Pass inputs to WORHP data structures casadi_copy(m->x, nx_, m->worhp_o.X); casadi_copy(m->lbx, nx_, m->worhp_o.XL); casadi_copy(m->ubx, nx_, m->worhp_o.XU); casadi_copy(m->lam_x, nx_, m->worhp_o.Lambda); if (m->worhp_o.m>0) { casadi_copy(m->lam_g, ng_, m->worhp_o.Mu); casadi_copy(m->lbg, ng_, m->worhp_o.GL); casadi_copy(m->ubg, ng_, m->worhp_o.GU); } // Replace infinite bounds with m->worhp_p.Infty double inf = numeric_limits<double>::infinity(); for (casadi_int i=0; i<nx_; ++i) if (m->worhp_o.XL[i]==-inf) m->worhp_o.XL[i] = -m->worhp_p.Infty; for (casadi_int i=0; i<nx_; ++i) if (m->worhp_o.XU[i]== inf) m->worhp_o.XU[i] = m->worhp_p.Infty; for (casadi_int i=0; i<ng_; ++i) if (m->worhp_o.GL[i]==-inf) m->worhp_o.GL[i] = -m->worhp_p.Infty; for (casadi_int i=0; i<ng_; ++i) if (m->worhp_o.GU[i]== inf) m->worhp_o.GU[i] = m->worhp_p.Infty; if (verbose_) casadi_message("WorhpInterface::starting iteration"); bool firstIteration = true; // Reverse Communication loop while (m->worhp_c.status < TerminateSuccess && m->worhp_c.status > TerminateError) { if (GetUserAction(&m->worhp_c, callWorhp)) { Worhp(&m->worhp_o, &m->worhp_w, &m->worhp_p, &m->worhp_c); } if (GetUserAction(&m->worhp_c, iterOutput)) { if (!firstIteration) { firstIteration = true; if (!fcallback_.is_null()) { m->iter = m->worhp_w.MajorIter; m->iter_sqp = m->worhp_w.MinorIter; m->inf_pr = m->worhp_w.NormMax_CV; m->inf_du = m->worhp_p.ScaledKKT; m->alpha_pr = m->worhp_w.ArmijoAlpha; // Inputs fill_n(m->arg, fcallback_.n_in(), nullptr); m->arg[NLPSOL_X] = m->worhp_o.X; m->arg[NLPSOL_F] = &m->worhp_o.F; m->arg[NLPSOL_G] = m->worhp_o.G; m->arg[NLPSOL_LAM_P] = nullptr; m->arg[NLPSOL_LAM_X] = m->worhp_o.Lambda; m->arg[NLPSOL_LAM_G] = m->worhp_o.Mu; // Outputs fill_n(m->res, fcallback_.n_out(), nullptr); double ret_double; m->res[0] = &ret_double; m->fstats.at("callback_fun").tic(); // Evaluate the callback function fcallback_(m->arg, m->res, m->iw, m->w, 0); m->fstats.at("callback_fun").toc(); casadi_int ret = static_cast<casadi_int>(ret_double); if (ret) m->worhp_c.status = TerminateError; } } IterationOutput(&m->worhp_o, &m->worhp_w, &m->worhp_p, &m->worhp_c); DoneUserAction(&m->worhp_c, iterOutput); } if (GetUserAction(&m->worhp_c, evalF)) { m->arg[0] = m->worhp_o.X; m->arg[1] = m->p; m->res[0] = &m->worhp_o.F; calc_function(m, "nlp_f"); m->f = m->worhp_o.F; // Store cost, before scaling m->worhp_o.F *= m->worhp_w.ScaleObj; DoneUserAction(&m->worhp_c, evalF); } if (GetUserAction(&m->worhp_c, evalG)) { m->arg[0] = m->worhp_o.X; m->arg[1] = m->p; m->res[0] = m->worhp_o.G; calc_function(m, "nlp_g"); DoneUserAction(&m->worhp_c, evalG); } if (GetUserAction(&m->worhp_c, evalDF)) { m->arg[0] = m->worhp_o.X; m->arg[1] = m->p; m->res[0] = nullptr; m->res[1] = m->worhp_w.DF.val; calc_function(m, "nlp_grad_f"); casadi_scal(nx_, m->worhp_w.ScaleObj, m->worhp_w.DF.val); DoneUserAction(&m->worhp_c, evalDF); } if (GetUserAction(&m->worhp_c, evalDG)) { m->arg[0] = m->worhp_o.X; m->arg[1] = m->p; m->res[0] = nullptr; m->res[1] = m->worhp_w.DG.val; calc_function(m, "nlp_jac_g"); DoneUserAction(&m->worhp_c, evalDG); } if (GetUserAction(&m->worhp_c, evalHM)) { m->arg[0] = m->worhp_o.X; m->arg[1] = m->p; m->arg[2] = &m->worhp_w.ScaleObj; m->arg[3] = m->worhp_o.Mu; m->res[0] = m->worhp_w.HM.val; calc_function(m, "nlp_hess_l"); // Diagonal values double *dval = m->w; casadi_fill(dval, nx_, 0.); // Remove diagonal const casadi_int* colind = hesslag_sp_.colind(); const casadi_int* row = hesslag_sp_.row(); casadi_int ind=0; for (casadi_int c=0; c<nx_; ++c) { for (casadi_int el=colind[c]; el<colind[c+1]; ++el) { if (row[el]==c) { dval[c] = m->worhp_w.HM.val[el]; } else { m->worhp_w.HM.val[ind++] = m->worhp_w.HM.val[el]; } } } // Add diagonal entries at the end casadi_copy(dval, nx_, m->worhp_w.HM.val+ind); DoneUserAction(&m->worhp_c, evalHM); } if (GetUserAction(&m->worhp_c, fidif)) { WorhpFidif(&m->worhp_o, &m->worhp_w, &m->worhp_p, &m->worhp_c); } } // Copy outputs casadi_copy(m->worhp_o.X, nx_, m->x); casadi_copy(m->worhp_o.G, ng_, m->g); casadi_copy(m->worhp_o.Lambda, nx_, m->lam_x); casadi_copy(m->worhp_o.Mu, ng_, m->lam_g); StatusMsg(&m->worhp_o, &m->worhp_w, &m->worhp_p, &m->worhp_c); m->return_code = m->worhp_c.status; m->return_status = return_codes(m->worhp_c.status); m->success = m->return_code > TerminateSuccess; return 0; }
void GurobiInterface:: eval(void* mem, const double** arg, double** res, int* iw, double* w) const { auto m = static_cast<GurobiMemory*>(mem); // Inputs const double *h=arg[CONIC_H], *g=arg[CONIC_G], *a=arg[CONIC_A], *lba=arg[CONIC_LBA], *uba=arg[CONIC_UBA], *lbx=arg[CONIC_LBX], *ubx=arg[CONIC_UBX], *x0=arg[CONIC_X0], *lam_x0=arg[CONIC_LAM_X0]; // Outputs double *x=res[CONIC_X], *cost=res[CONIC_COST], *lam_a=res[CONIC_LAM_A], *lam_x=res[CONIC_LAM_X]; // Temporary memory double *val=w; w+=nx_; int *ind=iw; iw+=nx_; int *ind2=iw; iw+=nx_; int *tr_ind=iw; iw+=nx_; // Greate an empty model GRBmodel *model = 0; try { int flag = GRBnewmodel(m->env, &model, name_.c_str(), 0, 0, 0, 0, 0, 0); casadi_assert_message(!flag, GRBgeterrormsg(m->env)); // Add variables for (int i=0; i<nx_; ++i) { // Get bounds double lb = lbx ? lbx[i] : 0., ub = ubx ? ubx[i] : 0.; if (isinf(lb)) lb = -GRB_INFINITY; if (isinf(ub)) ub = GRB_INFINITY; // Get variable type char vtype; if (!vtype_.empty()) { // Explicitly set 'vtype' takes precedence vtype = vtype_.at(i); } else if (!discrete_.empty() && discrete_.at(i)) { // Variable marked as discrete (integer or binary) vtype = lb==0 && ub==1 ? GRB_BINARY : GRB_INTEGER; } else { // Continious variable vtype = GRB_CONTINUOUS; } // Pass to model flag = GRBaddvar(model, 0, 0, 0, g ? g[i] : 0., lb, ub, vtype, 0); casadi_assert_message(!flag, GRBgeterrormsg(m->env)); } flag = GRBupdatemodel(model); casadi_assert_message(!flag, GRBgeterrormsg(m->env)); // Add quadratic terms const int *H_colind=sparsity_in(CONIC_H).colind(), *H_row=sparsity_in(CONIC_H).row(); for (int i=0; i<nx_; ++i) { // Quadratic term nonzero indices int numqnz = H_colind[1]-H_colind[0]; casadi_copy(H_row, numqnz, ind); H_colind++; H_row += numqnz; // Corresponding column casadi_fill(ind2, numqnz, i); // Quadratic term nonzeros if (h) { casadi_copy(h, numqnz, val); casadi_scal(numqnz, 0.5, val); h += numqnz; } else { casadi_fill(val, numqnz, 0.); } // Pass to model flag = GRBaddqpterms(model, numqnz, ind, ind2, val); casadi_assert_message(!flag, GRBgeterrormsg(m->env)); } // Add constraints const int *A_colind=sparsity_in(CONIC_A).colind(), *A_row=sparsity_in(CONIC_A).row(); casadi_copy(A_colind, nx_, tr_ind); for (int i=0; i<na_; ++i) { // Get bounds double lb = lba ? lba[i] : 0., ub = uba ? uba[i] : 0.; // if (isinf(lb)) lb = -GRB_INFINITY; // if (isinf(ub)) ub = GRB_INFINITY; // Constraint nonzeros int numnz = 0; for (int j=0; j<nx_; ++j) { if (tr_ind[j]<A_colind[j+1] && A_row[tr_ind[j]]==i) { ind[numnz] = j; val[numnz] = a ? a[tr_ind[j]] : 0; numnz++; tr_ind[j]++; } } // Pass to model if (isinf(lb)) { if (isinf(ub)) { // Neither upper or lower bounds, skip } else { // Only upper bound flag = GRBaddconstr(model, numnz, ind, val, GRB_LESS_EQUAL, ub, 0); casadi_assert_message(!flag, GRBgeterrormsg(m->env)); } } else { if (isinf(ub)) { // Only lower bound flag = GRBaddconstr(model, numnz, ind, val, GRB_GREATER_EQUAL, lb, 0); casadi_assert_message(!flag, GRBgeterrormsg(m->env)); } else if (lb==ub) { // Upper and lower bounds equal flag = GRBaddconstr(model, numnz, ind, val, GRB_EQUAL, lb, 0); casadi_assert_message(!flag, GRBgeterrormsg(m->env)); } else { // Both upper and lower bounds flag = GRBaddrangeconstr(model, numnz, ind, val, lb, ub, 0); casadi_assert_message(!flag, GRBgeterrormsg(m->env)); } } } // Solve the optimization problem flag = GRBoptimize(model); casadi_assert_message(!flag, GRBgeterrormsg(m->env)); int optimstatus; flag = GRBgetintattr(model, GRB_INT_ATTR_STATUS, &optimstatus); casadi_assert_message(!flag, GRBgeterrormsg(m->env)); // Get the objective value, if requested if (cost) { flag = GRBgetdblattr(model, GRB_DBL_ATTR_OBJVAL, cost); casadi_assert_message(!flag, GRBgeterrormsg(m->env)); } // Get the optimal solution, if requested if (x) { flag = GRBgetdblattrarray(model, GRB_DBL_ATTR_X, 0, nx_, x); casadi_assert_message(!flag, GRBgeterrormsg(m->env)); } // Free memory GRBfreemodel(model); } catch (...) { // Free memory if (model) GRBfreemodel(model); throw; } }
int OoqpInterface:: eval(const double** arg, double** res, casadi_int* iw, double* w, void* mem) const { return_status_ = -1; success_ = false; if (inputs_check_) { check_inputs(arg[CONIC_LBX], arg[CONIC_UBX], arg[CONIC_LBA], arg[CONIC_UBA]); } // Get problem data double* g=w; w += nx_; casadi_copy(arg[CONIC_G], nx_, g); double* lbx=w; w += nx_; casadi_copy(arg[CONIC_LBX], nx_, lbx); double* ubx=w; w += nx_; casadi_copy(arg[CONIC_UBX], nx_, ubx); double* lba=w; w += na_; casadi_copy(arg[CONIC_LBA], na_, lba); double* uba=w; w += na_; casadi_copy(arg[CONIC_UBA], na_, uba); double* H=w; w += nnz_in(CONIC_H); casadi_copy(arg[CONIC_H], nnz_in(CONIC_H), H); double* A=w; w += nnz_in(CONIC_A); casadi_copy(arg[CONIC_A], nnz_in(CONIC_A), A); // Temporary memory double* c_ = w; w += nx_; double* bA_ = w; w += na_; double* xlow_ = w; w += nx_; double* xupp_ = w; w += nx_; double* clow_ = w; w += na_; double* cupp_ = w; w += na_; double* x_ = w; w += nx_; double* gamma_ = w; w += nx_; double* phi_ = w; w += nx_; double* y_ = w; w += na_; double* z_ = w; w += na_; double* lambda_ = w; w += na_; double* pi_ = w; w += na_; char* ixlow_ = reinterpret_cast<char*>(iw); iw += nx_; char* ixupp_ = reinterpret_cast<char*>(iw); iw += nx_; char* iclow_ = reinterpret_cast<char*>(iw); iw += na_; char* icupp_ = reinterpret_cast<char*>(iw); iw += na_; double* dQ_ = w; w += nQ_; double* dA_ = w; w += nA_; double* dC_ = w; w += nA_; int* irowQ_ = reinterpret_cast<int*>(iw); iw += nQ_; int* jcolQ_ = reinterpret_cast<int*>(iw); iw += nQ_; int* irowA_ = reinterpret_cast<int*>(iw); iw += nA_; int* jcolA_ = reinterpret_cast<int*>(iw); iw += nA_; int* irowC_ = reinterpret_cast<int*>(iw); iw += nA_; int* jcolC_ = reinterpret_cast<int*>(iw); iw += nA_; int* x_index_ = reinterpret_cast<int*>(iw); iw += nx_; int* c_index_ = reinterpret_cast<int*>(iw); iw += na_; double* p_ = w; w += nx_; double* AT = w; w += nA_; // Parameter contribution to the objective double objParam = 0; // Get the number of free variables and their types casadi_int nx = 0, np=0; for (casadi_int i=0; i<nx_; ++i) { if (lbx[i]==ubx[i]) { // Save parameter p_[np] = lbx[i]; // Add contribution to objective objParam += g[i]*p_[np]; // Save index x_index_[i] = -1-np++; } else { // True free variable if (lbx[i]==-numeric_limits<double>::infinity()) { xlow_[nx] = 0; ixlow_[nx] = 0; } else { xlow_[nx] = lbx[i]; ixlow_[nx] = 1; } if (ubx[i]==numeric_limits<double>::infinity()) { xupp_[nx] = 0; ixupp_[nx] = 0; } else { xupp_[nx] = ubx[i]; ixupp_[nx] = 1; } c_[nx] = g[i]; x_index_[i] = nx++; } } // Get quadratic term const casadi_int* H_colind = H_.colind(); const casadi_int* H_row = H_.row(); casadi_int nnzQ = 0; // Loop over the columns of the quadratic term for (casadi_int cc=0; cc<nx_; ++cc) { // Loop over nonzero elements of the column for (casadi_int el=H_colind[cc]; el<H_colind[cc+1]; ++el) { // Only upper triangular part casadi_int rr=H_row[el]; if (rr>cc) break; // Get variable types casadi_int icc=x_index_[cc]; casadi_int irr=x_index_[rr]; if (icc<0) { if (irr<0) { // Add contribution to objective objParam += icc==irr ? H[el]*sq(p_[-1-icc])/2 : H[el]*p_[-1-irr]*p_[-1-icc]; } else { // Add contribution to gradient term c_[irr] += H[el]*p_[-1-icc]; } } else { if (irr<0) { // Add contribution to gradient term c_[icc] += H[el]*p_[-1-irr]; } else { // Add to sparsity pattern irowQ_[nnzQ] = icc; // row-major --> indices swapped jcolQ_[nnzQ] = irr; // row-major --> indices swapped dQ_[nnzQ++] = H[el]; } } } } // Get the transpose of the sparsity pattern to be able to loop over the constraints casadi_trans(A, A_, AT, spAT_, iw); // Loop over constraints const casadi_int* A_colind = A_.colind(); const casadi_int* A_row = A_.row(); const casadi_int* AT_colind = spAT_.colind(); const casadi_int* AT_row = spAT_.row(); casadi_int nA=0, nC=0, /*mz=0, */ nnzA=0, nnzC=0; for (casadi_int j=0; j<na_; ++j) { if (lba[j] == -numeric_limits<double>::infinity() && uba[j] == numeric_limits<double>::infinity()) { // Redundant constraint c_index_[j] = 0; } else if (lba[j]==uba[j]) { // Equality constraint bA_[nA] = lba[j]; // Add to A for (casadi_int el=AT_colind[j]; el<AT_colind[j+1]; ++el) { casadi_int i=AT_row[el]; if (x_index_[i]<0) { // Parameter bA_[nA] -= AT[el]*p_[-x_index_[i]-1]; } else { // Free variable irowA_[nnzA] = nA; jcolA_[nnzA] = x_index_[i]; dA_[nnzA++] = AT[el]; } } c_index_[j] = -1-nA++; } else { // Inequality constraint if (lba[j]==-numeric_limits<double>::infinity()) { clow_[nC] = 0; iclow_[nC] = 0; } else { clow_[nC] = lba[j]; iclow_[nC] = 1; } if (uba[j]==numeric_limits<double>::infinity()) { cupp_[nC] = 0; icupp_[nC] = 0; } else { cupp_[nC] = uba[j]; icupp_[nC] = 1; } // Add to C for (casadi_int el=AT_colind[j]; el<AT_colind[j+1]; ++el) { casadi_int i=AT_row[el]; if (x_index_[i]<0) { // Parameter if (iclow_[nC]==1) clow_[nC] -= AT[el]*p_[-x_index_[i]-1]; if (icupp_[nC]==1) cupp_[nC] -= AT[el]*p_[-x_index_[i]-1]; } else { // Free variable irowC_[nnzC] = nC; jcolC_[nnzC] = x_index_[i]; dC_[nnzC++] = AT[el]; } } c_index_[j] = 1+nC++; } } // Reset the solution casadi_fill(x_, nx_, 0.); casadi_fill(gamma_, nx_, 0.); casadi_fill(phi_, nx_, 0.); casadi_fill(y_, na_, 0.); casadi_fill(z_, na_, 0.); casadi_fill(lambda_, na_, 0.); casadi_fill(pi_, na_, 0.); // Solve the QP double objectiveValue; int ierr; if (false) { // Use C interface // TODO(jgillis): Change to conicvehb, see OOQP users guide qpsolvesp(c_, nx, irowQ_, nnzQ, jcolQ_, dQ_, xlow_, ixlow_, xupp_, ixupp_, irowA_, nnzA, jcolA_, dA_, bA_, nA, irowC_, nnzC, jcolC_, dC_, clow_, nC, iclow_, cupp_, icupp_, x_, gamma_, phi_, y_, z_, lambda_, pi_, &objectiveValue, print_level_, &ierr); } else { // Use C++ interface ierr=0; // All OOQP related allocations in evaluate std::vector<int> krowQ(nx+1); std::vector<int> krowA(nA+1); std::vector<int> krowC(nC+1); //casadi_int status_code = 0; makehb(irowQ_, nnzQ, get_ptr(krowQ), nx, &ierr); if (ierr == 0) makehb(irowA_, nnzA, get_ptr(krowA), nA, &ierr); if (ierr == 0) makehb(irowC_, nnzC, get_ptr(krowC), nC, &ierr); if (ierr == 0) { QpGenContext ctx; QpGenHbGondzioSetup(c_, nx, get_ptr(krowQ), jcolQ_, dQ_, xlow_, ixlow_, xupp_, ixupp_, get_ptr(krowA), nA, jcolA_, dA_, bA_, get_ptr(krowC), nC, jcolC_, dC_, clow_, iclow_, cupp_, icupp_, &ctx, &ierr); if (ierr == 0) { Solver* solver = static_cast<Solver *>(ctx.solver); gOoqpPrintLevel = print_level_; solver->monitorSelf(); solver->setMuTol(mutol_); solver->setMuTol(mutol_); QpGenFinish(&ctx, x_, gamma_, phi_, y_, z_, lambda_, pi_, &objectiveValue, &ierr); } QpGenCleanup(&ctx); } } return_status_ = ierr; success_ = ierr==SUCCESSFUL_TERMINATION; if (ierr>0) { casadi_warning("Unable to solve problem: " + str(errFlag(ierr))); } else if (ierr<0) { casadi_error("Fatal error: " + str(errFlag(ierr))); } // Retrieve eliminated decision variables for (casadi_int i=nx_-1; i>=0; --i) { casadi_int ii = x_index_[i]; if (ii<0) { x_[i] = p_[-1-ii]; } else { x_[i] = x_[ii]; } } // Retreive eliminated dual variables (linear bounds) for (casadi_int j=na_-1; j>=0; --j) { casadi_int jj = c_index_[j]; if (jj==0) { lambda_[j] = 0; } else if (jj<0) { lambda_[j] = -y_[-1-jj]; } else { lambda_[j] = pi_[-1+jj]-lambda_[-1+jj]; } } // Retreive eliminated dual variables (simple bounds) for (casadi_int i=nx_-1; i>=0; --i) { casadi_int ii = x_index_[i]; if (ii<0) { // The dual solution for the fixed parameters follows from the KKT conditions gamma_[i] = -g[i]; for (casadi_int el=H_colind[i]; el<H_colind[i+1]; ++el) { casadi_int j=H_row[el]; gamma_[i] -= H[el]*x_[j]; } for (casadi_int el=A_colind[i]; el<A_colind[i+1]; ++el) { casadi_int j=A_row[el]; gamma_[i] -= A[el]*lambda_[j]; } } else { gamma_[i] = phi_[ii]-gamma_[ii]; } } // Save optimal cost if (res[CONIC_COST]) *res[CONIC_COST] = objectiveValue + objParam; // Save primal solution casadi_copy(x_, nx_, res[CONIC_X]); // Save dual solution (linear bounds) casadi_copy(lambda_, na_, res[CONIC_LAM_A]); // Save dual solution (simple bounds) casadi_copy(gamma_, nx_, res[CONIC_LAM_X]); return 0; }