integrator_impl.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_INTEGRATOR_IMPL_HPP
#define CASADI_INTEGRATOR_IMPL_HPP
 
#include "integrator.hpp"
#include "oracle_function.hpp"
#include "plugin_interface.hpp"
#include "casadi_enum.hpp"
 
 
namespace casadi {
 
struct CASADI_EXPORT IntegratorMemory : public OracleMemory {
  // Work vectors, forward problem
  double *q, *x, *z, *p, *u, *e, *edot, *old_e, *xdot, *zdot;
  // Work vectors, backward problem
  double *adj_x, *adj_z, *adj_p, *adj_q;
  // Temporary work vectors of length max(nx + nz, nrx, nrz)
  double *tmp1, *tmp2;
  // Current control interval
  casadi_int k;
  // Current time
  double t;
  // Next time to be visited by the integrator
  double t_next;
  // Time not to be exceeded by during integrator integration
  double t_stop;
  // Time at the beginning of the current control interval
  double t_start;
  // Next output time
  double t_next_out;
  // Next stop time due to step change in input
  double t_step;
  // Which events have been triggered
  casadi_int *event_triggered;
  // Do we need to reset the solver?
  bool reset_solver;
  // Number of root-finding iterations for a single event
  casadi_int event_iter;
  // Number of events encountered thus far
  casadi_int num_events;
  // Index of event last triggered
  casadi_int event_index;
};
 
struct CASADI_EXPORT SpForwardMem {
  const bvec_t** arg;
  bvec_t** res;
  casadi_int* iw;
  bvec_t* w;
};
 
struct CASADI_EXPORT SpReverseMem {
  bvec_t** arg;
  bvec_t** res;
  casadi_int* iw;
  bvec_t* w;
};
 
class CASADI_EXPORT
Integrator : public OracleFunction, public PluginInterface<Integrator> {
 public:
  Integrator(const std::string& name, const Function& oracle,
    double t0, const std::vector<double>& tout);
 
  ~Integrator() override=0;
 
 
  size_t get_n_in() override { return INTEGRATOR_NUM_IN;}
  size_t get_n_out() override { return INTEGRATOR_NUM_OUT;}
 
 
  Sparsity get_sparsity_in(casadi_int i) override;
  Sparsity get_sparsity_out(casadi_int i) override;
 
 
  std::string get_name_in(casadi_int i) override { return integrator_in(i);}
  std::string get_name_out(casadi_int i) override { return integrator_out(i);}
 
  int init_mem(void* mem) const override;
 
 
  static const Options options_;
  const Options& get_options() const override { return options_;}
 
  void init(const Dict& opts) override;
 
  void set_work(void* mem, const double**& arg, double**& res,
    casadi_int*& iw, double*& w) const override;
 
  virtual Function create_advanced(const Dict& opts);
 
  virtual MX algebraic_state_init(const MX& x0, const MX& z0) const { return z0; }
  virtual MX algebraic_state_output(const MX& Z) const { return Z; }
 
  // Set the quadrature states
  void set_q(IntegratorMemory* m, const double* q) const;
 
  // Set the differential states
  void set_x(IntegratorMemory* m, const double* x) const;
 
  // Set the algebraic variables
  void set_z(IntegratorMemory* m, const double* z) const;
 
  // Set the parameters
  void set_p(IntegratorMemory* m, const double* p) const;
 
  // Set the controls
  void set_u(IntegratorMemory* m, const double* u) const;
 
  // Get the quadrature states
  void get_q(IntegratorMemory* m, double* q) const;
 
  // Get the differential states
  void get_x(IntegratorMemory* m, double* x) const;
 
  // Get the algebraic variables
  void get_z(IntegratorMemory* m, double* z) const;
 
  virtual void reset(IntegratorMemory* mem, bool first_call) const {}
 
  casadi_int next_stop(casadi_int k, const double* u) const;
 
  int calc_edot(IntegratorMemory* m) const;
 
  int predict_events(IntegratorMemory* m) const;
 
  int trigger_event(IntegratorMemory* m, casadi_int* ind) const;
 
  int advance(IntegratorMemory* m) const;
 
  virtual int advance_noevent(IntegratorMemory* mem) const = 0;
 
  virtual void resetB(IntegratorMemory* mem) const = 0;
 
  casadi_int next_stopB(casadi_int k, const double* u) const;
 
  virtual void impulseB(IntegratorMemory* mem,
    const double* adj_x, const double* adj_z, const double* adj_q) const = 0;
 
  virtual void retreat(IntegratorMemory* mem, const double* u,
    double* adj_x, double* adj_p, double* adj_u) const = 0;
 
  int eval(const double** arg, double** res, casadi_int* iw, double* w, void* mem) const override;
 
  virtual void print_stats(IntegratorMemory* mem) const {}
 
  int fdae_sp_forward(SpForwardMem* m, const bvec_t* x,
    const bvec_t* p, const bvec_t* u, bvec_t* ode, bvec_t* alg) const;
 
  int fquad_sp_forward(SpForwardMem* m, const bvec_t* x, const bvec_t* z,
    const bvec_t* p, const bvec_t* u, bvec_t* quad) const;
 
  int bdae_sp_forward(SpForwardMem* m, const bvec_t* x, const bvec_t* z,
    const bvec_t* p, const bvec_t* u, const bvec_t* adj_ode, const bvec_t* adj_quad,
    bvec_t* adj_x, bvec_t* adj_z) const;
 
  int bquad_sp_forward(SpForwardMem* m, const bvec_t* x, const bvec_t* z,
    const bvec_t* p, const bvec_t* u, const bvec_t* adj_ode, const bvec_t* adj_alg,
    const bvec_t* adj_quad, bvec_t* adj_p, bvec_t* adj_u) const;
 
  int sp_forward(const bvec_t** arg, bvec_t** res,
    casadi_int* iw, bvec_t* w, void* mem) const override;
 
  int fdae_sp_reverse(SpReverseMem* m, bvec_t* x,
    bvec_t* p, bvec_t* u, bvec_t* ode, bvec_t* alg) const;
 
  int fquad_sp_reverse(SpReverseMem* m, bvec_t* x, bvec_t* z,
    bvec_t* p, bvec_t* u, bvec_t* quad) const;
 
  int bdae_sp_reverse(SpReverseMem* m, bvec_t* x, bvec_t* z,
    bvec_t* p, bvec_t* u, bvec_t* adj_ode, bvec_t* adj_quad,
    bvec_t* adj_x, bvec_t* adj_z) const;
 
  int bquad_sp_reverse(SpReverseMem* m, bvec_t* x, bvec_t* z,
    bvec_t* p, bvec_t* u, bvec_t* adj_ode, bvec_t* adj_alg, bvec_t* adj_quad,
    bvec_t* adj_p, bvec_t* adj_u) const;
 
  int sp_reverse(bvec_t** arg, bvec_t** res, casadi_int* iw, bvec_t* w, void* mem) const override;
 
  bool has_spfwd() const override { return true;}
  bool has_sprev() const override { return true;}
 
 
  Function get_forward(casadi_int nfwd, const std::string& name,
                        const std::vector<std::string>& inames,
                        const std::vector<std::string>& onames,
                        const Dict& opts) const override;
  bool has_forward(casadi_int nfwd) const override { return true;}
 
 
  Function get_reverse(casadi_int nadj, const std::string& name,
                        const std::vector<std::string>& inames,
                        const std::vector<std::string>& onames,
                        const Dict& opts) const override;
  bool has_reverse(casadi_int nadj) const override { return ne_ == 0;}
 
  virtual Dict getDerivativeOptions(bool fwd) const;
 
 
  template<typename MatType> Function get_forward_dae(const std::string& name) const;
  Function augmented_dae() const;
 
  static bool all_zero(const double* v, casadi_int n);
 
  Sparsity sp_jac_aug(const Sparsity& J, const Sparsity& J1) const;
 
  Sparsity sp_jac_dae();
 
  Sparsity sp_jac_rdae();
 
  Sparsity sp_jac_dae_, sp_jac_rdae_;
 
  inline casadi_int nt() const { return tout_.size();}
 
 
  enum DaeOut { DAE_ODE, DAE_ALG, DAE_NUM_OUT};
  static std::vector<std::string> dae_out() { return {"ode", "alg"}; }
  enum QuadOut { QUAD_QUAD, QUAD_NUM_OUT};
  static std::vector<std::string> quad_out() { return {"quad"}; }
  enum BDynIn { BDYN_T, BDYN_X, BDYN_Z, BDYN_P, BDYN_U,
    BDYN_OUT_ODE, BDYN_OUT_ALG, BDYN_OUT_QUAD, BDYN_OUT_ZERO,
    BDYN_ADJ_ODE, BDYN_ADJ_ALG, BDYN_ADJ_QUAD, BDYN_ADJ_ZERO, BDYN_NUM_IN};
  static std::string bdyn_in(casadi_int i);
  static std::vector<std::string> bdyn_in();
  enum BDynOut { BDYN_ADJ_T, BDYN_ADJ_X, BDYN_ADJ_Z, BDYN_ADJ_P, BDYN_ADJ_U, BDYN_NUM_OUT};
  static std::string bdyn_out(casadi_int i);
  static std::vector<std::string> bdyn_out();
  enum DAEBOut { BDAE_ADJ_X, BDAE_ADJ_Z, BDAE_NUM_OUT};
  static std::vector<std::string> bdae_out() { return {"adj_x", "adj_z"}; }
  enum QuadBOut { BQUAD_ADJ_P, BQUAD_ADJ_U, BQUAD_NUM_OUT};
  static std::vector<std::string> bquad_out() { return {"adj_p", "adj_u"}; }
 
  double t0_;
 
  std::vector<double> tout_;
 
  casadi_int nfwd_, nadj_;
 
  Function rdae_;
 
  casadi_int nx_, nz_, nq_, nx1_, nz1_, nq1_;
 
  casadi_int nrx_, nrz_, nrq_, nuq_, nrx1_, nrz1_, nrq1_, nuq1_;
 
  casadi_int np_, nrp_, np1_, nrp1_;
 
  casadi_int nu_, nu1_;
 
  casadi_int ne_;
 
  casadi_int ntmp_;
 
  // Nominal values for states
  std::vector<double> nom_x_, nom_z_;
 
  Dict augmented_options_;
 
  Dict opts_;
 
  bool print_stats_;
 
  Function transition_;
 
  casadi_int max_event_iter_;
 
  casadi_int max_events_;
 
  double event_tol_;
 
  double event_acceptable_tol_;
 
  // Creator function for internal class
  typedef Integrator* (*Creator)(const std::string& name, const Function& oracle,
    double t0, const std::vector<double>& tout);
 
  // No static functions exposed
  struct Exposed{ };
 
  static std::map<std::string, Plugin> solvers_;
 
#ifdef CASADI_WITH_THREADSAFE_SYMBOLICS
    static std::mutex mutex_solvers_;
#endif // CASADI_WITH_THREADSAFE_SYMBOLICS
 
  static const std::string infix_;
 
  template<typename XType>
  static Function map2oracle(const std::string& name, const std::map<std::string, XType>& d);
 
  void serialize_body(SerializingStream &s) const override;
  void serialize_type(SerializingStream &s) const override;
 
  static ProtoFunction* deserialize(DeserializingStream& s);
 
  std::string serialize_base_function() const override { return "Integrator"; }
 
  static bool grid_in(casadi_int i);
 
  static bool grid_out(casadi_int i);
 
  static casadi_int adjmap_out(casadi_int i);
 
 protected:
  explicit Integrator(DeserializingStream& s);
};
 
enum StepIn {
  STEP_T,
  STEP_H,
  STEP_X0,
  STEP_V0,
  STEP_P,
  STEP_U,
  STEP_NUM_IN
};
 
enum StepOut {
  STEP_XF,
  STEP_VF,
  STEP_QF,
  STEP_NUM_OUT
};
 
enum BStepIn {
  BSTEP_T,
  BSTEP_H,
  BSTEP_X0,
  BSTEP_V0,
  BSTEP_P,
  BSTEP_U,
  BSTEP_OUT_XF,
  BSTEP_OUT_VF,
  BSTEP_OUT_QF,
  BSTEP_ADJ_XF,
  BSTEP_ADJ_VF,
  BSTEP_ADJ_QF,
  BSTEP_NUM_IN
};
 
enum BStepOut {
  BSTEP_ADJ_T,
  BSTEP_ADJ_H,
  BSTEP_ADJ_X0,
  BSTEP_ADJ_V0,
  BSTEP_ADJ_P,
  BSTEP_ADJ_U,
  BSTEP_NUM_OUT
};
 
struct CASADI_EXPORT FixedStepMemory : public IntegratorMemory {
  double *v, *v_prev, *q_prev;
 
  double *rv, *adj_u, *adj_p_prev, *adj_u_prev;
 
  double *x_tape, *v_tape;
};
 
class CASADI_EXPORT FixedStepIntegrator : public Integrator {
 public:
 
  explicit FixedStepIntegrator(const std::string& name, const Function& dae,
    double t0, const std::vector<double>& tout);
 
  ~FixedStepIntegrator() override;
 
 
  static const Options options_;
  const Options& get_options() const override { return options_;}
 
  void init(const Dict& opts) override;
 
  void set_work(void* mem, const double**& arg, double**& res,
    casadi_int*& iw, double*& w) const override;
 
  Function create_advanced(const Dict& opts) override;
 
  void* alloc_mem() const override { return new FixedStepMemory();}
 
  int init_mem(void* mem) const override;
 
  void free_mem(void *mem) const override { delete static_cast<FixedStepMemory*>(mem);}
 
  virtual void setup_step() = 0;
 
  void reset(IntegratorMemory* mem, bool first_call) const override;
 
  int advance_noevent(IntegratorMemory* mem) const override;
 
  void resetB(IntegratorMemory* mem) const override;
 
  void impulseB(IntegratorMemory* mem,
    const double* adj_x, const double* adj_z, const double* adj_q) const override;
 
  void retreat(IntegratorMemory* mem, const double* u,
    double* adj_x, double* adj_p, double* adj_u) const override;
 
  void stepF(FixedStepMemory* m, double t, double h,
    const double* x0, const double* v0, double* xf, double* vf, double* qf) const;
 
  void stepB(FixedStepMemory* m, double t, double h,
    const double* x0, const double* xf, const double* vf,
    const double* adj_xf, const double* rv0,
    double* adj_x0, double* adj_p, double* adj_u) const;
 
  // Target number of finite elements
  casadi_int nk_target_;
 
  // Number of steps per control interval
  std::vector<casadi_int> disc_;
 
  casadi_int nv_, nv1_, nrv_, nrv1_;
 
  void serialize_body(SerializingStream &s) const override;
 
protected:
  explicit FixedStepIntegrator(DeserializingStream& s);
};
 
class CASADI_EXPORT ImplicitFixedStepIntegrator : public FixedStepIntegrator {
 public:
 
  explicit ImplicitFixedStepIntegrator(const std::string& name, const Function& dae,
    double t0, const std::vector<double>& tout);
 
  ~ImplicitFixedStepIntegrator() override;
 
 
  static const Options options_;
  const Options& get_options() const override { return options_;}
 
  void init(const Dict& opts) override;
 
  void serialize_body(SerializingStream &s) const override;
 
protected:
  explicit ImplicitFixedStepIntegrator(DeserializingStream& s);
};
 
} // namespace casadi
 
#endif // CASADI_INTEGRATOR_IMPL_HPP