void primitive(double *hold)
{
switch (token_type)
{
case VARIABLE:
*hold = find_var(token);
get_token();
return;
case NUMBER:
//
*hold = atof((token));
//if (sscanf(token, "%lf", hold) != 1)
*hold = convert(token);
if (convert_error == ERROR)
serror( ERR_BADNUMER);
get_token();
return;
case FUNCTION:
calc_function( hold);
if (*token != ')')
serror(ERR_NOBRACKET);
get_token();
return;
default: // error
return;
}
}
void recalculate_and_draw(Coordinate_system *cs, char expr[], char number_of_derivatives, float step, bool clear_all)
{
calc_function(cs, expr, number_of_derivatives, 0.001);
if(clear_all)
draw_empty_chart(cs->get_axis_coord('x'), cs->get_axis_coord('y'), GRAPH_WIDHT, GRAPH_HEIGHT, cs->get_scale_of_axis('x'), cs->get_scale_of_axis('y'));
draw_function(cs, expr, number_of_derivatives);
}
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::
userfun(SnoptMemory* m, int* mode, int nnObj, int nnCon, int nnJac, int nnL, int neJac,
double* x, double* fObj, double*gObj, double* fCon, double* gCon,
int nState, char* cu, int lencu, int* iu, int leniu, double* ru,
int lenru) const {
try {
casadi_assert_message(nnCon_ == nnCon, "Con " << nnCon_ << " <-> " << nnCon);
casadi_assert_message(nnObj_ == nnObj, "Obj " << nnObj_ << " <-> " << nnObj);
casadi_assert_message(nnJac_ == nnJac, "Jac " << nnJac_ << " <-> " << nnJac);
// Get reduced decision variables
casadi_fill(m->xk2, nx_, 0.);
for (int k = 0; k < nnObj; ++k) m->xk2[k] = x[k];
// Evaluate gradF with the linear variables put to zero
const double** arg = m->arg;
*arg++ = m->xk2;
*arg++ = m->p;
double** res = m->res;
*res++ = fObj;
*res++ = m->jac_fk;
calc_function(m, "nlp_jac_f");
// provide nonlinear part of objective gradient to SNOPT
for (int k = 0; k < nnObj; ++k) {
int el = jac_f_fcn_.sparsity_out(1).colind(k);
if (jac_f_fcn_.sparsity_out(1).colind(k+1) > el) {
gObj[k] = m->jac_fk[el];
} else {
gObj[k] = 0;
}
}
jac_f_fcn_.sparsity_out(1).sanity_check(true);
jac_f_fcn_.sparsity_out(1).sanity_check(false);
if (!jac_g_fcn_.is_null()) {
// Get reduced decision variables
casadi_fill(m->xk2, nx_, 0.);
for (int k = 0; k < nnJac; ++k) {
m->xk2[k] = x[k];
}
// Evaluate jacG with the linear variabes put to zero
const double** arg = m->arg;
*arg++ = m->xk2;
*arg++ = m->p;
double** res = m->res;
*res++ = m->gk;
*res++ = m->jac_gk;
calc_function(m, "nlp_jac_g");
// provide nonlinear part of constraint jacobian to SNOPT
int kk = 0;
for (int j = 0; j < nnJac; ++j) {
for (int k = A_structure_.colind(j); k < A_structure_.sparsity().colind(j+1); ++k) {
if (A_structure_.row(k) >= nnCon) break;
int i = A_structure_.nonzeros()[k];
if (i > 0) {
gCon[kk++] = m->jac_gk[i-1];
}
}
}
casadi_assert(kk == 0 || kk == neJac);
// provide nonlinear part of objective to SNOPT
for (int k = 0; k < nnCon; ++k) {
fCon[k] = m->gk[k];
}
}
} catch(std::exception& ex) {
userOut<true, PL_WARN>() << "eval_nlp failed: " << ex.what() << std::endl;
*mode = -1; // Reduce step size - we've got problems
return;
}
}
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");
}
