hpmpc_interface.hpp

/*
 *    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
 *
 */
 
 
#ifndef CASADI_HPMPC_INTERFACE_HPP
#define CASADI_HPMPC_INTERFACE_HPP
 
#include "casadi/core/conic_impl.hpp"
#include "casadi/core/linsol.hpp"
#include <casadi/interfaces/hpmpc/casadi_conic_hpmpc_export.h>
 
#ifndef HPMPC_DLOPEN
#include <target.h>
extern "C" {
#include <c_interface.h>
}
#endif
 
namespace casadi {
 
  // Forward declaration
  class HpmpcInterface;
 
  struct CASADI_CONIC_HPMPC_EXPORT HpmpcMemory : public ConicMemory {
 
    std::vector<double> A, B, b, b2, Q, S, R, q, r, lb, ub, C, D, lg, ug;
    std::vector<double*> As, Bs, bs, Qs, Ss, Rs, qs, rs, lbs, ubs, Cs, Ds, lgs, ugs;
 
    std::vector<double> I;
    std::vector<double*> Is;
    std::vector<double> x, u, pi, lam;
    std::vector<double*> xs, us, pis, lams;
 
    std::vector<int> hidxb;
    std::vector<int*> hidxbs;
 
    std::vector<int> nx;
    std::vector<int> nu;
    std::vector<int> ng;
    std::vector<int> nb;
    std::vector<double> stats;
 
    std::vector<char> workspace;
 
    std::vector<double> pv;
 
    int iter_count;
    int return_status;
    std::vector<double> res;
 
    HpmpcMemory();
 
    ~HpmpcMemory();
  };
 
  class CASADI_CONIC_HPMPC_EXPORT HpmpcInterface : public Conic {
  public:
    explicit HpmpcInterface();
 
    static Conic* creator(const std::string& name,
                          const std::map<std::string, Sparsity>& st) {
      return new HpmpcInterface(name, st);
    }
 
    explicit HpmpcInterface(const std::string& name,
                              const std::map<std::string, Sparsity>& st);
 
    Dict get_stats(void* mem) const override;
 
    ~HpmpcInterface() override;
 
    // Get name of the plugin
    const char* plugin_name() const override { return "hpmpc";}
 
    // Get name of the class
    std::string class_name() const override { return "HpmpcInterface";}
 
 
    static const Options options_;
    const Options& get_options() const override { return options_;}
 
    void init(const Dict& opts) override;
 
    void* alloc_mem() const override { return new HpmpcMemory();}
 
    int init_mem(void* mem) const override;
 
    void free_mem(void *mem) const override { delete static_cast<HpmpcMemory*>(mem);}
 
    int solve(const double** arg, double** res,
      casadi_int* iw, double* w, void* mem) const override;
 
    static void mproject(double factor, const double* x, const casadi_int* sp_x,
                         double* y, const casadi_int* sp_y, double* w);
 
    static void dense_transfer(double factor, const double* x, const casadi_int* sp_x, double* y,
                               const casadi_int* sp_y, double* w);
 
    static const std::string meta_doc;
 
#ifdef HPMPC_DLOPEN
    typedef int (*Work_size)(int N, int *nx, int *nu, int *nb, int **hidxb, int *ng, int N2);
    typedef int (*Ocp_solve)(int *kk, int k_max, double mu0, double mu_tol,
      int N, int *nx, int *nu, int *nb, int **hidxb, int *ng, int N2, int warm_start,
      double **A, double **B, double **b, double **Q, double **S, double **R, double **q,
      double **r, double **lb, double **ub, double **C, double **D, double **lg, double **ug,
      double **x, double **u, double **pi, double **lam, /*double **t,*/
      double *inf_norm_res, void *work0, double *stat);
#endif
 
  protected:
 
    struct Block {
      casadi_int offset_r;
      casadi_int offset_c;
      casadi_int rows;
      casadi_int cols;
    };
 
    static Sparsity blocksparsity(casadi_int rows, casadi_int cols,
                                   const std::vector<Block>& blocks, bool eye=false);
    static void blockptr(std::vector<double *>& vs, std::vector<double>& v,
      const std::vector<Block>& blocks, bool eye=false);
    Sparsity Asp_, Bsp_, Csp_, Dsp_, Isp_, Rsp_, Ssp_, Qsp_, bsp_, lugsp_, usp_, xsp_;
    Sparsity theirs_xsp_, theirs_usp_, theirs_Xsp_, theirs_Usp_;
    Sparsity lamg_gapsp_, lamg_csp_, lam_ulsp_, lam_uusp_, lam_xlsp_, lam_xusp_, lam_clsp_;
    Sparsity lam_cusp_, pisp_;
 
    std::vector< Block > R_blocks, S_blocks, Q_blocks;
    std::vector< Block > b_blocks, lug_blocks;
    std::vector< Block > u_blocks, x_blocks;
    std::vector< Block > lam_ul_blocks, lam_xl_blocks, lam_uu_blocks, lam_xu_blocks, lam_cl_blocks;
    std::vector< Block > lam_cu_blocks, A_blocks, B_blocks, C_blocks, D_blocks, I_blocks;
 
    std::vector<casadi_int> nxs_;
    std::vector<casadi_int> nus_;
    std::vector<casadi_int> ngs_;
    casadi_int N_;
    casadi_int print_level_;
 
    bool warm_start_;
    double inf_;
 
    double mu0_; // max element in cost function as estimate of max multiplier
    casadi_int max_iter_; // maximum number of iterations
    double tol_; // tolerance in the duality measure
 
    std::string blasfeo_target_;
    std::string target_;
 
#ifdef HPMPC_DLOPEN
    Work_size hpmpc_d_ip_ocp_hard_tv_work_space_size_bytes;
    Ocp_solve fortran_order_d_ip_ocp_hard_tv;
#endif
 
 
  };
 
} // namespace casadi
 
#endif // CASADI_HPMPC_INTERFACE_HPP