fatrop_test.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 <ocp/OCPAbstract.hpp>
#include <ocp/StageOCPApplication.hpp>
#include <casadi/core/casadi_misc.hpp>
 
namespace casadi {
 
class CasadiStructuredQP : public fatrop::OCPAbstract {
  fatrop_int get_nxk(const fatrop_int k) const override {
    std::vector<fatrop_int> res = {2,3,2,1};
    return res[k];
  }
  fatrop_int get_nuk(const fatrop_int k) const override {
    std::vector<fatrop_int> res = {1, 2,1};
    return res[k];
  };
  fatrop_int get_ngk(const fatrop_int k) const override {
    std::vector<fatrop_int> res = {2, 0,0,0};
    return res[k]; // 2 from lbx
 
  };
  fatrop_int get_n_stage_params_k(const fatrop_int k) const override { return 0;}
  fatrop_int get_n_global_params() const override { return 0;}
  fatrop_int get_default_stage_paramsk(double *stage_params, const fatrop_int k) const override { return 0;}
  fatrop_int get_default_global_params(double *global_params) const override{ return 0; }
  virtual fatrop_int get_ng_ineq_k(const fatrop_int k) const {
    std::vector<fatrop_int> res_lbg = {2, 1, 0, 0};
    std::vector<fatrop_int> res_lbx = {1, 5, 3, 1};
    return res_lbg[k]+res_lbx[k];
  }
  fatrop_int get_horizon_length() const override { return 4; }
  fatrop_int eval_BAbtk(
      const double *states_kp1,
      const double *inputs_k,
      const double *states_k,
      const double *stage_params_k,
      const double *global_params,
      MAT *res,
      const fatrop_int k) override {
        printf("eval_BAbtk k=%d\n", k);
        if (k==0) {
          std::vector<double> r = {1, 0.2, 1,   0,
                                      -0.1, 0.4,0 ,  0,
                                      0.3,  0.2,    0, 0};
          int out_m = 4; // rows
          int out_n = 3; // cols
          PACKMAT(out_m, out_n, get_ptr(r), out_m, res, 0, 0);
          blasfeo_print_dmat(out_m, out_n,  res, 0, 0);
 
          std::vector<double> r2 = {1, -0.1, 0.3, 0.2, 0.4, 0.2, 1, 0, 0, 0, 0, 0};
 
          blasfeo_pack_tran_dmat(out_n, out_m, get_ptr(r2), out_n, res, 0, 0);
 
          blasfeo_print_dmat(out_m, out_n,  res, 0, 0);
 
          //casadi_error("foo");
 
        } else if (k==1) {
          std::vector<double> r = {1,   4,  2,  1,  0.3, 0,
                                    3,  1,  00, 1,  0.2, 0};
          int out_m = 6; // rows
          int out_n = 2; // cols
          PACKMAT(out_m, out_n, get_ptr(r), out_m, res, 0, 0);
          blasfeo_print_dmat(out_m, out_n,  res, 0, 0);
 
 
          std::vector<double> r2 = {1,3,4,1,2,0,1,1,0.3,0.2,0,0,0,0,0};
 
          //blasfeo_pack_tran_dmat(out_n, out_m, get_ptr(r2), out_n, res, 0, 0);
 
          //blasfeo_print_dmat(out_m, out_n,  res, 0, 0);
 
        } else if (k==2) {
          std::vector<double> r = {2, 4, 0, 0};
          int out_m = 4; // rows
          int out_n = 1; // cols
          PACKMAT(out_m, out_n, get_ptr(r), out_m, res, 0, 0);
          blasfeo_print_dmat(out_m, out_n,  res, 0, 0);
        }
 
        
      };
  fatrop_int eval_RSQrqtk(
      const double *objective_scale,
      const double *inputs_k,
      const double *states_k,
      const double *lam_dyn_k,
      const double *lam_eq_k,
      const double *lam_eq_ineq_k,
      const double *stage_params_k,
      const double *global_params,
      MAT *res,
      const fatrop_int k) override {
 
      }
  fatrop_int eval_Ggtk(
      const double *inputs_k,
      const double *states_k,
      const double *stage_params_k,
      const double *global_params,
      MAT *res,
      const fatrop_int k) override {
    printf("eval_Ggtk k=%d\n", k);
    if (k==0) {
      std::vector<double> r = {2, 0, 0.3, 0,
                              1, 1, 0.4, 0};
      int out_m = 4; // rows
      int out_n = 2; // cols
      PACKMAT(out_m, out_n, get_ptr(r), out_m, res, 0, 0);
      blasfeo_print_dmat(out_m, out_n,  res, 0, 0);
    }
  }
  fatrop_int eval_Ggt_ineqk(
      const double *inputs_k,
      const double *states_k,
      const double *stage_params_k,
      const double *global_params,
      MAT *res,
      const fatrop_int k) {
    printf("eval_Ggt_ineqk k=%d\n", k);
  }
  fatrop_int eval_bk(
      const double *states_kp1,
      const double *inputs_k,
      const double *states_k,
      const double *stage_params_k,
      const double *global_params,
      double *res,
      const fatrop_int k) override {
  printf("eval_bk k=%d\n", k);
      }
  fatrop_int eval_gk(
      const double *states_k,
      const double *inputs_k,
      const double *stage_params_k,
      const double *global_params,
      double *res,
      const fatrop_int k) override {
    printf("eval_gk k=%d\n", k);
  }
  fatrop_int eval_gineqk(
      const double *states_k,
      const double *inputs_k,
      const double *stage_params_k,
      const double *global_params,
      double *res,
      const fatrop_int k)  override {
      printf("eval_gineqk k=%d\n", k);
  }
  fatrop_int eval_rqk(
      const double *objective_scale,
      const double *inputs_k,
      const double *states_k,
      const double *stage_params_k,
      const double *global_params,
      double *res,
      const fatrop_int k) override {
      printf("eval_rqk k=%d\n", k);
  }
  fatrop_int eval_Lk(
      const double *objective_scale,
      const double *inputs_k,
      const double *states_k,
      const double *stage_params_k,
      const double *global_params,
      double *res,
      const fatrop_int k) override {
      printf("eval_Lk k=%d\n", k);
  }
  fatrop_int get_boundsk(double *lower, double *upper, const fatrop_int k) const override {
      printf("get_boundsk k=%d\n", k);
  }
  fatrop_int get_initial_xk(double *xk, const fatrop_int k) const override {
    printf("get_initial_xk k=%d\n", k);
  }
  fatrop_int get_initial_uk(double *uk, const fatrop_int k) const override {
    printf("get_initial_uk k=%d\n", k);
  }
 
};
 
} // namespace casadi
 
int main() {
 
  int m = 5;
  int n = 7;
 
 
  size_t s = blasfeo_memsize_dmat(m, n);
  std::vector<double> mem(s, 0);
 
  blasfeo_dmat res;
  blasfeo_create_dmat(m, n, &res, &mem.front());
 
  blasfeo_print_dmat(m, n, &res, 0, 0);
 
 
  std::vector<double> a(100);
  for (int i=0;i<100;++i) a[i] = i+1;
 
 
  //blasfeo_pack_dmat(2, 3, &a.front(), 10, &res, 1, 3);
 
  //blasfeo_pack_tran_dmat(2, 3, &a.front(), 10, &res, 1, 3);
  blasfeo_pack_tran_dmat(1, 3, &a.front(), 2, &res, 1, 3);
 
  blasfeo_print_dmat(m, n, &res, 0, 0);
  return 0;
 
  /*blasfeo_pack_dmat(int m, int n, double *A, int lda, struct blasfeo_dmat *sB, int bi, int bj);
 
  blasfeo_pack_tran_dmat(1, n, d->CD+p->CD_offsets[k]+(i-start), ng_ineq, res, 0, column++);
 
  blasfeo_print_dmat(m, n,  res, 0, 0);*/
 
 
 
  casadi::CasadiStructuredQP qp;
 
  fatrop::OCPApplication app(std::make_shared<casadi::CasadiStructuredQP>(qp));
  app.build();
 
  app.optimize();
 
}