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);
}
int IdasInterface::rhsQB(double t, N_Vector xz, N_Vector xzdot, N_Vector rxz,
N_Vector rxzdot, N_Vector rqdot, void *user_data) {
try {
auto m = to_mem(user_data);
auto& s = m->self;
m->arg[0] = NV_DATA_S(rxz);
m->arg[1] = NV_DATA_S(rxz)+s.nrx_;
m->arg[2] = m->rp;
m->arg[3] = NV_DATA_S(xz);
m->arg[4] = NV_DATA_S(xz)+s.nx_;
m->arg[5] = m->p;
m->arg[6] = &t;
m->res[0] = NV_DATA_S(rqdot);
s.calc_function(m, "quadB");
// Negate (note definition of g)
casadi_scal(s.nrq_, -1., NV_DATA_S(rqdot));
return 0;
} catch(int flag) { // recoverable error
return flag;
} catch(exception& e) { // non-recoverable error
userOut<true, PL_WARN>() << "resQB failed: " << e.what() << endl;
return -1;
}
}
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;
}
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);
}