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");
}
void SXFunctionInternal::evalD(const double** arg, double** res,
int* iw, double* w) {
double time_start=0;
double time_stop=0;
if (CasadiOptions::profiling) {
time_start = getRealTime();
if (CasadiOptions::profilingBinary) {
profileWriteEntry(CasadiOptions::profilingLog, this);
} else {
CasadiOptions::profilingLog << "start " << this << ":" <<getOption("name") << std::endl;
}
}
casadi_msg("SXFunctionInternal::evaluate():begin " << getOption("name"));
// NOTE: The implementation of this function is very delicate. Small changes in the
// class structure can cause large performance losses. For this reason,
// the preprocessor macros are used below
if (!free_vars_.empty()) {
std::stringstream ss;
repr(ss);
casadi_error("Cannot evaluate \"" << ss.str() << "\" since variables "
<< free_vars_ << " are free.");
}
#ifdef WITH_OPENCL
if (just_in_time_opencl_) {
// Evaluate with OpenCL
return evaluateOpenCL();
}
#endif // WITH_OPENCL
// Evaluate the algorithm
for (vector<AlgEl>::iterator it=algorithm_.begin(); it!=algorithm_.end(); ++it) {
switch (it->op) {
CASADI_MATH_FUN_BUILTIN(w[it->i1], w[it->i2], w[it->i0])
case OP_CONST: w[it->i0] = it->d; break;
case OP_INPUT: w[it->i0] = arg[it->i1]==0 ? 0 : arg[it->i1][it->i2]; break;
case OP_OUTPUT: if (res[it->i0]!=0) res[it->i0][it->i2] = w[it->i1]; break;
default:
casadi_error("SXFunctionInternal::evalD: Unknown operation" << it->op);
}
}
casadi_msg("SXFunctionInternal::evalD():end " << getOption("name"));
if (CasadiOptions::profiling) {
time_stop = getRealTime();
if (CasadiOptions::profilingBinary) {
profileWriteExit(CasadiOptions::profilingLog, this, time_stop-time_start);
} else {
CasadiOptions::profilingLog
<< (time_stop-time_start)*1e6 << " ns | "
<< (time_stop-time_start)*1e3 << " ms | "
<< this << ":" <<getOption("name")
<< ":0||SX algorithm size: " << algorithm_.size()
<< std::endl;
}
}
}
void Newton::solveNonLinear() {
casadi_msg("Newton::solveNonLinear:begin");
// Set up timers for profiling
double time_zero=0;
double time_start=0;
double time_stop=0;
if (CasadiOptions::profiling && !CasadiOptions::profilingBinary) {
time_zero = getRealTime();
CasadiOptions::profilingLog << "start " << this << ":" <<getOption("name") << std::endl;
}
// Pass the inputs to J
for (int i=0; i<nIn(); ++i) {
if (i!=iin_) jac_.setInput(input(i), i);
}
// Aliases
DMatrix &u = output(iout_);
DMatrix &J = jac_.output(0);
DMatrix &F = jac_.output(1+iout_);
// Perform the Newton iterations
int iter=0;
bool success = true;
while (true) {
// Break if maximum number of iterations already reached
if (iter >= max_iter_) {
log("evaluate", "Max. iterations reached.");
stats_["return_status"] = "max_iteration_reached";
success = false;
break;
}
// Start a new iteration
iter++;
// Print progress
if (monitored("step") || monitored("stepsize")) {
userOut() << "Step " << iter << "." << std::endl;
}
if (monitored("step")) {
userOut() << " u = " << u << std::endl;
}
// Use u to evaluate J
jac_.setInput(u, iin_);
for (int i=0; i<nIn(); ++i)
if (i!=iin_) jac_.setInput(input(i), i);
if (CasadiOptions::profiling) {
time_start = getRealTime(); // Start timer
}
jac_.evaluate();
// Write out profiling information
if (CasadiOptions::profiling && !CasadiOptions::profilingBinary) {
time_stop = getRealTime(); // Stop timer
CasadiOptions::profilingLog
<< (time_stop-time_start)*1e6 << " ns | "
<< (time_stop-time_zero)*1e3 << " ms | "
<< this << ":" << getOption("name") << ":0|" << jac_.get() << ":"
<< jac_.getOption("name") << "|evaluate jacobian" << std::endl;
}
if (monitored("F")) userOut() << " F = " << F << std::endl;
if (monitored("normF"))
userOut() << " F (min, max, 1-norm, 2-norm) = "
<< (*std::min_element(F.data().begin(), F.data().end()))
<< ", " << (*std::max_element(F.data().begin(), F.data().end()))
<< ", " << norm_1(F) << ", " << norm_F(F) << std::endl;
if (monitored("J")) userOut() << " J = " << J << std::endl;
double abstol = 0;
if (numeric_limits<double>::infinity() != abstol_) {
abstol = std::max((*std::max_element(F.data().begin(),
F.data().end())),
-(*std::min_element(F.data().begin(),
F.data().end())));
if (abstol <= abstol_) {
casadi_msg("Converged to acceptable tolerance - abstol: " << abstol_);
break;
}
}
// Prepare the linear solver with J
linsol_.setInput(J, LINSOL_A);
if (CasadiOptions::profiling) {
time_start = getRealTime(); // Start timer
}
linsol_.prepare();
// Write out profiling information
if (CasadiOptions::profiling && !CasadiOptions::profilingBinary) {
time_stop = getRealTime(); // Stop timer
CasadiOptions::profilingLog
<< (time_stop-time_start)*1e6 << " ns | "
<< (time_stop-time_zero)*1e3 << " ms | "
<< this << ":" << getOption("name")
<< ":1||prepare linear system" << std::endl;
}
if (CasadiOptions::profiling) {
time_start = getRealTime(); // Start timer
}
// Solve against F
linsol_.solve(&F.front(), 1, false);
if (CasadiOptions::profiling && !CasadiOptions::profilingBinary) {
time_stop = getRealTime(); // Stop timer
CasadiOptions::profilingLog
<< (time_stop-time_start)*1e6 << " ns | "
<< (time_stop-time_zero)*1e3 << " ms | "
<< this << ":" << getOption("name") << ":2||solve linear system" << std::endl;
}
if (monitored("step")) {
userOut() << " step = " << F << std::endl;
}
double abstolStep=0;
if (numeric_limits<double>::infinity() != abstolStep_) {
abstolStep = std::max((*std::max_element(F.data().begin(),
F.data().end())),
-(*std::min_element(F.data().begin(),
F.data().end())));
if (monitored("stepsize")) {
userOut() << " stepsize = " << abstolStep << std::endl;
}
if (abstolStep <= abstolStep_) {
casadi_msg("Converged to acceptable tolerance - abstolStep: " << abstolStep_);
break;
}
}
if (print_iteration_) {
// Only print iteration header once in a while
if (iter % 10==0) {
printIteration(userOut());
}
// Print iteration information
printIteration(userOut(), iter, abstol, abstolStep);
}
// Update Xk+1 = Xk - J^(-1) F
std::transform(u.begin(), u.end(), F.begin(), u.begin(), std::minus<double>());
}
// Get auxiliary outputs
for (int i=0; i<nOut(); ++i) {
if (i!=iout_) jac_.getOutput(output(i), 1+i);
}
// Store the iteration count
if (gather_stats_) stats_["iter"] = iter;
if (success) stats_["return_status"] = "success";
// Factorization up-to-date
fact_up_to_date_ = true;
casadi_msg("Newton::solveNonLinear():end after " << iter << " steps");
}
