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->x0, nx_, x); } // Initialize dual variables (simple bounds) if (init_z) { if (m->lam_x0) { for (int i=0; i<nx_; ++i) { z_L[i] = max(0., -m->lam_x0[i]); z_U[i] = max(0., m->lam_x0[i]); } } else { casadi_fill(z_L, nx_, 0.); casadi_fill(z_U, nx_, 0.); } } // Initialize dual variables (nonlinear bounds) if (init_lambda) { casadi_copy(m->lam_g0, ng_, lambda); } return true; } catch(exception& ex) { userOut<true, PL_WARN>() << "get_starting_point 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 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 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); }
/* 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; }
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 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; }
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; } }
void SnoptInterface:: userfun(SnoptMemory* m, int* mode, int nnObj, int nnCon, int nnJac, int nnL, int neJac, double* x, double* fObj, double*gObj, double* fCon, double* gCon, int nState, char* cu, int lencu, int* iu, int leniu, double* ru, int lenru) const { try { casadi_assert_message(nnCon_ == nnCon, "Con " << nnCon_ << " <-> " << nnCon); casadi_assert_message(nnObj_ == nnObj, "Obj " << nnObj_ << " <-> " << nnObj); casadi_assert_message(nnJac_ == nnJac, "Jac " << nnJac_ << " <-> " << nnJac); // Get reduced decision variables casadi_fill(m->xk2, nx_, 0.); for (int k = 0; k < nnObj; ++k) m->xk2[k] = x[k]; // Evaluate gradF with the linear variables put to zero const double** arg = m->arg; *arg++ = m->xk2; *arg++ = m->p; double** res = m->res; *res++ = fObj; *res++ = m->jac_fk; calc_function(m, "nlp_jac_f"); // provide nonlinear part of objective gradient to SNOPT for (int k = 0; k < nnObj; ++k) { int el = jac_f_fcn_.sparsity_out(1).colind(k); if (jac_f_fcn_.sparsity_out(1).colind(k+1) > el) { gObj[k] = m->jac_fk[el]; } else { gObj[k] = 0; } } jac_f_fcn_.sparsity_out(1).sanity_check(true); jac_f_fcn_.sparsity_out(1).sanity_check(false); if (!jac_g_fcn_.is_null()) { // Get reduced decision variables casadi_fill(m->xk2, nx_, 0.); for (int k = 0; k < nnJac; ++k) { m->xk2[k] = x[k]; } // Evaluate jacG with the linear variabes put to zero const double** arg = m->arg; *arg++ = m->xk2; *arg++ = m->p; double** res = m->res; *res++ = m->gk; *res++ = m->jac_gk; calc_function(m, "nlp_jac_g"); // provide nonlinear part of constraint jacobian to SNOPT int kk = 0; for (int j = 0; j < nnJac; ++j) { for (int k = A_structure_.colind(j); k < A_structure_.sparsity().colind(j+1); ++k) { if (A_structure_.row(k) >= nnCon) break; int i = A_structure_.nonzeros()[k]; if (i > 0) { gCon[kk++] = m->jac_gk[i-1]; } } } casadi_assert(kk == 0 || kk == neJac); // provide nonlinear part of objective to SNOPT for (int k = 0; k < nnCon; ++k) { fCon[k] = m->gk[k]; } } } catch(std::exception& ex) { userOut<true, PL_WARN>() << "eval_nlp failed: " << ex.what() << std::endl; *mode = -1; // Reduce step size - we've got problems return; } }
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); }
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; }