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;
}
