crocoddyl  1.3.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
ddp.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_CORE_SOLVERS_DDP_HPP_
10 #define CROCODDYL_CORE_SOLVERS_DDP_HPP_
11 
12 #include <Eigen/Cholesky>
13 #include <vector>
14 
15 #include "crocoddyl/core/solver-base.hpp"
16 
17 namespace crocoddyl {
18 
19 class SolverDDP : public SolverAbstract {
20  public:
21  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
22 
23  explicit SolverDDP(boost::shared_ptr<ShootingProblem> problem);
24  virtual ~SolverDDP();
25 
26  virtual bool solve(const std::vector<Eigen::VectorXd>& init_xs = DEFAULT_VECTOR,
27  const std::vector<Eigen::VectorXd>& init_us = DEFAULT_VECTOR, const std::size_t& maxiter = 100,
28  const bool& is_feasible = false, const double& regInit = 1e-9);
29  virtual void computeDirection(const bool& recalc = true);
30  virtual double tryStep(const double& steplength = 1);
31  virtual double stoppingCriteria();
32  virtual const Eigen::Vector2d& expectedImprovement();
33  virtual double calcDiff();
34  virtual void backwardPass();
35  virtual void forwardPass(const double& stepLength);
36 
37  virtual void computeGains(const std::size_t& t);
38  void increaseRegularization();
39  void decreaseRegularization();
40  virtual void allocateData();
41 
42  const double& get_regfactor() const;
43  const double& get_regmin() const;
44  const double& get_regmax() const;
45  const std::vector<double>& get_alphas() const;
46  const double& get_th_stepdec() const;
47  const double& get_th_stepinc() const;
48  const double& get_th_grad() const;
49  const std::vector<Eigen::MatrixXd>& get_Vxx() const;
50  const std::vector<Eigen::VectorXd>& get_Vx() const;
51  const std::vector<Eigen::MatrixXd>& get_Qxx() const;
52  const std::vector<Eigen::MatrixXd>& get_Qxu() const;
53  const std::vector<Eigen::MatrixXd>& get_Quu() const;
54  const std::vector<Eigen::VectorXd>& get_Qx() const;
55  const std::vector<Eigen::VectorXd>& get_Qu() const;
56  const std::vector<Eigen::MatrixXd>& get_K() const;
57  const std::vector<Eigen::VectorXd>& get_k() const;
58  const std::vector<Eigen::VectorXd>& get_fs() const;
59 
60  void set_regfactor(const double& reg_factor);
61  void set_regmin(const double& regmin);
62  void set_regmax(const double& regmax);
63  void set_alphas(const std::vector<double>& alphas);
64  void set_th_stepdec(const double& th_step);
65  void set_th_stepinc(const double& th_step);
66  void set_th_grad(const double& th_grad);
67 
68  protected:
69  double regfactor_;
70  double regmin_;
71  double regmax_;
72 
73  double cost_try_;
74  std::vector<Eigen::VectorXd> xs_try_;
75  std::vector<Eigen::VectorXd> us_try_;
76  std::vector<Eigen::VectorXd> dx_;
77 
78  // allocate data
79  std::vector<Eigen::MatrixXd> Vxx_;
80  std::vector<Eigen::VectorXd> Vx_;
81  std::vector<Eigen::MatrixXd> Qxx_;
82  std::vector<Eigen::MatrixXd> Qxu_;
83  std::vector<Eigen::MatrixXd> Quu_;
84  std::vector<Eigen::VectorXd> Qx_;
85  std::vector<Eigen::VectorXd> Qu_;
86  std::vector<Eigen::MatrixXd> K_;
87  std::vector<Eigen::VectorXd> k_;
88  std::vector<Eigen::VectorXd> fs_;
89 
90  Eigen::VectorXd xnext_;
91  Eigen::MatrixXd FxTVxx_p_;
92  std::vector<Eigen::MatrixXd> FuTVxx_p_;
93  Eigen::VectorXd fTVxx_p_;
94  std::vector<Eigen::LLT<Eigen::MatrixXd> > Quu_llt_;
95  std::vector<Eigen::VectorXd> Quuk_;
96  std::vector<double> alphas_;
97  double th_grad_;
98  double th_stepdec_;
99  double th_stepinc_;
101 };
102 
103 } // namespace crocoddyl
104 
105 #endif // CROCODDYL_CORE_SOLVERS_DDP_HPP_
std::vector< Eigen::VectorXd > fs_
Gaps/defects between shooting nodes.
Definition: ddp.hpp:88
std::vector< Eigen::VectorXd > xs_try_
State trajectory computed by line-search procedure.
Definition: ddp.hpp:74
double th_stepdec_
Step-length threshold used to decrease regularization.
Definition: ddp.hpp:98
std::vector< Eigen::VectorXd > k_
Feed-forward terms.
Definition: ddp.hpp:87
std::vector< Eigen::VectorXd > Vx_
Gradient of the Value function.
Definition: ddp.hpp:80
std::vector< Eigen::MatrixXd > Qxx_
Hessian of the Hamiltonian.
Definition: ddp.hpp:81
std::vector< Eigen::MatrixXd > K_
Feedback gains.
Definition: ddp.hpp:86
std::vector< Eigen::MatrixXd > Vxx_
Hessian of the Value function.
Definition: ddp.hpp:79
double th_stepinc_
Step-length threshold used to increase regularization.
Definition: ddp.hpp:99
bool was_feasible_
Label that indicates in the previous iterate was feasible.
Definition: ddp.hpp:100
double th_grad_
Tolerance of the expected gradient used for testing the step.
Definition: ddp.hpp:97
std::vector< double > alphas_
Set of step lengths using by the line-search procedure.
Definition: ddp.hpp:96
std::vector< Eigen::VectorXd > Qx_
Gradient of the Hamiltonian.
Definition: ddp.hpp:84
double regmin_
Minimum allowed regularization value.
Definition: ddp.hpp:70
double regmax_
Maximum allowed regularization value.
Definition: ddp.hpp:71
double regfactor_
Regularization factor used to decrease / increase it.
Definition: ddp.hpp:69
std::vector< Eigen::MatrixXd > Qxu_
Hessian of the Hamiltonian.
Definition: ddp.hpp:82
std::vector< Eigen::VectorXd > us_try_
Control trajectory computed by line-search procedure.
Definition: ddp.hpp:75
std::vector< Eigen::MatrixXd > Quu_
Hessian of the Hamiltonian.
Definition: ddp.hpp:83
std::vector< Eigen::VectorXd > Qu_
Gradient of the Hamiltonian.
Definition: ddp.hpp:85
double cost_try_
Total cost computed by line-search procedure.
Definition: ddp.hpp:73