solver-base.hpp

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

#ifndef CROCODDYL_CORE_SOLVER_BASE_HPP_
#define CROCODDYL_CORE_SOLVER_BASE_HPP_

#include "crocoddyl/core/fwd.hpp"
#include "crocoddyl/core/optctrl/shooting.hpp"
#include "crocoddyl/core/utils/stop-watch.hpp"

namespace crocoddyl {

template <typename Scalar>
struct DefaultVector {
 static const std::vector<Eigen::Matrix<Scalar, Eigen::Dynamic, 1>> value;
};

template <typename Scalar>
const std::vector<Eigen::Matrix<Scalar, Eigen::Dynamic, 1>>
 DefaultVector<Scalar>::value = {};

#define DEFAULT_VECTOR DefaultVector<double>::value

enum FeasibilityNorm { LInf = 0, L1 = 1, L2 = 2 };

class SolverBase {
 public:
 virtual ~SolverBase() = default;

 CROCODDYL_BASE_CAST(SolverBase, SolverAbstractTpl)
};

template <typename _Scalar>
class SolverAbstractTpl : public SolverBase {
 public:
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 typedef _Scalar Scalar;
 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;

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

 virtual bool solve(
 const std::vector<VectorXs>& init_xs = DefaultVector<Scalar>::value,
 const std::vector<VectorXs>& init_us = DefaultVector<Scalar>::value,
 const std::size_t maxiter = 100, const bool is_feasible = false,
 const Scalar reg_init = std::numeric_limits<Scalar>::quiet_NaN());

 virtual void computeDirection(const bool recalc) = 0;

 virtual Scalar tryStep(const Scalar steplength = Scalar(1.));

 virtual void computeCandidate(const Scalar steplength = Scalar(1.));

 virtual Scalar stoppingCriteria() = 0;

 virtual Vector3s expectedImprovement() = 0;

 virtual void computeMeritFunctionImprovement();

 virtual void computeExpectedMeritFunctionImprovement();

 virtual void updateMeritFunction();

 virtual bool checkAcceptance();

 virtual void calcDir();

 virtual void setCandidate(
 const std::vector<VectorXs>& xs_warm = DefaultVector<Scalar>::value,
 const std::vector<VectorXs>& us_warm = DefaultVector<Scalar>::value,
 const bool is_feasible = false);

 virtual void updateCandidate();

 virtual bool decreaseRegularizationCriteria();

 virtual bool increaseRegularizationCriteria();

 virtual void increaseRegularization();

 virtual void decreaseRegularization();

 virtual void resizeData();

 Scalar computeFeasibility(const std::vector<VectorXs>& fs);

 Scalar computeDynamicFeasibility();

 Scalar computeStateFeasibility(const std::vector<VectorXs>& xs);

 Scalar computeControlFeasibility(const std::vector<VectorXs>& us);

 Scalar computeInequalityFeasibility();

 Scalar computeEqualityFeasibility();

 void setCallbacks(
 const std::vector<std::shared_ptr<CallbackAbstract>>& callbacks);

 const std::vector<std::shared_ptr<CallbackAbstract>>& getCallbacks() const;

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

 const std::vector<Scalar>& get_alphas() const;

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

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

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

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

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

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

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

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

 bool get_is_feasible() const;

 Scalar get_cost() const;

 Scalar get_merit() const;

 Scalar get_stop() const;

 const Vector3s& get_DV() const;

 Scalar get_dV() const;

 Scalar get_dPhi() const;

 Scalar get_dVexp() const;

 Scalar get_dPhiexp() const;

 Scalar get_dfeas() const;

 Scalar get_feas() const;

 Scalar get_ffeas() const;

 Scalar get_gfeas() const;

 Scalar get_hfeas() const;

 Scalar get_ffeas_try() const;

 Scalar get_gfeas_try() const;

 Scalar get_hfeas_try() const;

 Scalar get_preg() const;

 Scalar get_dreg() const;

 DEPRECATED(
 "Use get_preg for primal-variable regularization",
 Scalar get_xreg() const { return preg_; })
 DEPRECATED(
 "Use get_preg for primal-variable regularization",
 Scalar get_ureg() const { return preg_; })

 Scalar get_reg_min() const;

 Scalar get_reg_max() const;

 Scalar get_steplength() const;

 Scalar get_th_acceptstep() const;

 Scalar get_th_stop() const;

 DEPRECATED(
 "Do not use this threshold. It is not needed by our solvers",
 Scalar get_th_gaptol() const { return th_gaptol_; })

 FeasibilityNorm get_feasnorm() const;

 std::size_t get_iter() const;

 void set_alphas(const std::vector<Scalar>& alphas);

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

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

 void set_preg(const Scalar preg);

 void set_dreg(const Scalar dreg);

 void set_reg_min(const Scalar regmin);

 void set_reg_max(const Scalar regmax);

 DEPRECATED(
 "Use set_preg for primal-variable regularization",
 void set_xreg(const Scalar xreg) {
 if (xreg < Scalar(0.)) {
 throw_pretty(
 "Invalid argument: " << "xreg value has to be positive.");
 }
 xreg_ = xreg;
 preg_ = xreg;
 })
 DEPRECATED(
 "Use set_preg for primal-variable regularization",
 void set_ureg(const Scalar ureg) {
 if (ureg < Scalar(0.)) {
 throw_pretty(
 "Invalid argument: " << "ureg value has to be positive.");
 }
 ureg_ = ureg;
 preg_ = ureg;
 })

 void set_th_acceptstep(const Scalar th_acceptstep);

 void set_th_stop(const Scalar th_stop);

 DEPRECATED("Do not use this threshold. It is not needed by our solvers",
 void set_th_gaptol(const Scalar th_gaptol);)

 void set_feasnorm(const FeasibilityNorm feas_norm);

 protected:
 void allocateData();

 virtual void resizeRunningData();

 virtual void resizeTerminalData();

 std::shared_ptr<ShootingProblem> problem_;
 std::vector<std::shared_ptr<CallbackAbstract>>
 callbacks_;
 std::vector<Scalar>
 alphas_;
 Scalar th_acceptstep_;
 Scalar th_stop_;
 DEPRECATED("Do not use this threshold. It is not needed by our solvers",
 Scalar th_gaptol_;)
 enum FeasibilityNorm feasnorm_;

 // allocate data
 Scalar dImpr_;
 std::vector<VectorXs> xs_;
 std::vector<VectorXs> us_;
 std::vector<VectorXs> fs_;
 std::vector<VectorXs>
 xs_try_;
 std::vector<VectorXs>
 us_try_;
 std::vector<VectorXs> fs_try_;
 std::vector<VectorXs> dxs_;
 std::vector<VectorXs> dus_;
 bool is_feasible_;
 bool was_feasible_;
 Scalar cost_;
 Scalar cost_try_;
 Scalar merit_;
 Scalar stop_;
 std::size_t iter_;
 Vector3s DV_;
 Scalar dV_;
 Scalar dPhi_;
 Scalar dVexp_full_;
 Scalar dVexp_;
 Scalar dPhiexp_;
 Scalar dfeas_;
 Scalar feas_;
 Scalar
 ffeas_;
 Scalar gfeas_;
 Scalar hfeas_;
 Scalar ffeas_try_;
 Scalar gfeas_try_;
 Scalar hfeas_try_;
 Scalar tmp_feas_;
 Scalar preg_;
 Scalar dreg_;
 DEPRECATED("Use preg_ for primal-variable regularization",
 Scalar xreg_;)
 DEPRECATED("Use dreg_ for primal-variable regularization",
 Scalar ureg_;)
 Scalar reg_min_;
 Scalar reg_max_;
 Scalar steplength_;
 std::vector<VectorXs> g_adj_;
 std::vector<VectorXs> x_adj_;
 std::vector<VectorXs> u_adj_;
 std::size_t nh_T_;
 std::size_t ng_T_;

 bool acceptstep_;
 bool recalcdir_;

 private:
 SolverAbstractTpl() : problem_(nullptr) {}
};

class CallbackBase {
 public:
 virtual ~CallbackBase() = default;

 CROCODDYL_BASE_CAST(CallbackBase, CallbackAbstractTpl)
};

template <typename _Scalar>
class CallbackAbstractTpl : public CallbackBase {
 public:
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 typedef _Scalar Scalar;
 typedef SolverAbstractTpl<Scalar> SolverAbstract;

 CallbackAbstractTpl() {}
 virtual ~CallbackAbstractTpl() = default;

 virtual void operator()(SolverAbstract& solver) = 0;
};

template <typename Scalar>
bool raiseIfNaN(const Scalar value);

} // namespace crocoddyl

/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "crocoddyl/core/solver-base.hxx"

CROCODDYL_DISABLE_WARNING_DEPRECATED

CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::SolverAbstractTpl)

CROCODDYL_ENABLE_WARNING_DEPRECATED

#endif // CROCODDYL_CORE_SOLVER_BASE_HPP_