intro.hpp

 
// BSD 3-Clause License
//
// Copyright (C) 2021-2023, 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 {
 
enum EqualitySolverType { LuNull = 0, QrNull, Schur };
 
class SolverIntro : public SolverFDDP {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  explicit SolverIntro(std::shared_ptr<ShootingProblem> problem);
  virtual ~SolverIntro();
 
  virtual bool solve(
      const std::vector<Eigen::VectorXd>& init_xs = DEFAULT_VECTOR,
      const std::vector<Eigen::VectorXd>& init_us = DEFAULT_VECTOR,
      const std::size_t maxiter = 100, const bool is_feasible = false,
      const double init_reg = NAN);
  virtual double tryStep(const double step_length = 1);
  virtual double stoppingCriteria();
  virtual void resizeData();
  virtual double calcDiff();
  virtual void computeValueFunction(
      const std::size_t t, const std::shared_ptr<ActionModelAbstract>& model);
  virtual void computeGains(const std::size_t t);
 
  EqualitySolverType get_equality_solver() const;
 
  double get_th_feas() const;
 
  double get_rho() const;
 
  double get_upsilon() const;
 
  const std::vector<std::size_t>& get_Hu_rank() const;
 
  const std::vector<Eigen::MatrixXd>& get_YZ() const;
 
  const std::vector<Eigen::MatrixXd>& get_Qzz() const;
 
  const std::vector<Eigen::MatrixXd>& get_Qxz() const;
 
  const std::vector<Eigen::MatrixXd>& get_Quz() const;
 
  const std::vector<Eigen::VectorXd>& get_Qz() const;
 
  const std::vector<Eigen::MatrixXd>& get_Hy() const;
 
  const std::vector<Eigen::VectorXd>& get_kz() const;
 
  const std::vector<Eigen::MatrixXd>& get_Kz() const;
 
  const std::vector<Eigen::VectorXd>& get_ks() const;
 
  const std::vector<Eigen::MatrixXd>& get_Ks() const;
 
  bool get_zero_upsilon() const;
 
  void set_equality_solver(const EqualitySolverType type);
 
  void set_th_feas(const double th_feas);
 
  void set_rho(const double rho);
 
  void set_zero_upsilon(const bool zero_upsilon);
 
 protected:
  enum EqualitySolverType
      eq_solver_;   
  double th_feas_;  
  double rho_;      
  double
      upsilon_;  
  bool zero_upsilon_;  
 
  std::vector<std::size_t>
      Hu_rank_;  
  std::vector<Eigen::MatrixXd> KQuu_tmp_;
  std::vector<Eigen::MatrixXd>
      YZ_;  
  std::vector<Eigen::MatrixXd>
      Hy_;  
  std::vector<Eigen::VectorXd>
      Qz_;  
  std::vector<Eigen::MatrixXd>
      Qzz_;  
  std::vector<Eigen::MatrixXd>
      Qxz_;  
  std::vector<Eigen::MatrixXd>
      Quz_;  
  std::vector<Eigen::VectorXd>
      kz_;  
  std::vector<Eigen::MatrixXd>
      Kz_;  
  std::vector<Eigen::VectorXd>
      ks_;  
  std::vector<Eigen::MatrixXd>
      Ks_;  
  std::vector<Eigen::MatrixXd> QuuinvHuT_;
  std::vector<Eigen::LLT<Eigen::MatrixXd> > Qzz_llt_;  
  std::vector<Eigen::FullPivLU<Eigen::MatrixXd> >
      Hu_lu_;  
  std::vector<Eigen::ColPivHouseholderQR<Eigen::MatrixXd> >
      Hu_qr_;  
  std::vector<Eigen::PartialPivLU<Eigen::MatrixXd> >
      Hy_lu_;  
};
 
}  // namespace crocoddyl
 
#endif  // CROCODDYL_CORE_SOLVERS_INTRO_HPP_