ipopt-iface.hpp

// BSD 3-Clause License
//
// Copyright (C) 2022-2023, IRI: CSIC-UPC, Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
 
#ifndef __CROCODDYL_CORE_SOLVERS_IPOPT_IPOPT_IFACE_HPP__
#define __CROCODDYL_CORE_SOLVERS_IPOPT_IPOPT_IFACE_HPP__
 
#define HAVE_CSTDDEF
#include <IpTNLP.hpp>
#undef HAVE_CSTDDEF
 
#include "crocoddyl/core/optctrl/shooting.hpp"
 
namespace crocoddyl {
 
struct IpoptInterfaceData;
 
class IpoptInterface : public Ipopt::TNLP {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  IpoptInterface(const std::shared_ptr<crocoddyl::ShootingProblem>& problem);
 
  virtual ~IpoptInterface();
 
  virtual bool get_nlp_info(Ipopt::Index& n, Ipopt::Index& m,
                            Ipopt::Index& nnz_jac_g, Ipopt::Index& nnz_h_lag,
                            IndexStyleEnum& index_style);
 
  virtual bool get_bounds_info(Ipopt::Index n, Ipopt::Number* x_l,
                               Ipopt::Number* x_u, Ipopt::Index m,
                               Ipopt::Number* g_l, Ipopt::Number* g_u);
 
  // [TNLP_get_starting_point]
  virtual bool get_starting_point(Ipopt::Index n, bool init_x, Ipopt::Number* x,
                                  bool init_z, Ipopt::Number* z_L,
                                  Ipopt::Number* z_U, Ipopt::Index m,
                                  bool init_lambda, Ipopt::Number* lambda);
 
  virtual bool eval_f(Ipopt::Index n, const Ipopt::Number* x, bool new_x,
                      Ipopt::Number& obj_value);
 
  virtual bool eval_grad_f(Ipopt::Index n, const Ipopt::Number* x, bool new_x,
                           Ipopt::Number* grad_f);
 
  virtual bool eval_g(Ipopt::Index n, const Ipopt::Number* x, bool new_x,
                      Ipopt::Index m, Ipopt::Number* g);
 
  virtual bool eval_jac_g(Ipopt::Index n, const Ipopt::Number* x, bool new_x,
                          Ipopt::Index m, Ipopt::Index nele_jac,
                          Ipopt::Index* iRow, Ipopt::Index* jCol,
                          Ipopt::Number* values);
 
  virtual bool eval_h(Ipopt::Index n, const Ipopt::Number* x, bool new_x,
                      Ipopt::Number obj_factor, Ipopt::Index m,
                      const Ipopt::Number* lambda, bool new_lambda,
                      Ipopt::Index nele_hess, Ipopt::Index* iRow,
                      Ipopt::Index* jCol, Ipopt::Number* values);
 
  virtual void finalize_solution(
      Ipopt::SolverReturn status, Ipopt::Index n, const Ipopt::Number* x,
      const Ipopt::Number* z_L, const Ipopt::Number* z_U, Ipopt::Index m,
      const Ipopt::Number* g, const Ipopt::Number* lambda,
      Ipopt::Number obj_value, const Ipopt::IpoptData* ip_data,
      Ipopt::IpoptCalculatedQuantities* ip_cq);
 
  bool intermediate_callback(
      Ipopt::AlgorithmMode mode, Ipopt::Index iter, Ipopt::Number obj_value,
      Ipopt::Number inf_pr, Ipopt::Number inf_du, Ipopt::Number mu,
      Ipopt::Number d_norm, Ipopt::Number regularization_size,
      Ipopt::Number alpha_du, Ipopt::Number alpha_pr, Ipopt::Index ls_trials,
      const Ipopt::IpoptData* ip_data, Ipopt::IpoptCalculatedQuantities* ip_cq);
 
  std::shared_ptr<IpoptInterfaceData> createData(const std::size_t nx,
                                                 const std::size_t ndx,
                                                 const std::size_t nu);
 
  void resizeData();
 
  std::size_t get_nvar() const;
 
  std::size_t get_nconst() const;
 
  const std::vector<Eigen::VectorXd>& get_xs() const;
 
  const std::vector<Eigen::VectorXd>& get_us() const;
 
  const std::shared_ptr<crocoddyl::ShootingProblem>& get_problem() const;
 
  double get_cost() const;
 
  void set_xs(const std::vector<Eigen::VectorXd>& xs);
 
  void set_us(const std::vector<Eigen::VectorXd>& us);
 
 private:
  std::shared_ptr<crocoddyl::ShootingProblem>
      problem_;                      
  std::vector<Eigen::VectorXd> xs_;  
  std::vector<Eigen::VectorXd> us_;  
  std::vector<std::size_t> ixu_;     
  std::size_t nvar_;                 
  std::size_t nconst_;               
  std::vector<std::shared_ptr<IpoptInterfaceData>> datas_;  
  double cost_;                                             
 
  IpoptInterface(const IpoptInterface&);
 
  IpoptInterface& operator=(const IpoptInterface&);
};
 
struct IpoptInterfaceData {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  IpoptInterfaceData(const std::size_t nx, const std::size_t ndx,
                     const std::size_t nu)
      : x(nx),
        xnext(nx),
        dx(ndx),
        dxnext(ndx),
        x_diff(ndx),
        u(nu),
        Jint_dx(ndx, ndx),
        Jint_dxnext(ndx, ndx),
        Jdiff_x(ndx, ndx),
        Jdiff_xnext(ndx, ndx),
        Jg_dx(ndx, ndx),
        Jg_dxnext(ndx, ndx),
        Jg_u(ndx, ndx),
        Jg_ic(ndx, ndx),
        FxJint_dx(ndx, ndx),
        Ldx(ndx),
        Ldxdx(ndx, ndx),
        Ldxu(ndx, nu) {
    x.setZero();
    xnext.setZero();
    dx.setZero();
    dxnext.setZero();
    x_diff.setZero();
    u.setZero();
    Jint_dx.setZero();
    Jint_dxnext.setZero();
    Jdiff_x.setZero();
    Jdiff_xnext.setZero();
    Jg_dx.setZero();
    Jg_dxnext.setZero();
    Jg_u.setZero();
    Jg_ic.setZero();
    FxJint_dx.setZero();
    Ldx.setZero();
    Ldxdx.setZero();
    Ldxu.setZero();
  }
 
  void resize(const std::size_t nx, const std::size_t ndx,
              const std::size_t nu) {
    x.conservativeResize(nx);
    xnext.conservativeResize(nx);
    dx.conservativeResize(ndx);
    dxnext.conservativeResize(ndx);
    x_diff.conservativeResize(ndx);
    u.conservativeResize(nu);
    Jint_dx.conservativeResize(ndx, ndx);
    Jint_dxnext.conservativeResize(ndx, ndx);
    Jdiff_x.conservativeResize(ndx, ndx);
    Jdiff_xnext.conservativeResize(ndx, ndx);
    Jg_dx.conservativeResize(ndx, ndx);
    Jg_dxnext.conservativeResize(ndx, ndx);
    Jg_u.conservativeResize(ndx, ndx);
    Jg_ic.conservativeResize(ndx, ndx);
    FxJint_dx.conservativeResize(ndx, ndx);
    Ldx.conservativeResize(ndx);
    Ldxdx.conservativeResize(ndx, ndx);
    Ldxu.conservativeResize(ndx, nu);
  }
 
  Eigen::VectorXd x;        
  Eigen::VectorXd xnext;    
  Eigen::VectorXd dx;       
  Eigen::VectorXd dxnext;   
  Eigen::VectorXd x_diff;   
  Eigen::VectorXd u;        
  Eigen::MatrixXd Jint_dx;  
  Eigen::MatrixXd
      Jint_dxnext;  
  Eigen::MatrixXd
      Jdiff_x;  
  Eigen::MatrixXd Jdiff_xnext;  
  Eigen::MatrixXd Jg_dx;        
  Eigen::MatrixXd
      Jg_dxnext;         
  Eigen::MatrixXd Jg_u;  
  Eigen::MatrixXd
      Jg_ic;  
  Eigen::MatrixXd FxJint_dx;  
  Eigen::VectorXd Ldx;        
  Eigen::MatrixXd Ldxdx;      
  Eigen::MatrixXd Ldxu;       
};
 
}  // namespace crocoddyl
 
#endif