ipopt-iface.hpp

// BSD 3-Clause License
//
// Copyright (C) 2022-2025, 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 {

template <typename _Scalar>
class IpoptInterfaceTpl : public Ipopt::TNLP {
 public:
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 typedef _Scalar Scalar;
 typedef ShootingProblemTpl<Scalar> ShootingProblem;
 typedef typename ShootingProblem::ActionModelAbstract ActionModelAbstract;
 typedef typename ShootingProblem::ActionDataAbstract ActionDataAbstract;
 typedef IpoptInterfaceDataTpl<Scalar> IpoptInterfaceData;
 typedef MathBaseTpl<Scalar> MathBase;
 typedef typename MathBase::VectorXs VectorXs;

 IpoptInterfaceTpl(const std::shared_ptr<ShootingProblem>& problem);

 virtual ~IpoptInterfaceTpl() = default;

 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<VectorXs>& get_xs() const;

 const std::vector<VectorXs>& get_us() const;

 const std::shared_ptr<ShootingProblem>& get_problem() const;

 Scalar get_cost() const;

 void set_xs(const std::vector<VectorXs>& xs);

 void set_us(const std::vector<VectorXs>& us);

 private:
 std::shared_ptr<ShootingProblem> problem_;
 std::vector<VectorXs> xs_;
 std::vector<VectorXs> us_;
 std::vector<std::size_t> ixu_;
 std::size_t nvar_;
 std::size_t nconst_;
 std::vector<std::shared_ptr<IpoptInterfaceData>> datas_;
 Scalar cost_;

 IpoptInterfaceTpl(const IpoptInterfaceTpl&);

 IpoptInterfaceTpl& operator=(const IpoptInterfaceTpl&);
};

template <typename _Scalar>
struct IpoptInterfaceDataTpl {
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 typedef _Scalar Scalar;
 typedef MathBaseTpl<Scalar> MathBase;
 typedef typename MathBase::VectorXs VectorXs;
 typedef typename MathBase::MatrixXs MatrixXs;

 IpoptInterfaceDataTpl(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);
 }

 VectorXs x;
 VectorXs xnext;
 VectorXs dx;
 VectorXs dxnext;
 VectorXs x_diff;
 VectorXs u;
 MatrixXs Jint_dx;
 MatrixXs
 Jint_dxnext;
 MatrixXs Jdiff_x;
 MatrixXs Jdiff_xnext;
 MatrixXs Jg_dx;
 MatrixXs Jg_dxnext;
 MatrixXs Jg_u;
 MatrixXs Jg_ic;
 MatrixXs FxJint_dx;
 VectorXs Ldx;
 MatrixXs Ldxdx;
 MatrixXs Ldxu;
};

} // namespace crocoddyl

/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "crocoddyl/core/solvers/ipopt/ipopt-iface.hxx"

#endif