ddp.hpp

// BSD 3-Clause License
//
// Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh,
//                          University of Oxford, Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
 
#ifndef CROCODDYL_CORE_SOLVERS_DDP_HPP_
#define CROCODDYL_CORE_SOLVERS_DDP_HPP_
 
#include <Eigen/Cholesky>
#include <vector>
 
#include "crocoddyl/core/mathbase.hpp"
#include "crocoddyl/core/solver-base.hpp"
#include "crocoddyl/core/utils/deprecate.hpp"
 
namespace crocoddyl {
 
class SolverDDP : public SolverAbstract {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
  typedef typename MathBaseTpl<double>::MatrixXsRowMajor MatrixXdRowMajor;
 
  explicit SolverDDP(boost::shared_ptr<ShootingProblem> problem);
  virtual ~SolverDDP();
 
  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 void computeDirection(const bool recalc = true);
  virtual double tryStep(const double steplength = 1);
  virtual double stoppingCriteria();
  virtual const Eigen::Vector2d& expectedImprovement();
  virtual void resizeData();
 
  virtual double calcDiff();
 
  virtual void backwardPass();
 
  virtual void forwardPass(const double stepLength);
 
  virtual void computeActionValueFunction(
      const std::size_t t, const boost::shared_ptr<ActionModelAbstract>& model,
      const boost::shared_ptr<ActionDataAbstract>& data);
 
  virtual void computeValueFunction(
      const std::size_t t, const boost::shared_ptr<ActionModelAbstract>& model);
 
  virtual void computeGains(const std::size_t t);
 
  void increaseRegularization();
 
  void decreaseRegularization();
 
  virtual void allocateData();
 
  double get_reg_incfactor() const;
 
  double get_reg_decfactor() const;
 
  DEPRECATED("Use get_reg_incfactor() or get_reg_decfactor()",
             double get_regfactor() const;)
 
  double get_reg_min() const;
  DEPRECATED("Use get_reg_min()", double get_regmin() const);
 
  double get_reg_max() const;
  DEPRECATED("Use get_reg_max()", double get_regmax() const);
 
  const std::vector<double>& get_alphas() const;
 
  double get_th_stepdec() const;
 
  double get_th_stepinc() const;
 
  double get_th_grad() const;
 
  const std::vector<Eigen::MatrixXd>& get_Vxx() const;
 
  const std::vector<Eigen::VectorXd>& get_Vx() const;
 
  const std::vector<Eigen::MatrixXd>& get_Qxx() const;
 
  const std::vector<Eigen::MatrixXd>& get_Qxu() const;
 
  const std::vector<Eigen::MatrixXd>& get_Quu() const;
 
  const std::vector<Eigen::VectorXd>& get_Qx() const;
 
  const std::vector<Eigen::VectorXd>& get_Qu() const;
 
  const std::vector<MatrixXdRowMajor>& get_K() const;
 
  const std::vector<Eigen::VectorXd>& get_k() const;
 
  void set_reg_incfactor(const double reg_factor);
 
  void set_reg_decfactor(const double reg_factor);
 
  DEPRECATED("Use set_reg_incfactor() or set_reg_decfactor()",
             void set_regfactor(const double reg_factor);)
 
  void set_reg_min(const double regmin);
  DEPRECATED("Use set_reg_min()", void set_regmin(const double regmin));
 
  void set_reg_max(const double regmax);
  DEPRECATED("Use set_reg_max()", void set_regmax(const double regmax));
 
  void set_alphas(const std::vector<double>& alphas);
 
  void set_th_stepdec(const double th_step);
 
  void set_th_stepinc(const double th_step);
 
  void set_th_grad(const double th_grad);
 
 protected:
  double reg_incfactor_;  
  double reg_decfactor_;  
  double reg_min_;        
  double reg_max_;        
 
  double cost_try_;  
  std::vector<Eigen::VectorXd>
      xs_try_;  
  std::vector<Eigen::VectorXd>
      us_try_;  
  std::vector<Eigen::VectorXd>
      dx_;  
 
  // allocate data
  std::vector<Eigen::MatrixXd>
      Vxx_;  
  Eigen::MatrixXd
      Vxx_tmp_;  
  std::vector<Eigen::VectorXd>
      Vx_;  
  std::vector<Eigen::MatrixXd>
      Qxx_;  
  std::vector<Eigen::MatrixXd>
      Qxu_;  
  std::vector<Eigen::MatrixXd>
      Quu_;  
  std::vector<Eigen::VectorXd>
      Qx_;  
  std::vector<Eigen::VectorXd>
      Qu_;  
  std::vector<MatrixXdRowMajor> K_;  
  std::vector<Eigen::VectorXd> k_;   
 
  Eigen::VectorXd xnext_;      
  MatrixXdRowMajor FxTVxx_p_;  
  std::vector<MatrixXdRowMajor>
      FuTVxx_p_;             
  Eigen::VectorXd fTVxx_p_;  
  std::vector<Eigen::LLT<Eigen::MatrixXd> > Quu_llt_;  
  std::vector<Eigen::VectorXd>
      Quuk_;  
  std::vector<double>
      alphas_;      
  double th_grad_;  
  double
      th_stepdec_;  
  double
      th_stepinc_;  
};
 
}  // namespace crocoddyl
 
#endif  // CROCODDYL_CORE_SOLVERS_DDP_HPP_