void QpToNlp::
eval(void* mem, const double** arg, double** res, int* iw, double* w) const {
// Inputs
const double *h_, *g_, *a_, *lba_, *uba_, *lbx_, *ubx_, *x0_;
// Outputs
double *x_, *f_, *lam_a_, *lam_x_;
// Get input pointers
h_ = arg[QPSOL_H];
g_ = arg[QPSOL_G];
a_ = arg[QPSOL_A];
lba_ = arg[QPSOL_LBA];
uba_ = arg[QPSOL_UBA];
lbx_ = arg[QPSOL_LBX];
ubx_ = arg[QPSOL_UBX];
x0_ = arg[QPSOL_X0];
// Get output pointers
x_ = res[QPSOL_X];
f_ = res[QPSOL_COST];
lam_a_ = res[QPSOL_LAM_A];
lam_x_ = res[QPSOL_LAM_X];
// Buffers for calling the NLP solver
const double** arg1 = arg + n_in();
double** res1 = res + n_out();
fill_n(arg1, static_cast<int>(NLPSOL_NUM_IN), nullptr);
fill_n(res1, static_cast<int>(NLPSOL_NUM_OUT), nullptr);
// NLP inputs
arg1[NLPSOL_X0] = x0_;
arg1[NLPSOL_LBG] = lba_;
arg1[NLPSOL_UBG] = uba_;
arg1[NLPSOL_LBX] = lbx_;
arg1[NLPSOL_UBX] = ubx_;
// NLP parameters
arg1[NLPSOL_P] = w;
// Quadratic term
int nh = nnz_in(QPSOL_H);
if (h_) {
copy_n(h_, nh, w);
} else {
fill_n(w, nh, 0);
}
w += nh;
// Linear objective term
int ng = nnz_in(QPSOL_G);
if (g_) {
copy_n(g_, ng, w);
} else {
fill_n(w, ng, 0);
}
w += ng;
// Linear constraints
int na = nnz_in(QPSOL_A);
if (a_) {
copy_n(a_, na, w);
} else {
fill_n(w, na, 0);
}
w += na;
// Solution
res1[NLPSOL_X] = x_;
res1[NLPSOL_F] = f_;
res1[NLPSOL_LAM_X] = lam_x_;
res1[NLPSOL_LAM_G] = lam_a_;
// Solve the NLP
solver_(arg1, res1, iw, w, 0);
}
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");
}
