crocoddyl  1.4.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 
48 class SolverDDP : public SolverAbstract {
49  public:
50  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
51 
57  explicit SolverDDP(boost::shared_ptr<ShootingProblem> problem);
58  virtual ~SolverDDP();
59 
60  virtual bool solve(const std::vector<Eigen::VectorXd>& init_xs = DEFAULT_VECTOR,
61  const std::vector<Eigen::VectorXd>& init_us = DEFAULT_VECTOR, const std::size_t& maxiter = 100,
62  const bool& is_feasible = false, const double& regInit = 1e-9);
63  virtual void computeDirection(const bool& recalc = true);
64  virtual double tryStep(const double& steplength = 1);
65  virtual double stoppingCriteria();
66  virtual const Eigen::Vector2d& expectedImprovement();
67 
76  virtual double calcDiff();
77 
105  virtual void backwardPass();
106 
121  virtual void forwardPass(const double& stepLength);
122 
136  virtual void computeGains(const std::size_t& t);
137 
141  void increaseRegularization();
142 
146  void decreaseRegularization();
147 
151  virtual void allocateData();
152 
156  const double& get_regfactor() const;
157 
161  const double& get_regmin() const;
162 
166  const double& get_regmax() const;
167 
171  const std::vector<double>& get_alphas() const;
172 
176  const double& get_th_stepdec() const;
177 
181  const double& get_th_stepinc() const;
182 
186  const double& get_th_grad() const;
187 
191  const std::vector<Eigen::MatrixXd>& get_Vxx() const;
192 
196  const std::vector<Eigen::VectorXd>& get_Vx() const;
197 
201  const std::vector<Eigen::MatrixXd>& get_Qxx() const;
202 
206  const std::vector<Eigen::MatrixXd>& get_Qxu() const;
207 
211  const std::vector<Eigen::MatrixXd>& get_Quu() const;
212 
216  const std::vector<Eigen::VectorXd>& get_Qx() const;
217 
221  const std::vector<Eigen::VectorXd>& get_Qu() const;
222 
226  const std::vector<Eigen::MatrixXd>& get_K() const;
227 
231  const std::vector<Eigen::VectorXd>& get_k() const;
232 
236  const std::vector<Eigen::VectorXd>& get_fs() const;
237 
241  void set_regfactor(const double& reg_factor);
242 
246  void set_regmin(const double& regmin);
247 
251  void set_regmax(const double& regmax);
252 
256  void set_alphas(const std::vector<double>& alphas);
257 
261  void set_th_stepdec(const double& th_step);
262 
266  void set_th_stepinc(const double& th_step);
267 
271  void set_th_grad(const double& th_grad);
272 
273  protected:
274  double regfactor_;
275  double regmin_;
276  double regmax_;
277 
278  double cost_try_;
279  std::vector<Eigen::VectorXd> xs_try_;
280  std::vector<Eigen::VectorXd> us_try_;
281  std::vector<Eigen::VectorXd> dx_;
282 
283  // allocate data
284  std::vector<Eigen::MatrixXd> Vxx_;
285  std::vector<Eigen::VectorXd> Vx_;
286  std::vector<Eigen::MatrixXd> Qxx_;
287  std::vector<Eigen::MatrixXd> Qxu_;
288  std::vector<Eigen::MatrixXd> Quu_;
289  std::vector<Eigen::VectorXd> Qx_;
290  std::vector<Eigen::VectorXd> Qu_;
291  std::vector<Eigen::MatrixXd> K_;
292  std::vector<Eigen::VectorXd> k_;
293  std::vector<Eigen::VectorXd> fs_;
294 
295  Eigen::VectorXd xnext_;
296  Eigen::MatrixXd FxTVxx_p_;
297  std::vector<Eigen::MatrixXd> FuTVxx_p_;
298  Eigen::VectorXd fTVxx_p_;
299  std::vector<Eigen::LLT<Eigen::MatrixXd> > Quu_llt_;
300  std::vector<Eigen::VectorXd> Quuk_;
301  std::vector<double> alphas_;
302  double th_grad_;
303  double th_stepdec_;
304  double th_stepinc_;
306 };
307 
308 } // namespace crocoddyl
309 
310 #endif // CROCODDYL_CORE_SOLVERS_DDP_HPP_
const double & get_regfactor() const
Return the regularization factor used to decrease / increase it.
Definition: ddp.cpp:386
std::vector< Eigen::VectorXd > fs_
Gaps/defects between shooting nodes.
Definition: ddp.hpp:293
void set_th_stepdec(const double &th_step)
Modify the step-length threshold used to decrease regularization.
Definition: ddp.cpp:464
EIGEN_MAKE_ALIGNED_OPERATOR_NEW SolverDDP(boost::shared_ptr< ShootingProblem > problem)
Initialize the DDP solver.
Definition: ddp.cpp:15
virtual void backwardPass()
Run the backward pass (Riccati sweep)
Definition: ddp.cpp:180
const double & get_regmax() const
Return the maximum regularization value.
Definition: ddp.cpp:390
const double & get_th_grad() const
Return the tolerance of the expected gradient used for testing the step.
Definition: ddp.cpp:398
std::vector< Eigen::MatrixXd > FuTVxx_p_
fuTVxx_p_
Definition: ddp.hpp:297
const std::vector< Eigen::VectorXd > & get_k() const
Return the feedforward gains .
Definition: ddp.cpp:416
Eigen::MatrixXd FxTVxx_p_
fxTVxx_p_
Definition: ddp.hpp:296
const std::vector< Eigen::VectorXd > & get_Qx() const
Return the Jacobian of the Hamiltonian function .
Definition: ddp.cpp:410
const std::vector< Eigen::VectorXd > & get_Vx() const
Return the Hessian of the Value function .
Definition: ddp.cpp:402
const std::vector< Eigen::MatrixXd > & get_Qxu() const
Return the Hessian of the Hamiltonian function .
Definition: ddp.cpp:406
const double & get_th_stepinc() const
Return the step-length threshold used to increase regularization.
Definition: ddp.cpp:396
virtual void allocateData()
Allocate all the internal data needed for the solver.
Definition: ddp.cpp:328
std::vector< Eigen::VectorXd > xs_try_
State trajectory computed by line-search procedure.
Definition: ddp.hpp:279
double th_stepdec_
Step-length threshold used to decrease regularization.
Definition: ddp.hpp:303
const std::vector< Eigen::MatrixXd > & get_Quu() const
Return the Hessian of the Hamiltonian function .
Definition: ddp.cpp:408
const std::vector< double > & get_alphas() const
Return the set of step lengths using by the line-search procedure.
Definition: ddp.cpp:392
Abstract class for optimal control solvers.
Definition: solver-base.hpp:50
std::vector< Eigen::VectorXd > k_
Feed-forward terms.
Definition: ddp.hpp:292
const std::vector< Eigen::MatrixXd > & get_Qxx() const
Return the Hessian of the Hamiltonian function .
Definition: ddp.cpp:404
std::vector< Eigen::VectorXd > Vx_
Gradient of the Value function.
Definition: ddp.hpp:285
void increaseRegularization()
Increase the state and control regularization values by a regfactor_ factor.
Definition: ddp.cpp:312
virtual double stoppingCriteria()
Return a positive value that quantifies the algorithm termination.
Definition: ddp.cpp:132
Eigen::VectorXd fTVxx_p_
fTVxx_p term
Definition: ddp.hpp:298
void set_th_grad(const double &th_grad)
Modify the tolerance of the expected gradient used for testing the step.
Definition: ddp.cpp:480
Eigen::VectorXd xnext_
Next state.
Definition: ddp.hpp:295
std::vector< Eigen::MatrixXd > Qxx_
Hessian of the Hamiltonian.
Definition: ddp.hpp:286
virtual void forwardPass(const double &stepLength)
Run the forward pass or rollout.
Definition: ddp.cpp:255
void decreaseRegularization()
Decrease the state and control regularization values by a regfactor_ factor.
Definition: ddp.cpp:320
void set_alphas(const std::vector< double > &alphas)
Modify the set of step lengths using by the line-search procedure.
Definition: ddp.cpp:444
std::vector< Eigen::MatrixXd > K_
Feedback gains.
Definition: ddp.hpp:291
virtual void computeDirection(const bool &recalc=true)
Compute the search direction for the current guess .
Definition: ddp.cpp:120
const std::vector< Eigen::VectorXd > & get_fs() const
Return the gaps .
Definition: ddp.cpp:418
std::vector< Eigen::MatrixXd > Vxx_
Hessian of the Value function.
Definition: ddp.hpp:284
double th_stepinc_
Step-length threshold used to increase regularization.
Definition: ddp.hpp:304
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 &regInit=1e-9)
Compute the optimal trajectory as lists of and terms.
Definition: ddp.cpp:41
bool was_feasible_
Label that indicates in the previous iterate was feasible.
Definition: ddp.hpp:305
double th_grad_
Tolerance of the expected gradient used for testing the step.
Definition: ddp.hpp:302
std::vector< double > alphas_
Set of step lengths using by the line-search procedure.
Definition: ddp.hpp:301
const std::vector< Eigen::VectorXd > & get_Qu() const
Return the Jacobian of the Hamiltonian function .
Definition: ddp.cpp:412
const std::vector< Eigen::MatrixXd > & get_Vxx() const
Return the Hessian of the Value function .
Definition: ddp.cpp:400
std::vector< Eigen::VectorXd > Qx_
Gradient of the Hamiltonian.
Definition: ddp.hpp:289
virtual void computeGains(const std::size_t &t)
Compute the feedforward and feedback terms using a Cholesky decomposition.
Definition: ddp.cpp:298
const std::vector< Eigen::MatrixXd > & get_K() const
Return the feedback gains .
Definition: ddp.cpp:414
double regmin_
Minimum allowed regularization value.
Definition: ddp.hpp:275
double regmax_
Maximum allowed regularization value.
Definition: ddp.hpp:276
double regfactor_
Regularization factor used to decrease / increase it.
Definition: ddp.hpp:274
Differential Dynamic Programming (DDP) solver.
Definition: ddp.hpp:48
std::vector< Eigen::VectorXd > Quuk_
Quuk term.
Definition: ddp.hpp:300
virtual double calcDiff()
Update the Jacobian and Hessian of the optimal control problem.
Definition: ddp.cpp:157
virtual double tryStep(const double &steplength=1)
Try a predefined step length and compute its cost improvement.
Definition: ddp.cpp:127
std::vector< Eigen::MatrixXd > Qxu_
Hessian of the Hamiltonian.
Definition: ddp.hpp:287
const double & get_th_stepdec() const
Return the step-length threshold used to decrease regularization.
Definition: ddp.cpp:394
void set_th_stepinc(const double &th_step)
Modify the step-length threshold used to increase regularization.
Definition: ddp.cpp:472
void set_regfactor(const double &reg_factor)
Modify the regularization factor used to decrease / increase it.
Definition: ddp.cpp:420
void set_regmin(const double &regmin)
Modify the minimum regularization value.
Definition: ddp.cpp:428
std::vector< Eigen::VectorXd > us_try_
Control trajectory computed by line-search procedure.
Definition: ddp.hpp:280
virtual const Eigen::Vector2d & expectedImprovement()
Return the expected improvement from a given current search direction.
Definition: ddp.cpp:144
void set_regmax(const double &regmax)
Modify the maximum regularization value.
Definition: ddp.cpp:436
const double & get_regmin() const
Return the minimum regularization value.
Definition: ddp.cpp:388
std::vector< Eigen::MatrixXd > Quu_
Hessian of the Hamiltonian.
Definition: ddp.hpp:288
std::vector< Eigen::VectorXd > Qu_
Gradient of the Hamiltonian.
Definition: ddp.hpp:290
std::vector< Eigen::LLT< Eigen::MatrixXd > > Quu_llt_
Cholesky LLT solver.
Definition: ddp.hpp:299
double cost_try_
Total cost computed by line-search procedure.
Definition: ddp.hpp:278