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