intro.hpp

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

#ifndef CROCODDYL_CORE_SOLVERS_INTRO_HPP_
#define CROCODDYL_CORE_SOLVERS_INTRO_HPP_

#include "crocoddyl/core/solvers/fddp.hpp"

namespace crocoddyl {

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

 typedef _Scalar Scalar;
 typedef SolverAbstractTpl<Scalar> SolverAbstract;
 typedef SolverFDDPTpl<Scalar> SolverFDDP;
 typedef ShootingProblemTpl<Scalar> ShootingProblem;
 typedef typename ShootingProblem::ActionModelAbstract ActionModelAbstract;
 typedef typename ShootingProblem::ActionDataAbstract ActionDataAbstract;
 typedef MathBaseTpl<Scalar> MathBase;
 typedef typename MathBase::VectorXs VectorXs;
 typedef typename MathBase::MatrixXs MatrixXs;
 typedef typename MathBase::MatrixXsRowMajor MatrixXsRowMajor;

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

 virtual void calcDir() override;

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

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

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

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

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

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

 EqualitySolverType get_equality_solver() const;

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

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

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

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

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

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

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

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

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

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

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

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

 void set_equality_solver(const EqualitySolverType type);

 protected:
 void allocateData();

 virtual void resizeRunningData() override;

 virtual void resizeTerminalData() override;

 using SolverAbstract::feasnorm_;
 using SolverAbstract::is_feasible_;
 using SolverAbstract::th_gaptol_;
 using SolverAbstract::us_;
 using SolverAbstract::xs_;
 using SolverFDDP::alphas_;
 using SolverFDDP::callbacks_;
 using SolverFDDP::dyn_solver_;
 using SolverFDDP::fs_;
 using SolverFDDP::K_;
 using SolverFDDP::k_;
 using SolverFDDP::Kc_;
 using SolverFDDP::problem_;
 using SolverFDDP::Qu_;
 using SolverFDDP::Quc_;
 using SolverFDDP::Quu_;
 using SolverFDDP::Quu_llt_;
 using SolverFDDP::Quuk_;
 using SolverFDDP::Qx_;
 using SolverFDDP::Qxu_;
 using SolverFDDP::Qxx_;
 using SolverFDDP::reg_decfactor_;
 using SolverFDDP::reg_incfactor_;
 using SolverFDDP::reg_max_;
 using SolverFDDP::reg_min_;
 using SolverFDDP::rho_;
 using SolverFDDP::term_solver_;
 using SolverFDDP::th_acceptminstep_;
 using SolverFDDP::th_acceptnegstep_;
 using SolverFDDP::th_acceptstep_;
 using SolverFDDP::th_grad_;
 using SolverFDDP::th_minfeas_;
 using SolverFDDP::th_minimprove_;
 using SolverFDDP::th_noimprovement_;
 using SolverFDDP::th_stepdec_;
 using SolverFDDP::th_stepinc_;
 using SolverFDDP::th_stop_;
 using SolverFDDP::Ts_;
 using SolverFDDP::upsilon_decfactor_;
 using SolverFDDP::Vx_;
 using SolverFDDP::Vxc_;
 using SolverFDDP::Vxx_;
 using SolverFDDP::Vxx_f_;
 using SolverFDDP::Vxx_tmp_;
 using SolverFDDP::zero_upsilon_;

 void calcLuNullDir();
 void calcQrNullDir();
 void computeNullPolicy(const std::size_t t);
 void computeNullBatchPolicy(const std::size_t t);
 void computeSchurPolicy(const std::size_t t);
 void computeSchurBatchPolicy(const std::size_t t);

 enum EqualitySolverType
 eq_solver_;

 std::vector<std::size_t>
 Hu_rank_;
 std::vector<MatrixXsRowMajor> KQuu_2Qxu_;
 std::vector<MatrixXs>
 YZ_;
 std::vector<MatrixXs>
 Hy_;
 std::vector<VectorXs>
 Qz_;
 std::vector<MatrixXs>
 Qzz_;
 std::vector<MatrixXs>
 Qxz_;
 std::vector<MatrixXs>
 Quz_;
 std::vector<VectorXs>
 kz_;
 std::vector<MatrixXs>
 Kz_;
 std::vector<VectorXs>
 ks_;
 std::vector<MatrixXs>
 Ks_;
 std::vector<MatrixXs> QuuinvHuT_;
 std::vector<Eigen::LLT<MatrixXs> > Qzz_llt_;
 std::vector<Eigen::FullPivLU<MatrixXs> >
 Hu_lu_;
 std::vector<Eigen::ColPivHouseholderQR<MatrixXs> >
 Hu_qr_;
 std::vector<Eigen::PartialPivLU<MatrixXs> >
 Hy_lu_;

 std::vector<MatrixXs> Kcs_;
 std::vector<MatrixXs> QuuKc_Quc_;
 std::vector<MatrixXs>
 Qzc_;
};

} // namespace crocoddyl

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

CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::SolverIntroTpl)

#endif // CROCODDYL_CORE_SOLVERS_INTRO_HPP_