madnlp_runtime.hpp

//
//    MIT No Attribution
//
//    Copyright (C) 2010-2023 Joel Andersson, Joris Gillis, Moritz Diehl, KU Leuven.
//
//    Permission is hereby granted, free of charge, to any person obtaining a copy of this
//    software and associated documentation files (the "Software"), to deal in the Software
//    without restriction, including without limitation the rights to use, copy, modify,
//    merge, publish, distribute, sublicense, and/or sell copies of the Software, and to
//    permit persons to whom the Software is furnished to do so.
//
//    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
//    INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
//    PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
//    HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
//    OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
//    SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
//
 
// C-REPLACE "SOLVER_RET_SUCCESS" "0"
// C-REPLACE "SOLVER_RET_UNKNOWN" "1"
// C-REPLACE "SOLVER_RET_LIMITED" "2"
// C-REPLACE "SOLVER_RET_NAN" "3"
// C-REPLACE "SOLVER_RET_INFEASIBLE" "4"
// C-REPLACE "SOLVER_RET_EXCEPTION" "5"
 
// C-REPLACE "casadi_nlpsol_prob<T1>" "struct casadi_nlpsol_prob"
// C-REPLACE "casadi_nlpsol_data<T1>" "struct casadi_nlpsol_data"
 
// C-REPLACE "reinterpret_cast<int**>" "(int**) "
// C-REPLACE "reinterpret_cast<int*>" "(int*) "
// C-REPLACE "const_cast<int*>" "(int*) "
 
// C-REPLACE "OracleCallback" "struct casadi_oracle_callback"
 
// SYMBOL "madnlp_prob"
template<typename T1>
struct casadi_madnlp_prob {
  const casadi_nlpsol_prob<T1>* nlp;
 
  // Sparsity patterns
  const casadi_int *sp_h, *sp_a;
 
  casadi_int nnz_hess_l, nnz_jac_g;
 
  OracleCallback nlp_hess_l;
  OracleCallback nlp_jac_g;
  OracleCallback nlp_grad_f;
  OracleCallback nlp_f;
  OracleCallback nlp_g;
};
// C-REPLACE "casadi_madnlp_prob<T1>" "struct casadi_madnlp_prob"
 
// SYMBOL "madnlp_data"
template<typename T1>
struct casadi_madnlp_data {
  // Problem structure
  const casadi_madnlp_prob<T1>* prob;
  // Problem structure
  casadi_nlpsol_data<T1>* nlp;
 
  const T1** arg;
  T1** res;
  casadi_int* iw;
  T1 *w;
 
  // temporary data
  T1 *x, *lam_g, *g, *f, *jac_g, *grad_f, *hess_l, *grad_l;
 
  int unified_return_status;
  int success;
 
  //struct blasfeo_dvec v, r;
  //struct blasfeo_dmat R;
 
  CNLPModel* cnlp_model;
  OptsDict* libmad_opts;
  MadNLPExecutionStats* stats;
  MadNLPSolver* solver;
};
// C-REPLACE "casadi_madnlp_data<T1>" "struct casadi_madnlp_data"
 
// SYMBOL "madnlp_setup"
template<typename T1>
void casadi_madnlp_setup(casadi_madnlp_prob<T1>* p) {
  if (p->sp_h) {
    p->nnz_hess_l = p->sp_h[2+p->sp_h[1]];
  } else {
    p->nnz_hess_l = 0;
  }
  p->nnz_jac_g = p->sp_a[2+p->sp_a[1]];
}
 
// C-REPLACE "static_cast< casadi_madnlp_data<T1>* >" "(struct casadi_madnlp_data*)"
// C-REPLACE "casadi_oracle_data<T1>" "struct casadi_oracle_data"
// C-REPLACE "calc_function" "casadi_oracle_call"
// C-REPLACE "casadi_error" "//casadi_error"
 
// SYMBOL "madnlp_constr_jac_structure"
template<typename T1>
int casadi_madnlp_constr_jac_structure(libmad_int* I, libmad_int* J, void* user_data)
{
  casadi_int cc, el;
  const casadi_int *colind, *row;
  casadi_madnlp_data<T1>* d = static_cast< casadi_madnlp_data<T1>* >(user_data);
  const casadi_madnlp_prob<T1>* p = d->prob;
 
  // convert ccs to cco
  casadi_int ncol = p->sp_a[1];
  colind = p->sp_a+2;
  row = colind+ncol+1;
 
  for (cc=0; cc<ncol; ++cc) {
      for (el=colind[cc]; el<colind[cc+1]; ++el) {
          *I++ = row[el]+1;
          *J++ = cc+1;
      }
  }
  return 0;
}
// C-REPLACE "casadi_madnlp_constr_jac_structure<T1>" "casadi_madnlp_constr_jac_structure"
 
// SYMBOL "madnlp_lag_hess_structure"
template<typename T1>
int casadi_madnlp_lag_hess_structure(libmad_int* I, libmad_int* J, void* user_data)
{
  casadi_int cc, el;
  const casadi_int *colind, *row;
  casadi_madnlp_data<T1>* d = static_cast< casadi_madnlp_data<T1>* >(user_data);
  const casadi_madnlp_prob<T1>* p = d->prob;
 
  // convert ccs to cco
  casadi_int ncol = p->sp_h[1];
  colind = p->sp_h+2;
  row = colind+ncol+1;
 
  for (cc=0; cc<ncol; ++cc) {
      for (el=colind[cc]; el<colind[cc+1]; ++el) {
          *I++ = row[el]+1;
          *J++ = cc+1;
      }
  }
  return 0;
}
// C-REPLACE "casadi_madnlp_lag_hess_structure<T1>" "casadi_madnlp_lag_hess_structure"
 
// SYMBOL "madnlp_eval_constr_jac"
template<typename T1>
int casadi_madnlp_eval_constr_jac(const T1* w, T1* res, void* user_data) {
  casadi_madnlp_data<T1>* d = static_cast< casadi_madnlp_data<T1>* >(user_data);
  casadi_nlpsol_data<T1>* d_nlp = d->nlp;
  casadi_oracle_data<T1>* d_oracle = d_nlp->oracle;
 
  d_oracle->arg[0] = w;
  d_oracle->arg[1] = d_nlp->p;
  d_oracle->res[0] = res;
 
  return calc_function(&d->prob->nlp_jac_g, d_oracle);
}
 
// SYMBOL "madnlp_eval_constr"
template<typename T1>
int casadi_madnlp_eval_constr(const T1* w, T1* res, void* user_data) {
  casadi_madnlp_data<T1>* d = static_cast< casadi_madnlp_data<T1>* >(user_data);
  casadi_nlpsol_data<T1>* d_nlp = d->nlp;
  casadi_oracle_data<T1>* d_oracle = d_nlp->oracle;
 
  d_oracle->arg[0] = w;
  d_oracle->arg[1] = d_nlp->p;
  d_oracle->res[0] = res;
 
  return calc_function(&d->prob->nlp_g, d_oracle);
}
 
// SYMBOL "madnlp_eval_obj_grad"
template<typename T1>
int casadi_madnlp_eval_obj_grad(const T1* w, T1* res, void* user_data) {
  casadi_madnlp_data<T1>* d = static_cast< casadi_madnlp_data<T1>* >(user_data);
  casadi_nlpsol_data<T1>* d_nlp = d->nlp;
  casadi_oracle_data<T1>* d_oracle = d_nlp->oracle;
 
  // TODO add argument
  //T1 objective_scale = 1.0;
 
  d_oracle->arg[0] = w;
  d_oracle->arg[1] = d_nlp->p;
  d_oracle->res[0] = res;
  return calc_function(&d->prob->nlp_grad_f, d_oracle);
}
 
// SYMBOL "madnlp_eval_obj"
template<typename T1>
int casadi_madnlp_eval_obj(const T1* w, T1* res, void* user_data) {
  casadi_madnlp_data<T1>* d = static_cast< casadi_madnlp_data<T1>* >(user_data);
  casadi_nlpsol_data<T1>* d_nlp = d->nlp;
  casadi_oracle_data<T1>* d_oracle = d_nlp->oracle;
 
  // TODO add argument
  //T1 objective_scale = 1.0;
 
  d_oracle->arg[0] = w;
  d_oracle->arg[1] = d_nlp->p;
  d_oracle->res[0] = res;
  return calc_function(&d->prob->nlp_f, d_oracle);
}
 
// SYMBOL "madnlp_eval_lag_hess"
template<typename T1>
int casadi_madnlp_eval_lag_hess(T1 objective_scale, const T1* w, const T1* lam,
                                            T1* res, void* user_data){
  casadi_madnlp_data<T1>* d = static_cast< casadi_madnlp_data<T1>* >(user_data);
  casadi_nlpsol_data<T1>* d_nlp = d->nlp;
  casadi_oracle_data<T1>* d_oracle = d_nlp->oracle;
 
  // evaluate hessian with given parameters / inputs
  d_oracle->arg[0] = w;
  d_oracle->arg[1] = d_nlp->p;
  d_oracle->arg[2] = &objective_scale;
  d_oracle->arg[3] = lam;
  d_oracle->res[0] = res;
  
  return calc_function(&d->prob->nlp_hess_l, d_oracle);
}
 
// C-REPLACE "const_cast<T1*>" "(T1*)"
 
// C-REPLACE "casadi_madnlp_eval_constr_jac<T1>" "casadi_madnlp_eval_constr_jac"
// C-REPLACE "casadi_madnlp_eval_obj_grad<T1>" "casadi_madnlp_eval_obj_grad"
// C-REPLACE "casadi_madnlp_eval_obj<T1>" "casadi_madnlp_eval_obj"
// C-REPLACE "casadi_madnlp_eval_constr<T1>" "casadi_madnlp_eval_constr"
// C-REPLACE "casadi_madnlp_eval_lag_hess<T1>" "casadi_madnlp_eval_lag_hess"
// C-REPLACE "std::numeric_limits<T1>::infinity()" "casadi_inf"
 
// SYMBOL "madnlp_init_mem"
template<typename T1>
int casadi_madnlp_init_mem(casadi_madnlp_data<T1>* d) {
  // Problem structure
  
  return 0;
}
 
// SYMBOL "madnlp_free_mem"
template<typename T1>
void casadi_madnlp_free_mem(casadi_madnlp_data<T1>* d) {
  madnlp_delete_solver(d->solver);
  d->solver = 0;
  madnlp_delete_stats(d->stats);
  d->stats = 0;
  libmad_delete_options_dict(d->libmad_opts);
}
 
// SYMBOL "madnlp_work"
template<typename T1>
void casadi_madnlp_work(const casadi_madnlp_prob<T1>* p, casadi_int* sz_arg, casadi_int* sz_res, casadi_int* sz_iw, casadi_int* sz_w) {
  casadi_nlpsol_work(p->nlp, sz_arg, sz_res, sz_iw, sz_w);
 
  // Temporary work vectors
  *sz_w = casadi_max(*sz_w, 2*(p->nlp->nx+p->nlp->ng)); // pv
}
 
// SYMBOL "madnlp_set_work"
template<typename T1>
void casadi_madnlp_set_work(casadi_madnlp_data<T1>* d, const T1*** arg, T1*** res, casadi_int** iw, T1** w) {
  // Problem structure
  d->arg = *arg;
  d->res = *res;
  d->iw = *iw;
  d->w = *w;
}
 
// SYMBOL "madnlp_presolve"
template<typename T1>
int casadi_madnlp_presolve(casadi_madnlp_data<T1>* d) {
  int ret;
  const casadi_madnlp_prob<T1>* p = d->prob;
  const casadi_nlpsol_prob<T1>* p_nlp = p->nlp;
  casadi_nlpsol_data<T1>* d_nlp = d->nlp;
 
  d->unified_return_status = SOLVER_RET_UNKNOWN;
  d->success = 0;
  ret = libmad_nlpmodel_create(&(d->cnlp_model),
                               "MadNLP",
                               p_nlp->nx, p_nlp->ng,
                               p->nnz_jac_g, p->nnz_hess_l,
                               casadi_madnlp_constr_jac_structure<T1>,
                               casadi_madnlp_lag_hess_structure<T1>,
                               casadi_madnlp_eval_obj<T1>,
                               casadi_madnlp_eval_constr<T1>,
                               casadi_madnlp_eval_obj_grad<T1>,
                               casadi_madnlp_eval_constr_jac<T1>,
                               casadi_madnlp_eval_lag_hess<T1>,
                               d
    );
  if (ret != 0) return -1; // Failed to create model
  // set initial guess
  ret = libmad_nlpmodel_set_numerics(d->cnlp_model,
                                     d_nlp->z, d_nlp->lam + p_nlp->nx,
                                     d_nlp->lbx, d_nlp->ubx,
                                     d_nlp->lbg, d_nlp->ubg);
  if (ret != 0) return -2; // Failed to set numerics
  ret = madnlp_create_solver(&(d->solver), d->cnlp_model, d->libmad_opts);
  if (ret != 0) return -3; // Failed to create solver
 
  return 0;
}
 
// SYMBOL "madnlp_solve"
template<typename T1>
int casadi_madnlp_solve(casadi_madnlp_data<T1>* d) {
  // Problem structure
  casadi_nlpsol_data<T1>* d_nlp = d->nlp;
  const casadi_madnlp_prob<T1>* p = d->prob;
  const casadi_nlpsol_prob<T1>* p_nlp = p->nlp;
  int ret = madnlp_solve(d->solver, d->libmad_opts, &(d->stats));
  madnlp_delete_solver(d->solver);
  if (ret!=0) return -1; // cleanup done in free_mem
 
  // get objective
  ret = madnlp_get_obj(d->stats, &(d_nlp->objective)); if (ret != 0) return -2;
  // get primal solution for x
  ret = madnlp_get_solution(d->stats, d_nlp->z); if (ret != 0) return -2;
  // get the bound multipliers
  ret = madnlp_get_bound_multipliers(d->stats, d_nlp->lam); if (ret != 0) return -2;
  // get the nonlinear constraint function values
  ret = madnlp_get_constraints(d->stats, d_nlp->z + p_nlp->nx); if (ret != 0) return -2;
  // get the nonlinear constraint multipliers
  ret = madnlp_get_multipliers(d->stats, d_nlp->lam + p_nlp->nx); if (ret != 0) return -2;
 
  bool success_b;
  ret = madnlp_get_success(d->stats, &(success_b)); if (ret != 0) return -2;
 
  libmad_int status;
  ret = madnlp_get_status(d->stats, &status); if (ret != 0) return -2;
  if(success_b){
    d->success = 1;
    d->unified_return_status = SOLVER_RET_SUCCESS;
  }
 
  if (status == 5) d->unified_return_status = SOLVER_RET_INFEASIBLE;
  if (status == -2 || (status <= -7 && status >=1 -11)) d->unified_return_status = SOLVER_RET_NAN;
 
  return 0;
}