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