fddp.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_SOLVERS_FDDP_HPP_
#define CROCODDYL_CORE_SOLVERS_FDDP_HPP_

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

namespace crocoddyl {

enum DynamicsSolverType {
 FeasShoot = 0,
 MultiShoot,
 HybridShoot,
 SingleShoot
};

enum EqualitySolverType {
 LuNull = 0,
 QrNull,
 Schur,
};

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

 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 SolverAbstract::computeDynamicFeasibility;
 using SolverAbstract::computeEqualityFeasibility;
 using SolverAbstract::computeFeasibility;
 using SolverAbstract::computeInequalityFeasibility;
 using SolverAbstract::resizeData;

 explicit SolverFDDPTpl(std::shared_ptr<ShootingProblem> problem,
 const DynamicsSolverType dyn_solver = FeasShoot,
 const EqualitySolverType term_solver = LuNull);
 virtual ~SolverFDDPTpl() = default;

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

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

 void forwardPass(const Scalar steplength = Scalar(1.));

 virtual void updateDualsAndSlacks(const Scalar stepLength = Scalar(1.));

 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 backwardPass();

 void batchPass();

 void updateDir();

 virtual void computeActionValueFunction(
 const std::size_t t, const std::shared_ptr<ActionModelAbstract>& model,
 const std::shared_ptr<ActionDataAbstract>& data);

 virtual void computeBatchActionValueFunction(
 const std::size_t t, const std::shared_ptr<ActionDataAbstract>& data);

 virtual void computePolicy(const std::size_t t);

 virtual void computeBatchPolicy(const std::size_t t);

 virtual void computeValueFunction(
 const std::size_t t, const std::shared_ptr<ActionModelAbstract>& model);

 virtual void computeBatchValueFunction(const std::size_t t);

 void linearRollout();

 virtual void feasShootForwardPass(const Scalar steplength);

 virtual void multiShootForwardPass(const Scalar steplength);

 virtual void hybridShootForwardPass(const Scalar steplength);

 virtual void singleShootForwardPass(const Scalar steplength);

 void updateCandidate() override;

 bool decreaseRegularizationCriteria() override;

 bool increaseRegularizationCriteria() override;

 void increaseRegularization() override;

 void decreaseRegularization() override;

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

 DynamicsSolverType get_dynamics_solver() const;

 EqualitySolverType get_terminal_solver() const;

 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;

 const std::vector<std::size_t>& get_Ts() const;

 const std::vector<MatrixXs>& get_Vxx() const;

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

 const std::vector<MatrixXs>& get_Qxx() const;

 const std::vector<MatrixXs>& get_Qxu() const;

 const std::vector<MatrixXs>& get_Quu() const;

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

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

 const std::vector<MatrixXsRowMajor>& get_K() const;

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

 const std::vector<MatrixXs>& get_Vxc() const;

 const std::vector<MatrixXs>& get_Qxc() const;

 const std::vector<MatrixXs>& get_Quc() const;

 const std::vector<MatrixXs>& get_dXc() const;

 const std::vector<MatrixXs>& get_dUc() const;

 const std::vector<MatrixXs>& get_Kc() const;

 const MatrixXs& get_dHc() const;

 const VectorXs& get_hc() const;

 const VectorXs& get_beta_plus() const;

 void set_dynamics_solver(const DynamicsSolverType type,
 const std::size_t Tshoot = 0);

 void set_terminal_solver(const EqualitySolverType type);

 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 computeNullTerminalMultiplier();

 DynamicsSolverType dyn_solver_;
 EqualitySolverType term_solver_;
 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::vector<std::size_t> Ts_;

 // allocate data
 MatrixXs Vxx_tmp_;
 std::vector<MatrixXs>
 Vxx_;
 std::vector<VectorXs>
 Vxx_f_;
 std::vector<VectorXs>
 Vx_;
 std::vector<VectorXs>
 Lxx_dx_;
 std::vector<VectorXs>
 Luu_du_;
 std::vector<VectorXs>
 Lxu_du_;
 std::vector<MatrixXs>
 Qxx_;
 std::vector<MatrixXs>
 Qxu_;
 std::vector<MatrixXs>
 Quu_;
 std::vector<VectorXs>
 Qx_;
 std::vector<VectorXs>
 Qu_;
 std::vector<MatrixXsRowMajor> K_;
 std::vector<VectorXs> k_;
 std::vector<VectorXs>
 dx_;
 std::vector<MatrixXsRowMajor>
 FxTVxx_p_;
 std::vector<MatrixXsRowMajor>
 FuTVxx_p_;
 VectorXs fTVxx_p_;
 std::vector<Eigen::LLT<MatrixXs> > Quu_llt_;
 std::vector<VectorXs>
 Quuk_;

 std::size_t dHc_rank_;
 std::vector<MatrixXs>
 Vxc_;
 std::vector<MatrixXs>
 Qxc_;
 std::vector<MatrixXs>
 Quc_;
 std::vector<MatrixXs> dXc_;
 std::vector<MatrixXs> dUc_;
 std::vector<MatrixXs> Kc_;
 MatrixXs dHc_;
 VectorXs hc_;
 MatrixXs YZc_;
 VectorXs Yhc_;
 MatrixXs dHcY_;
 MatrixXs YdHcY_;
 VectorXs beta_plus_;
 Eigen::LLT<MatrixXs>
 YdHcY_llt_;
 Eigen::FullPivLU<MatrixXs>
 dHc_lu_;
 Eigen::ColPivHouseholderQR<MatrixXs>
 dHc_qr_;

 DEPRECATED(
 "Do not use this member",
 Scalar dg_;)
 DEPRECATED(
 "Do not use this member",
 Scalar dq_;)
 DEPRECATED(
 "Do not use this member",
 Scalar dv_;)

 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::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/fddp.hxx"

CROCODDYL_DISABLE_WARNING_DEPRECATED

CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::SolverFDDPTpl)

CROCODDYL_ENABLE_WARNING_DEPRECATED

#endif // CROCODDYL_CORE_SOLVERS_FDDP_HPP_