ccopt_interface.cpp

/*
 *    This file is part of CasADi.
 *
 *    CasADi -- A symbolic framework for dynamic optimization.
 *    Copyright (C) 2010-2023 Joel Andersson, Joris Gillis, Moritz Diehl,
 *                            KU Leuven. All rights reserved.
 *    Copyright (C) 2011-2014 Greg Horn
 *
 *    CasADi is free software; you can redistribute it and/or
 *    modify it under the terms of the GNU Lesser General Public
 *    License as published by the Free Software Foundation; either
 *    version 3 of the License, or (at your option) any later version.
 *
 *    CasADi is distributed in the hope that it will be useful,
 *    but WITHOUT ANY WARRANTY; without even the implied warranty of
 *    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 *    Lesser General Public License for more details.
 *
 *    You should have received a copy of the GNU Lesser General Public
 *    License along with CasADi; if not, write to the Free Software
 *    Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
 *
 */
 
 
#include "ccopt_interface.hpp"
#include <ccopt_runtime_str.h>
 
#include "casadi/core/casadi_misc.hpp"
#include "../../core/global_options.hpp"
#include "../../core/casadi_interrupt.hpp"
#include "../../core/convexify.hpp"
 
#include <ctime>
#include <stdlib.h>
#include <iostream>
#include <iomanip>
#include <chrono>
#include <cstring>
#include <string>
 
namespace casadi {
 
extern "C"
int CASADI_NLPSOL_CCOPT_EXPORT
casadi_register_nlpsol_ccopt(Nlpsol::Plugin* plugin) {
  plugin->creator = MadmpecInterface::creator;
  plugin->name = "ccopt";
  plugin->doc = MadmpecInterface::meta_doc.c_str();
  plugin->version = CASADI_VERSION;
  plugin->options = &MadmpecInterface::options_;
  plugin->deserialize = &MadmpecInterface::deserialize;
  return 0;
}
 
extern "C"
void CASADI_NLPSOL_CCOPT_EXPORT casadi_load_nlpsol_ccopt() {
  Nlpsol::registerPlugin(casadi_register_nlpsol_ccopt);
}
 
MadmpecInterface::MadmpecInterface(const std::string& name, const Function& nlp)
  : Nlpsol(name, nlp) {
}
 
MadmpecInterface::~MadmpecInterface() {
  clear_mem();
}
 
const Options MadmpecInterface::options_
= {{&Nlpsol::options_},
   {{"ccopt",
     {OT_DICT,
      "Options to be passed to ccopt relaxation algorithm"}},
    {"madnlp",
     {OT_DICT,
      "Options to be passed to madnlp"}},
    {"convexify_strategy",
     {OT_STRING,
      "NONE|regularize|eigen-reflect|eigen-clip. "
      "Strategy to convexify the Lagrange Hessian before passing it to the solver."}},
    {"convexify_margin",
     {OT_DOUBLE,
      "When using a convexification strategy, make sure that "
      "the smallest eigenvalue is at least this (default: 1e-7)."}},
    {"cc_pairs",
       {OT_INTVECTORVECTOR,
        "List of complementary constraints on simple bounds. "
        "Pair (i, j) encodes complementarity between the bounds on variable i and variable j."}},
    {"cc_types",
       {OT_INTVECTOR,
        "Type of cc pairs (default: 0). "}},
   }
};
 
// Recursively flatten options for libmad options dict.
void flatten_opts(Dict& ret, const Dict& opts, const std::string& prefix) {
  for (const auto& kv : opts) {
    switch (kv.second.getType()) {
     case OT_DICT:
       flatten_opts(ret, kv.second, prefix + kv.first + ".");
       break;
     default:
       ret[prefix + kv.first] = kv.second;
    }
  }
}
 
void MadmpecInterface::init(const Dict& opts) {
  // Call the init method of the base class
  Nlpsol::init(opts);
  casadi_int struct_cnt=0;
 
  // Default options
  std::string convexify_strategy = "none";
  double convexify_margin = 1e-7;
  casadi_int max_iter_eig = 200;
  std::vector< std::vector<casadi_int> > cc_pairs;
  std::vector<casadi_int> cc_types;
  convexify_ = false;
 
  calc_g_ = true;
  calc_f_ = true;
 
  // Read options
  for (auto&& op : opts) {
    if (op.first=="convexify_strategy") {
      convexify_strategy = op.second.to_string();
    } else if (op.first=="convexify_margin") {
      convexify_margin = op.second;
    } else if (op.first=="max_iter") {
      max_iter_eig = op.second;
    } else if (op.first=="madnlp") {
      flatten_opts(opts_, op.second, "");
    } else if (op.first=="ccopt") {
      flatten_opts(mpcc_opts_, op.second, "");
    } else if (op.first=="cc_pairs") {
      cc_pairs = op.second;
    } else if (op.first=="cc_types") {
      cc_types = op.second;
    }
  }
  ind_cc1_.reserve(cc_pairs.size());
  ind_cc2_.reserve(cc_pairs.size());
  cctypes_.reserve(cc_pairs.size());
  if (cc_types.empty()) {
    cc_types.resize(cc_pairs.size(), 0);
  }
  casadi_assert(cc_pairs.size()==cc_types.size(),
    "Options 'cc_pairs' and 'cc_types' must have the same length.");
  for (auto && e : cc_types) {
    cctypes_.push_back(e);
  }
  // CCType: VarVar=0, VarCon=1, ConVar=2, ConCon=3
  const libmad_int* type = get_ptr(cctypes_);
  for (auto && e : cc_pairs) {
    casadi_assert(e.size()==2, "Complementary constraints must come in pairs.");
    casadi_assert(e[0]>=0, "Invalid index.");
    casadi_assert(e[1]>=0, "Invalid index.");
    bool first_is_var = (*type == 0 || *type == 1);
    bool second_is_var = (*type == 0 || *type == 2);
    casadi_int n_first = first_is_var ? nx_ : ng_;
    casadi_int n_second = second_is_var ? nx_ : ng_;
    casadi_assert(e[0]<n_first, "Invalid index for first slot of cc_pair "
                  "(type " + str(*type) + ", limit " + str(n_first) + ").");
    casadi_assert(e[1]<n_second, "Invalid index for second slot of cc_pair "
                  "(type " + str(*type) + ", limit " + str(n_second) + ").");
    ind_cc1_.push_back(e[0]+1);
    ind_cc2_.push_back(e[1]+1);
    type++;
  }
  // Do we need second order derivatives?
  exact_hessian_ = true;
  auto hessian_approximation = opts_.find("hessian_approximation");
  if (hessian_approximation!=opts_.end()) {
    exact_hessian_ = hessian_approximation->second == "exact";
  }
 
  // Setup NLP functions
 
  create_function("nlp_f", {"x", "p"}, {"f"});
  create_function("nlp_g", {"x", "p"}, {"g"});
 
  if (!has_function("nlp_grad_f")) {
    create_function("nlp_grad_f", {"x", "p"}, {"grad:f:x"});
  }
 
  if (!has_function("nlp_jac_g")) {
    create_function("nlp_jac_g", {"x", "p"}, {"jac:g:x"});
  }
  jacg_sp_ = get_function("nlp_jac_g").sparsity_out(0);
 
  if (!has_function("nlp_hess_l")) {
    create_function("nlp_hess_l", {"x", "p", "lam:f", "lam:g"},
                    {"tril:hess:gamma:x:x"}, {{"gamma", {"f", "g"}}});
  }
  hesslag_sp_ = get_function("nlp_hess_l").sparsity_out(0);
  casadi_assert(hesslag_sp_.is_tril(), "Hessian must be lower triangular");
 
  if (convexify_strategy!="none") {
    convexify_ = true;
    Dict opts;
    opts["strategy"] = convexify_strategy;
    opts["margin"] = convexify_margin;
    opts["max_iter_eig"] = max_iter_eig;
    opts["verbose"] = verbose_;
    hesslag_sp_ = Convexify::setup(convexify_data_, hesslag_sp_, opts);
  }
 
  set_ccopt_prob();
 
  // Allocate memory
  casadi_int sz_arg, sz_res, sz_w, sz_iw;
  casadi_ccopt_work(&p_, &sz_arg, &sz_res, &sz_iw, &sz_w);
 
  alloc_arg(sz_arg, true);
  alloc_res(sz_res, true);
  alloc_iw(sz_iw, true);
  alloc_w(sz_w, true);
 
  std::vector<char*> _argv = {};
  std::string s;
 
  int argc = _argv.size();
  char** argv = reinterpret_cast<char**>(_argv.data());
 
}
 
int MadmpecInterface::init_mem(void* mem) const {
  if (Nlpsol::init_mem(mem)) return 1;
  if (!mem) return 1;
  auto m = static_cast<MadmpecMemory*>(mem);
  // Now create the new options struct
  libmad_create_options_dict(&(m->d.nlp_opts));
  for (const auto& kv : opts_) {
    switch (kv.second.getType()) {
     case OT_DOUBLE:
       libmad_set_double_option(m->d.nlp_opts, kv.first.c_str(), kv.second);
       break;
     case OT_INT:
       libmad_set_int64_option(m->d.nlp_opts, kv.first.c_str(), kv.second.to_int());
       break;
     case OT_STRING:
     {
       std::string s = kv.second.to_string();
       libmad_set_string_option(m->d.nlp_opts, kv.first.c_str(), s.c_str());
     }
     break;
     case OT_BOOL:
       libmad_set_bool_option(m->d.nlp_opts, kv.first.c_str(), kv.second.to_bool());
       break;
     default:
       casadi_error("Unknown option type.");
    }
  }
  // Now create the new options struct
  libmad_create_options_dict(&(m->d.mpcc_opts));
  for (const auto& kv : mpcc_opts_) {
    switch (kv.second.getType()) {
     case OT_DOUBLE:
       libmad_set_double_option(m->d.mpcc_opts, kv.first.c_str(), kv.second);
       break;
     case OT_INT:
       libmad_set_int64_option(m->d.mpcc_opts, kv.first.c_str(), kv.second.to_int());
       break;
     case OT_STRING:
     {
       std::string s = kv.second.to_string();
       libmad_set_string_option(m->d.mpcc_opts, kv.first.c_str(), s.c_str());
     }
     break;
     case OT_BOOL:
       libmad_set_bool_option(m->d.mpcc_opts, kv.first.c_str(), kv.second.to_bool());
       break;
     default:
       casadi_error("Unknown option type.");
    }
  }
 
  m->d.ind_cc1 = ind_cc1_.data();
  m->d.ind_cc2 = ind_cc2_.data();
  m->d.cctypes = cctypes_.data();
  m->d.ncc = ind_cc1_.size();
  casadi_ccopt_init_mem(&m->d);
 
  return 0;
}
 
void MadmpecInterface::free_mem(void* mem) const {
  auto m = static_cast<MadmpecMemory*>(mem);
  casadi_ccopt_free_mem(&m->d);
  delete static_cast<MadmpecMemory*>(mem);
}
 
void MadmpecInterface::set_work(void* mem, const double**& arg, double**& res,
                              casadi_int*& iw, double*& w) const {
  auto m = static_cast<MadmpecMemory*>(mem);
 
  // Set work in base classes
  Nlpsol::set_work(mem, arg, res, iw, w);
 
  m->d.prob = &p_;
  m->d.nlp = &m->d_nlp;
 
  casadi_ccopt_set_work(&m->d, &arg, &res, &iw, &w);
 
  m->d.nlp->oracle->m = static_cast<void*>(m);
}
 
int MadmpecInterface::solve(void* mem) const {
  auto m = static_cast<MadmpecMemory*>(mem);
 
  int ret = casadi_ccopt_presolve(&m->d);
  if (ret) throw CasadiException("CCOPTError");
  ret = casadi_ccopt_solve(&m->d);
  if (ret) throw CasadiException("CCOPTError");
 
  m->success = m->d.success;
  m->unified_return_status = static_cast<UnifiedReturnStatus>(m->d.unified_return_status);
 
  return 0;
}
 
Dict MadmpecInterface::get_stats(void* mem) const {
  Dict stats = Nlpsol::get_stats(mem);
  auto m = static_cast<MadmpecMemory*>(mem);
  libmad_int iter, status;
  double primal_feas, dual_feas, cc_feas, total_wall_time;
  std::vector<libmad_real> multipliers_x1, multipliers_x2;
  multipliers_x1.resize(ind_cc1_.size());
  multipliers_x2.resize(ind_cc2_.size());
  ccopt_relaxation_get_iters(m->d.stats, &iter);
  ccopt_relaxation_get_total_wall_time(m->d.stats, &total_wall_time);
  ccopt_relaxation_get_status(m->d.stats, &status);
  ccopt_relaxation_get_dual_feas(m->d.stats, &dual_feas);
  ccopt_relaxation_get_primal_feas(m->d.stats, &primal_feas);
  ccopt_relaxation_get_cc_feas(m->d.stats, &cc_feas);
  ccopt_relaxation_get_multipliers_x1(m->d.stats, multipliers_x1.data());
  ccopt_relaxation_get_multipliers_x2(m->d.stats, multipliers_x2.data());
 
  stats["iter_count"] = static_cast<casadi_int>(iter);
  Dict ccopt;
  ccopt["dual_feas"] = dual_feas;
  ccopt["cc_feas"] = cc_feas;
  ccopt["primal_feas"] = primal_feas;
  ccopt["status"] = static_cast<casadi_int>(status);
  ccopt["multipliers_x1"] = multipliers_x1;
  ccopt["multipliers_x2"] = multipliers_x2;
  ccopt["total_wall_time"] = total_wall_time;
  stats["ccopt"] = ccopt;
  return stats;
}
 
void MadmpecInterface::set_ccopt_prob() {
  // assign pointer to internal structur casadi_nlp_prob
  // p_nlp_ ~ casadi_nlp_prob casadi internal
  p_.nlp = &p_nlp_;
  // p_ casadi_ccopt_prob
 
  p_.sp_a = jacg_sp_;
  p_.sp_h = hesslag_sp_;
 
  p_.nlp_hess_l = OracleCallback("nlp_hess_l", this);
  p_.nlp_jac_g = OracleCallback("nlp_jac_g", this);
  p_.nlp_grad_f = OracleCallback("nlp_grad_f", this);
  p_.nlp_f = OracleCallback("nlp_f", this);
  p_.nlp_g = OracleCallback("nlp_g", this);
 
  casadi_ccopt_setup(&p_);
}
 
void MadmpecInterface::codegen_options(CodeGenerator& g, const Dict& dict,
                                       const std::string& field) const {
  g << "libmad_create_options_dict(&(" + codegen_mem(g) + "." + field + "));\n";
  for (const auto& kv : dict) {
    switch (kv.second.getType()) {
     case OT_DOUBLE:
       g << "libmad_set_double_option(" + codegen_mem(g) + "." + field + ", \""
            + kv.first + "\", " + str(kv.second) + ");\n";
       break;
     case OT_INT:
       g << "libmad_set_int64_option(" + codegen_mem(g) + "." + field + ", \""
            + kv.first + "\", " + str(kv.second) + ");\n";
       break;
     case OT_STRING:
     {
       std::string s = kv.second.to_string();
       g << "libmad_set_string_option(" + codegen_mem(g) + "." + field + ", \""
            + kv.first + "\", \"" + s + "\");\n";
     }
     break;
     case OT_BOOL:
       g << "libmad_set_bool_option(" + codegen_mem(g) + "." + field + ", \""
            + kv.first + "\", " + str(kv.second) + ");\n";
       break;
     default:
       casadi_error("Unknown option type.");
    }
  }
}
 
void MadmpecInterface::codegen_init_mem(CodeGenerator& g) const {
  codegen_options(g, opts_, "nlp_opts");
  codegen_options(g, mpcc_opts_, "mpcc_opts");
 
  g << codegen_mem(g) + ".ind_cc1 = " + g.constant(ind_cc1_) + ";\n";
  g << codegen_mem(g) + ".ind_cc2 = " + g.constant(ind_cc2_) + ";\n";
  g << codegen_mem(g) + ".cctypes = " + g.constant(cctypes_) + ";\n";
  g << codegen_mem(g) + ".ncc = " + str(ind_cc1_.size()) + ";\n";
 
  g << "casadi_ccopt_init_mem(&" + codegen_mem(g) + ");\n";
  g << "return 0;\n";
}
 
void MadmpecInterface::codegen_free_mem(CodeGenerator& g) const {
  // memory deallocation
  g << "casadi_ccopt_free_mem(&" + codegen_mem(g) + ");\n";
}
 
void MadmpecInterface::codegen_declarations(CodeGenerator& g) const {
  Nlpsol::codegen_declarations(g);
  g.add_auxiliary(CodeGenerator::AUX_NLP);
  g.add_auxiliary(CodeGenerator::AUX_MAX);
  g.add_auxiliary(CodeGenerator::AUX_COPY);
  g.add_auxiliary(CodeGenerator::AUX_PROJECT);
  g.add_auxiliary(CodeGenerator::AUX_SCAL);
  g.add_auxiliary(CodeGenerator::AUX_SPARSITY);
  g.add_auxiliary(CodeGenerator::AUX_ORACLE_CALLBACK);
  g.add_auxiliary(CodeGenerator::AUX_DENSIFY);
  g.add_auxiliary(CodeGenerator::AUX_SPARSIFY);
  g.add_auxiliary(CodeGenerator::AUX_INF);
  g.add_dependency(get_function("nlp_f"));
  g.add_dependency(get_function("nlp_grad_f"));
  g.add_dependency(get_function("nlp_g"));
  g.add_dependency(get_function("nlp_jac_g"));
  g.add_dependency(get_function("nlp_hess_l"));
  g.add_include("libMad.h");
}
 
void MadmpecInterface::codegen_body(CodeGenerator& g) const {
  codegen_body_enter(g);
  g.auxiliaries << g.sanitize_source(ccopt_runtime_str, {"casadi_real"});
 
  g.local("d", "struct casadi_ccopt_data*");
  g.init_local("d", "&" + codegen_mem(g));
  g.local("p", "struct casadi_ccopt_prob");
  set_ccopt_prob(g);
 
  g << "casadi_ccopt_set_work(d, &arg, &res, &iw, &w);\n";
  g << "casadi_oracle_set_work(d->nlp->oracle, &arg, &res, &iw, &w);\n";
  g << "casadi_ccopt_presolve(d);\n";
 
  g << "casadi_ccopt_solve(d);\n";
 
  codegen_body_exit(g);
 
  if (error_on_fail_) {
    g << "return d->unified_return_status;\n";
  } else {
    g << "return 0;\n";
  }
}
 
void MadmpecInterface::set_ccopt_prob(CodeGenerator& g) const {
  if (jacg_sp_.size1()>0 && jacg_sp_.nnz()==0) {
    casadi_error("Empty sparsity pattern not supported in CCOPT C interface");
  }
  g << "d->nlp = &d_nlp;\n";
  g << "d->prob = &p;\n";
  g << "p.nlp = &p_nlp;\n";
 
  g.setup_callback("p.nlp_jac_g", get_function("nlp_jac_g"));
  g.setup_callback("p.nlp_grad_f", get_function("nlp_grad_f"));
  g.setup_callback("p.nlp_f", get_function("nlp_f"));
  g.setup_callback("p.nlp_g", get_function("nlp_g"));
  g.setup_callback("p.nlp_hess_l", get_function("nlp_hess_l"));
 
  g << "p.sp_a = " << g.sparsity(jacg_sp_) << ";\n";
  if (exact_hessian_) {
    g << "p.sp_h = " << g.sparsity(hesslag_sp_) << ";\n";
  } else {
    g << "p.sp_h = 0;\n";
  }
 
  g << "casadi_ccopt_setup(&p);\n";
}
 
MadmpecInterface::MadmpecInterface(DeserializingStream& s) : Nlpsol(s) {
  s.version("MadmpecInterface", 1);
  s.unpack("MadmpecInterface::jacg_sp", jacg_sp_);
  s.unpack("MadmpecInterface::hesslag_sp", hesslag_sp_);
  s.unpack("MadmpecInterface::exact_hessian", exact_hessian_);
  s.unpack("MadmpecInterface::opts", opts_);
  s.unpack("MadmpecInterface::convexify", convexify_);
  s.unpack("MadmpecInterface::ind_cc1", ind_cc1_);
  s.unpack("MadmpecInterface::ind_cc2", ind_cc2_);
  s.unpack("MadmpecInterface::cctypes", cctypes_);
 
  set_ccopt_prob();
}
 
void MadmpecInterface::serialize_body(SerializingStream &s) const {
  Nlpsol::serialize_body(s);
  s.version("MadmpecInterface", 1);
 
  s.pack("MadmpecInterface::jacg_sp", jacg_sp_);
  s.pack("MadmpecInterface::hesslag_sp", hesslag_sp_);
  s.pack("MadmpecInterface::exact_hessian", exact_hessian_);
  s.pack("MadmpecInterface::opts", opts_);
  s.pack("MadmpecInterface::convexify", convexify_);
  s.pack("MadmpecInterface::ind_cc1", ind_cc1_);
  s.pack("MadmpecInterface::ind_cc2", ind_cc2_);
  s.pack("MadmpecInterface::cctypes", cctypes_);
 
}
 
} // namespace casadi