odyn-sqp.hpp

// BSD 3-Clause License
//
// Copyright (C) 2025-2026, Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.

#ifndef CROCODDYL_CORE_SOLVERS_ODYN_SQP_HPP_
#define CROCODDYL_CORE_SOLVERS_ODYN_SQP_HPP_

#include <odyn/data.hpp>
#include <odyn/model.hpp>
#include <odyn/params.hpp>
#include <odyn/solver.hpp>

#include "crocoddyl/core/solver-base.hpp"
#include "crocoddyl/core/utils/deprecate.hpp"

namespace crocoddyl {

template <typename _Scalar>
class SolverOdynSQPTpl : public SolverAbstractTpl<_Scalar> {
 public:
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 CROCODDYL_DERIVED_FLOATINGPOINT_CAST(SolverBase, SolverOdynSQPTpl)

 typedef _Scalar Scalar;
 typedef SolverAbstractTpl<Scalar> SolverAbstract;
 typedef ShootingProblemTpl<Scalar> ShootingProblem;
 typedef typename ShootingProblem::ActionModelAbstract ActionModelAbstract;
 typedef typename ShootingProblem::ActionDataAbstract ActionDataAbstract;
 typedef CallbackAbstractTpl<Scalar> CallbackAbstract;
 typedef MathBaseTpl<Scalar> MathBase;
 typedef typename MathBase::VectorXs VectorXs;
 typedef typename MathBase::Vector3s Vector3s;
 typedef typename MathBase::MatrixXs MatrixXs;
 typedef typename MathBase::MatrixXsRowMajor MatrixXsRowMajor;
 using Solver = typename odyn::SparseQPTpl<Scalar>;
 using Model = typename odyn::SparseModelTpl<Scalar>;
 using Data = typename odyn::SparseDataTpl<Scalar>;
 using Params = typename odyn::ParamsTpl<Scalar>;
 using SolverAbstract::computeDynamicFeasibility;
 using SolverAbstract::computeEqualityFeasibility;
 using SolverAbstract::computeFeasibility;
 using SolverAbstract::computeInequalityFeasibility;
 using SolverAbstract::resizeData;

 explicit SolverOdynSQPTpl(std::shared_ptr<ShootingProblem> problem);
 virtual ~SolverOdynSQPTpl() = default;

 virtual void computeDirection(const bool recalc = true) override;

 virtual Scalar stoppingCriteria() override;

 virtual Vector3s expectedImprovement() override;

 virtual void computeMeritFunctionImprovement() override;

 virtual void computeExpectedMeritFunctionImprovement() override;

 virtual void updateMeritFunction() override;

 virtual bool checkAcceptance() override;

 virtual void computeCandidate(const Scalar step_length = Scalar(1.)) override;

 void computeQuadraticModel();

 void updateCandidate() override;

 bool decreaseRegularizationCriteria() override;

 bool increaseRegularizationCriteria() override;

 void increaseRegularization() override;

 void decreaseRegularization() override;

 void extractQpDirection(const VectorXs& x);

 template <typename NewScalar>
 SolverOdynSQPTpl<NewScalar> cast() const;

 Model& qp_model() noexcept;

 Data& qp_data() noexcept;

 std::size_t get_n() const noexcept;

 std::size_t get_m() const noexcept;

 std::size_t get_p() const noexcept;

 Params& qp_params() noexcept;

 odyn::VerboseLevel get_verbose_level() const noexcept;

 Scalar get_reg_incfactor() const;

 Scalar get_reg_decfactor() const;

 Scalar get_th_grad() const;

 Scalar get_th_stepdec() const;

 Scalar get_th_stepinc() const;

 Scalar get_th_minimprove() const;

 Scalar get_th_acceptnegstep() const;

 Scalar get_th_acceptminstep() const;

 Scalar get_rho() const;

 Scalar get_th_minfeas() const;

 Scalar get_upsilon() const;

 Scalar get_upsilon_decfactor() const;

 bool get_zero_upsilon() const;

 void set_qp_params(const Params& params);

 void set_verbose_level(const odyn::VerboseLevel verbose_level);

 void set_reg_incfactor(const Scalar reg_factor);

 void set_reg_decfactor(const Scalar reg_factor);

 void set_th_grad(const Scalar th_grad);

 void set_th_noimprovement(const Scalar th_noimprovement);

 void set_th_stepdec(const Scalar th_step);

 void set_th_stepinc(const Scalar th_step);

 void set_th_minimprove(const Scalar th_step);

 void set_th_acceptnegstep(const Scalar th_acceptnegstep);

 void set_th_acceptminstep(const Scalar th_acceptminstep);

 void set_rho(const Scalar rho);

 void set_th_minfeas(const Scalar th_minfeas);

 void set_upsilon_decfactor(const Scalar th_step);

 void set_zero_upsilon(const bool zero_upsilon);

 protected:
 void allocateData();

 virtual void resizeRunningData() override;

 virtual void resizeTerminalData() override;

 void updateStateAndControlIndex();

 Scalar reg_incfactor_;
 Scalar reg_decfactor_;
 Scalar th_grad_;
 Scalar th_noimprovement_;
 Scalar
 th_stepdec_;
 Scalar
 th_stepinc_;
 Scalar th_minimprove_;
 Scalar th_acceptnegstep_;
 Scalar th_acceptminstep_;
 Scalar rho_;
 Scalar th_minfeas_;
 Scalar
 upsilon_;
 Scalar upsilon_decfactor_;
 bool zero_upsilon_;

 std::size_t n_;
 std::size_t m_;
 std::size_t p_;
 VectorXs x_;
 Solver qp_solver_;
 Model qp_model_;
 Data qp_data_;
 Params qp_params_;
 odyn::VerboseLevel verbose_level_;
 std::vector<std::size_t> xs_idx_;
 std::vector<std::size_t> us_idx_;
 std::vector<VectorXs>
 Lxx_dx_;
 std::vector<VectorXs>
 Luu_du_;
 std::vector<VectorXs>
 Lxu_du_;

 using SolverAbstract::acceptstep_;
 using SolverAbstract::alphas_;
 using SolverAbstract::callbacks_;
 using SolverAbstract::cost_;
 using SolverAbstract::cost_try_;
 using SolverAbstract::dfeas_;
 using SolverAbstract::dImpr_;
 using SolverAbstract::dPhi_;
 using SolverAbstract::dPhiexp_;
 using SolverAbstract::dreg_;
 using SolverAbstract::dus_;
 using SolverAbstract::DV_;
 using SolverAbstract::dV_;
 using SolverAbstract::dVexp_;
 using SolverAbstract::dVexp_full_;
 using SolverAbstract::dxs_;
 using SolverAbstract::feas_;
 using SolverAbstract::feasnorm_;
 using SolverAbstract::ffeas_;
 using SolverAbstract::ffeas_try_;
 using SolverAbstract::fs_;
 using SolverAbstract::fs_try_;
 using SolverAbstract::gfeas_;
 using SolverAbstract::gfeas_try_;
 using SolverAbstract::hfeas_;
 using SolverAbstract::hfeas_try_;
 using SolverAbstract::is_feasible_;
 using SolverAbstract::iter_;
 using SolverAbstract::merit_;
 using SolverAbstract::ng_T_;
 using SolverAbstract::nh_T_;
 using SolverAbstract::preg_;
 using SolverAbstract::problem_;
 using SolverAbstract::reg_max_;
 using SolverAbstract::reg_min_;
 using SolverAbstract::steplength_;
 using SolverAbstract::stop_;
 using SolverAbstract::th_acceptstep_;
 using SolverAbstract::th_gaptol_;
 using SolverAbstract::th_stop_;
 using SolverAbstract::us_;
 using SolverAbstract::us_try_;
 using SolverAbstract::xs_;
 using SolverAbstract::xs_try_;
};

} // namespace crocoddyl

/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "crocoddyl/core/solvers/odyn-sqp.hxx"

CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::SolverOdynSQPTpl)

#endif // CROCODDYL_CORE_SOLVERS_ODYN_SQP_HPP_