26 #include "worhp_interface.hpp"
28 #include "casadi/core/casadi_misc.hpp"
35 int CASADI_NLPSOL_WORHP_EXPORT
38 plugin->name =
"worhp";
40 plugin->version = CASADI_VERSION;
63 "Options to be passed to WORHP"}}
72 if (CheckWorhpVersion(WORHP_MAJOR, WORHP_MINOR, WORHP_PATCH)) {
73 casadi_warning(
"Worhp incompatibility. Interface was compiled for Worhp " +
74 str(WORHP_MAJOR) +
"." +
str(WORHP_MINOR) +
"." + std::string(WORHP_PATCH));
81 for (
auto&& op : opts) {
82 if (op.first==
"worhp") {
83 worhp_opts = op.second;
88 casadi_int nopts = WorhpGetParamCount();
89 for (
auto&& op : worhp_opts) {
97 for (ind=1; ind<=nopts; ++ind) {
99 const char* name = WorhpGetParamName(ind);
101 if (op.first==name)
break;
103 if (ind>nopts) casadi_error(
"No such Worhp option: " + op.first);
106 switch (WorhpGetParamType(ind)) {
117 casadi_error(
"Cannot handle WORHP option \"" + op.first +
"\": Unknown type " +
118 str(WorhpGetParamType(ind)) +
".");
129 {
"transpose:triu:hess:gamma:x:x"},
130 {{
"gamma", {
"f",
"g"}}});
131 jacg_sp_ = jac_g_fcn.sparsity_out(1);
139 if (mode & WORHP_PRINT_MESSAGE) {
140 uout() << message << std::endl;
142 if (mode & WORHP_PRINT_WARNING) {
143 uerr() << message << std::endl;
145 if (mode & WORHP_PRINT_ERROR) {
146 uerr() << message << std::endl;
156 WorhpPreInit(&m->worhp_o, &m->worhp_w, &m->worhp_p, &m->worhp_c);
157 m->worhp_o.initialised =
false;
158 m->worhp_w.initialised =
false;
159 m->worhp_p.initialised =
false;
160 m->worhp_c.initialised =
false;
164 InitParams(&status, &m->worhp_p);
165 casadi_assert(status==0,
"Problem in Worhp InitParams. Status: " +
str(status));
171 WorhpSetBoolParam(&m->worhp_p, op.first.c_str(), op.second),
172 "Problem setting boolean Worhp parameter " + op.first);
178 WorhpSetDoubleParam(&m->worhp_p, op.first.c_str(), op.second),
179 "Problem setting double Worhp parameter " + op.first);
185 WorhpSetIntParam(&m->worhp_p, op.first.c_str(), op.second),
186 "Problem setting integer Worhp parameter " + op.first);
191 if (op.first==
"ipBarrier") {
192 m->worhp_p.qp.ipBarrier = op.second;
193 }
else if (op.first==
"ipComTol") {
194 m->worhp_p.qp.ipComTol = op.second;
195 }
else if (op.first==
"ipFracBound") {
196 m->worhp_p.qp.ipFracBound = op.second;
197 }
else if (op.first==
"ipMinAlpha") {
198 m->worhp_p.qp.ipMinAlpha = op.second;
199 }
else if (op.first==
"ipRelaxDiv") {
200 m->worhp_p.qp.ipRelaxDiv = op.second;
201 }
else if (op.first==
"ipRelaxMax") {
202 m->worhp_p.qp.ipRelaxMax = op.second;
203 }
else if (op.first==
"ipRelaxMin") {
204 m->worhp_p.qp.ipRelaxMin = op.second;
205 }
else if (op.first==
"ipRelaxMult") {
206 m->worhp_p.qp.ipRelaxMult = op.second;
207 }
else if (op.first==
"ipResTol") {
208 m->worhp_p.qp.ipResTol = op.second;
209 }
else if (op.first==
"lsTol") {
210 m->worhp_p.qp.lsTol = op.second;
211 }
else if (op.first==
"nsnBeta") {
212 m->worhp_p.qp.nsnBeta = op.second;
213 }
else if (op.first==
"nsnKKT") {
214 m->worhp_p.qp.nsnKKT = op.second;
215 }
else if (op.first==
"nsnMinAlpha") {
216 m->worhp_p.qp.nsnMinAlpha = op.second;
217 }
else if (op.first==
"nsnSigma") {
218 m->worhp_p.qp.nsnSigma = op.second;
219 }
else if (op.first==
"ipLsMethod") {
220 m->worhp_p.qp.ipLsMethod = op.second;
221 }
else if (op.first==
"lsItMaxIter") {
222 m->worhp_p.qp.lsItMaxIter = op.second;
223 }
else if (op.first==
"lsItMethod") {
224 m->worhp_p.qp.lsItMethod = op.second;
225 }
else if (op.first==
"lsItPrecondMethod") {
226 m->worhp_p.qp.lsItPrecondMethod = op.second;
227 }
else if (op.first==
"lsRefineMaxIter") {
228 m->worhp_p.qp.lsRefineMaxIter = op.second;
229 }
else if (op.first==
"maxIter") {
230 m->worhp_p.qp.maxIter = op.second;
231 }
else if (op.first==
"method") {
232 m->worhp_p.qp.method = op.second;
233 }
else if (op.first==
"nsnLsMethod") {
234 m->worhp_p.qp.nsnLsMethod = op.second;
235 }
else if (op.first==
"printLevel") {
236 m->worhp_p.qp.printLevel = op.second;
237 }
else if (op.first==
"ipTryRelax") {
238 m->worhp_p.qp.ipTryRelax = op.second;
239 }
else if (op.first==
"lsScale") {
240 m->worhp_p.qp.lsScale = op.second;
241 }
else if (op.first==
"lsTrySimple") {
242 m->worhp_p.qp.lsTrySimple = op.second;
243 }
else if (op.first==
"nsnGradStep") {
244 m->worhp_p.qp.nsnGradStep = op.second;
245 }
else if (op.first==
"scaleIntern") {
246 m->worhp_p.qp.scaleIntern = op.second;
247 }
else if (op.first==
"strict") {
248 m->worhp_p.qp.strict = op.second;
250 casadi_error(
"No such Worhp option: qp." + op.first);
255 m->worhp_p.initialised =
true;
262 casadi_int*& iw,
double*& w)
const {
269 m->worhp_p.initialised =
false;
270 if (m->worhp_o.initialised || m->worhp_w.initialised || m->worhp_c.initialised) {
271 WorhpFree(&m->worhp_o, &m->worhp_w, &m->worhp_p, &m->worhp_c);
273 m->worhp_p.initialised =
true;
282 m->worhp_c.initialised =
false;
283 m->worhp_w.initialised =
false;
284 m->worhp_o.initialised =
false;
287 m->worhp_w.DF.nnz =
nx_;
288 if (m->worhp_o.m>0) {
291 m->worhp_w.DG.nnz = 0;
297 m->worhp_w.HM.nnz = 0;
301 WorhpInit(&m->worhp_o, &m->worhp_w, &m->worhp_p, &m->worhp_c);
303 if (m->worhp_c.status != FirstCall) {
305 casadi_error(
"Main: Initialisation failed. Status: " + msg);
308 if (m->worhp_w.DF.NeedStructure) {
309 for (casadi_int i=0; i<
nx_; ++i) {
310 m->worhp_w.DF.row[i] = i + 1;
314 if (m->worhp_o.m>0 && m->worhp_w.DG.NeedStructure) {
318 for (casadi_int c=0; c<
nx_; ++c) {
319 for (casadi_int el=colind[c]; el<colind[c+1]; ++el) {
320 casadi_int r = row[el];
321 m->worhp_w.DG.col[nz] = c + 1;
322 m->worhp_w.DG.row[nz] = r + 1;
328 if (m->worhp_w.HM.NeedStructure) {
336 for (casadi_int c=0; c<
nx_; ++c) {
337 for (casadi_int el=colind[c]; el<colind[c+1]; ++el) {
339 m->worhp_w.HM.row[nz] = row[el] + 1;
340 m->worhp_w.HM.col[nz] = c + 1;
347 for (casadi_int r=0; r<
nx_; ++r) {
348 m->worhp_w.HM.row[nz] = r + 1;
349 m->worhp_w.HM.col[nz] = r + 1;
357 auto d_nlp = &m->
d_nlp;
359 for (casadi_int i=0; i<
ng_; ++i) {
360 casadi_assert(!(d_nlp->lbz[
nx_+i]==-
inf && d_nlp->ubz[
nx_+i] ==
inf),
361 "WorhpInterface::evaluate: Worhp cannot handle the case when both "
362 "LBG and UBG are infinite."
363 "You have that case at non-zero " +
str(i)+
"."
364 "Reformulate your problem eliminating the corresponding constraint.");
372 if (m->worhp_o.m>0) {
379 double inf = std::numeric_limits<double>::infinity();
380 for (casadi_int i=0; i<
nx_; ++i)
381 if (m->worhp_o.XL[i]==-
inf) m->worhp_o.XL[i] = -m->worhp_p.Infty;
382 for (casadi_int i=0; i<
nx_; ++i)
383 if (m->worhp_o.XU[i]==
inf) m->worhp_o.XU[i] = m->worhp_p.Infty;
384 for (casadi_int i=0; i<
ng_; ++i)
385 if (m->worhp_o.GL[i]==-
inf) m->worhp_o.GL[i] = -m->worhp_p.Infty;
386 for (casadi_int i=0; i<
ng_; ++i)
387 if (m->worhp_o.GU[i]==
inf) m->worhp_o.GU[i] = m->worhp_p.Infty;
389 if (
verbose_) casadi_message(
"WorhpInterface::starting iteration");
391 bool firstIteration =
true;
394 while (m->worhp_c.status < TerminateSuccess && m->worhp_c.status > TerminateError) {
395 if (GetUserAction(&m->worhp_c, callWorhp)) {
396 Worhp(&m->worhp_o, &m->worhp_w, &m->worhp_p, &m->worhp_c);
400 if (GetUserAction(&m->worhp_c, iterOutput)) {
402 if (!firstIteration) {
403 firstIteration =
true;
406 m->iter = m->worhp_w.MajorIter;
407 m->iter_sqp = m->worhp_w.MinorIter;
408 m->inf_pr = m->worhp_w.NormMax_CV;
409 m->inf_du = m->worhp_p.ScaledKKT;
410 m->alpha_pr = m->worhp_w.ArmijoAlpha;
424 m->res[0] = &ret_double;
426 m->fstats.at(
"callback_fun").tic();
429 m->fstats.at(
"callback_fun").toc();
430 casadi_int ret =
static_cast<casadi_int
>(ret_double);
432 if (ret) m->worhp_c.status = TerminateError;
437 IterationOutput(&m->worhp_o, &m->worhp_w, &m->worhp_p, &m->worhp_c);
438 DoneUserAction(&m->worhp_c, iterOutput);
441 if (GetUserAction(&m->worhp_c, evalF)) {
442 m->arg[0] = m->worhp_o.X;
443 m->arg[1] = d_nlp->p;
444 m->res[0] = &m->worhp_o.F;
446 d_nlp->objective = m->worhp_o.F;
447 m->worhp_o.F *= m->worhp_w.ScaleObj;
448 DoneUserAction(&m->worhp_c, evalF);
451 if (GetUserAction(&m->worhp_c, evalG)) {
452 m->arg[0] = m->worhp_o.X;
453 m->arg[1] = d_nlp->p;
454 m->res[0] = m->worhp_o.G;
456 DoneUserAction(&m->worhp_c, evalG);
459 if (GetUserAction(&m->worhp_c, evalDF)) {
460 m->arg[0] = m->worhp_o.X;
461 m->arg[1] = d_nlp->p;
463 m->res[1] = m->worhp_w.DF.val;
466 DoneUserAction(&m->worhp_c, evalDF);
469 if (GetUserAction(&m->worhp_c, evalDG)) {
470 m->arg[0] = m->worhp_o.X;
471 m->arg[1] = d_nlp->p;
473 m->res[1] = m->worhp_w.DG.val;
475 DoneUserAction(&m->worhp_c, evalDG);
478 if (GetUserAction(&m->worhp_c, evalHM)) {
479 m->arg[0] = m->worhp_o.X;
480 m->arg[1] = d_nlp->p;
481 m->arg[2] = &m->worhp_w.ScaleObj;
482 m->arg[3] = m->worhp_o.Mu;
483 m->res[0] = m->worhp_w.HM.val;
493 for (casadi_int c=0; c<
nx_; ++c) {
494 for (casadi_int el=colind[c]; el<colind[c+1]; ++el) {
496 dval[c] = m->worhp_w.HM.val[el];
498 m->worhp_w.HM.val[ind++] = m->worhp_w.HM.val[el];
505 DoneUserAction(&m->worhp_c, evalHM);
508 if (GetUserAction(&m->worhp_c, fidif)) {
509 WorhpFidif(&m->worhp_o, &m->worhp_w, &m->worhp_p, &m->worhp_c);
519 StatusMsg(&m->worhp_o, &m->worhp_w, &m->worhp_p, &m->worhp_c);
521 m->return_code = m->worhp_c.status;
523 m->success = m->return_code > TerminateSuccess;
524 if (m->return_code==MaxCalls || m->return_code==MaxCalls || m->return_code==Timeout)
526 if (m->return_code==evalsNaN)
533 case OptimalSolution:
return "OptimalSolution";
534 case OptimalSolutionConstantF:
return "OptimalSolutionConstantF";
535 case SearchDirectionZero:
return "SearchDirectionZero";
536 case SearchDirectionSmall:
return "SearchDirectionSmall";
537 case FritzJohn:
return "FritzJohn";
538 case NotDiffable:
return "NotDiffable";
539 case Unbounded:
return "Unbounded";
540 case FeasibleSolution:
return "FeasibleSolution";
541 case LowPassFilterOptimal:
return "LowPassFilterOptimal";
542 case LowPassFilterAcceptable:
return "LowPassFilterAcceptable";
543 case AcceptableSolution:
return "AcceptableSolution";
544 case AcceptablePrevious:
return "AcceptablePrevious";
545 case AcceptableSolutionConstantF:
return "AcceptableSolutionConstantF";
546 case AcceptablePreviousConstantF:
return "AcceptablePreviousConstantF";
547 case AcceptableSolutionSKKT:
return "AcceptableSolutionSKKT";
548 case AcceptableSolutionScaled:
return "AcceptableSolutionScaled";
549 case AcceptablePreviousScaled:
return "AcceptablePreviousScaled";
550 case TerminateError:
return "TerminateError";
551 case MaxCalls:
return "MaxCalls";
552 case MaxIter:
return "MaxIter";
553 case Timeout:
return "Timeout";
554 case TooBig:
return "TooBig";
555 case evalsNaN:
return "evalsNaN";
556 case DivergingPrimal:
return "DivergingPrimal";
557 case DivergingDual:
return "DivergingDual";
558 case MinimumStepsize:
return "MinimumStepsize";
559 case RegularizationFailed:
return "RegularizationFailed";
560 case InitError:
return "InitError";
561 case DataError:
return "DataError";
562 case RestartError:
return "RestartError";
563 case QPerror:
return "QPerror";
564 case LinearSolverFailed:
return "LinearSolverFailed";
565 case TerminatedByCheckFD:
return "TerminatedByCheckFD";
566 case LicenseError:
return "LicenseError";
567 case Debug:
return "Debug";
569 return "Unknown WORHP return code";
573 this->
worhp_o.initialised =
false;
574 this->
worhp_w.initialised =
false;
575 this->
worhp_p.initialised =
false;
576 this->
worhp_c.initialised =
false;
581 if (this->
worhp_p.initialised || this->worhp_o.initialised ||
582 this->worhp_w.initialised || this->worhp_c.initialised) {
596 s.
version(
"WorhpInterface", 1);
607 s.
version(
"WorhpInterface", 1);
Helper class for Serialization.
void unpack(Sparsity &e)
Reconstruct an object from the input stream.
void version(const std::string &name, int v)
void alloc_w(size_t sz_w, bool persistent=false)
Ensure required length of w field.
casadi_int n_out() const
Get the number of function outputs.
casadi_int n_in() const
Get the number of function inputs.
bool is_null() const
Is a null pointer?
NLP solver storage class.
Dict get_stats(void *mem) const override
Get all statistics.
static const Options options_
Options.
void init(const Dict &opts) override
Initialize.
casadi_int ng_
Number of constraints.
int init_mem(void *mem) const override
Initalize memory block.
void serialize_body(SerializingStream &s) const override
Serialize an object without type information.
casadi_int nx_
Number of variables.
void set_work(void *mem, const double **&arg, double **&res, casadi_int *&iw, double *&w) const override
Set the (persistent) work vectors.
Function fcallback_
callback function, executed at each iteration
Function create_function(const Function &oracle, const std::string &fname, const std::vector< std::string > &s_in, const std::vector< std::string > &s_out, const Function::AuxOut &aux=Function::AuxOut(), const Dict &opts=Dict())
int calc_function(OracleMemory *m, const std::string &fcn, const double *const *arg=nullptr, int thread_id=0) const
static void registerPlugin(const Plugin &plugin, bool needs_lock=true)
Register an integrator in the factory.
bool verbose_
Verbose printout.
void clear_mem()
Clear all memory (called from destructor)
Helper class for Serialization.
void version(const std::string &name, int v)
void pack(const Sparsity &e)
Serializes an object to the output stream.
casadi_int nnz_lower(bool strictly=false) const
Number of non-zeros in the lower triangular half,.
casadi_int nnz() const
Get the number of (structural) non-zeros.
const casadi_int * row() const
Get a reference to row-vector,.
const casadi_int * colind() const
Get a reference to the colindex of all column element (see class description)
std::map< std::string, casadi_int > int_opts_
int init_mem(void *mem) const override
Initalize memory block.
static const std::string meta_doc
A documentation string.
void init(const Dict &opts) override
Initialize.
static const Options options_
Options.
static const char * return_codes(casadi_int flag)
static Nlpsol * creator(const std::string &name, const Function &nlp)
Create a new NLP Solver.
std::map< std::string, double > double_opts_
WorhpInterface(const std::string &name, const Function &nlp)
std::map< std::string, bool > bool_opts_
void set_work(void *mem, const double **&arg, double **&res, casadi_int *&iw, double *&w) const override
Set the (persistent) work vectors.
Dict get_stats(void *mem) const override
Get all statistics.
void serialize_body(SerializingStream &s) const override
Serialize an object without type information.
static ProtoFunction * deserialize(DeserializingStream &s)
Deserialize into MX.
int solve(void *mem) const override
~WorhpInterface() override
@ NLPSOL_G
Constraints function at the optimal solution (ng x 1)
@ NLPSOL_X
Decision variables at the optimal solution (nx x 1)
@ NLPSOL_LAM_P
Lagrange multipliers for bounds on P at the solution (np x 1)
@ NLPSOL_F
Cost function value at the optimal solution (1 x 1)
@ NLPSOL_LAM_G
Lagrange multipliers for bounds on G at the solution (ng x 1)
@ NLPSOL_LAM_X
Lagrange multipliers for bounds on X at the solution (nx x 1)
int CASADI_NLPSOL_WORHP_EXPORT casadi_register_nlpsol_worhp(Nlpsol::Plugin *plugin)
void casadi_copy(const T1 *x, casadi_int n, T1 *y)
COPY: y <-x.
std::string str(const T &v)
String representation, any type.
GenericType::Dict Dict
C++ equivalent of Python's dict or MATLAB's struct.
void CASADI_NLPSOL_WORHP_EXPORT casadi_load_nlpsol_worhp()
void casadi_scal(casadi_int n, T1 alpha, T1 *x)
SCAL: x <- alpha*x.
void casadi_clear(T1 *x, casadi_int n)
CLEAR: x <- 0.
void worhp_disp(int mode, const char message[])
casadi_nlpsol_data< double > d_nlp
Options metadata for a class.
~WorhpMemory()
Destructor.
const char * return_status
WorhpMemory()
Constructor.