crocoddyl  1.8.1
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
ddp.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh, University of Oxford
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 <vector>
13 #include <Eigen/Cholesky>
14 #include "crocoddyl/core/solver-base.hpp"
15 #include "crocoddyl/core/mathbase.hpp"
16 #include "crocoddyl/core/utils/deprecate.hpp"
17 
18 namespace crocoddyl {
19 
49 class SolverDDP : public SolverAbstract {
50  public:
51  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
52 
53  typedef typename MathBaseTpl<double>::MatrixXsRowMajor MatrixXdRowMajor;
54 
60  explicit SolverDDP(boost::shared_ptr<ShootingProblem> problem);
61  virtual ~SolverDDP();
62 
63  virtual bool solve(const std::vector<Eigen::VectorXd>& init_xs = DEFAULT_VECTOR,
64  const std::vector<Eigen::VectorXd>& init_us = DEFAULT_VECTOR, const std::size_t maxiter = 100,
65  const bool is_feasible = false, const double regInit = 1e-9);
66  virtual void computeDirection(const bool recalc = true);
67  virtual double tryStep(const double steplength = 1);
68  virtual double stoppingCriteria();
69  virtual const Eigen::Vector2d& expectedImprovement();
70 
79  virtual double calcDiff();
80 
108  virtual void backwardPass();
109 
124  virtual void forwardPass(const double stepLength);
125 
139  virtual void computeGains(const std::size_t t);
140 
144  void increaseRegularization();
145 
149  void decreaseRegularization();
150 
154  virtual void allocateData();
155 
159  double get_reg_incfactor() const;
160 
164  double get_reg_decfactor() const;
165 
169  DEPRECATED("Use get_reg_incfactor() or get_reg_decfactor()", double get_regfactor() const;)
170 
174  double get_reg_min() const;
175  DEPRECATED("Use get_reg_min()", double get_regmin() const);
176 
180  double get_reg_max() const;
181  DEPRECATED("Use get_reg_max()", double get_regmax() const);
182 
186  const std::vector<double>& get_alphas() const;
187 
191  double get_th_stepdec() const;
192 
196  double get_th_stepinc() const;
197 
201  double get_th_grad() const;
202 
206  double get_th_gaptol() const;
207 
211  const std::vector<Eigen::MatrixXd>& get_Vxx() const;
212 
216  const std::vector<Eigen::VectorXd>& get_Vx() const;
217 
221  const std::vector<Eigen::MatrixXd>& get_Qxx() const;
222 
226  const std::vector<Eigen::MatrixXd>& get_Qxu() const;
227 
231  const std::vector<Eigen::MatrixXd>& get_Quu() const;
232 
236  const std::vector<Eigen::VectorXd>& get_Qx() const;
237 
241  const std::vector<Eigen::VectorXd>& get_Qu() const;
242 
246  const std::vector<MatrixXdRowMajor>& get_K() const;
247 
251  const std::vector<Eigen::VectorXd>& get_k() const;
252 
256  const std::vector<Eigen::VectorXd>& get_fs() const;
257 
261  void set_reg_incfactor(const double reg_factor);
262 
266  void set_reg_decfactor(const double reg_factor);
267 
271  DEPRECATED("Use set_reg_incfactor() or set_reg_decfactor()", void set_regfactor(const double reg_factor);)
272 
276  void set_reg_min(const double regmin);
277  DEPRECATED("Use set_reg_min()", void set_regmin(const double regmin));
278 
282  void set_reg_max(const double regmax);
283  DEPRECATED("Use set_reg_max()", void set_regmax(const double regmax));
284 
288  void set_alphas(const std::vector<double>& alphas);
289 
293  void set_th_stepdec(const double th_step);
294 
298  void set_th_stepinc(const double th_step);
299 
303  void set_th_grad(const double th_grad);
304 
308  void set_th_gaptol(const double th_gaptol);
309 
310  protected:
311  double reg_incfactor_;
312  double reg_decfactor_;
313  double reg_min_;
314  double reg_max_;
315 
316  double cost_try_;
317  std::vector<Eigen::VectorXd> xs_try_;
318  std::vector<Eigen::VectorXd> us_try_;
319  std::vector<Eigen::VectorXd> dx_;
320 
321  // allocate data
322  std::vector<Eigen::MatrixXd> Vxx_;
323  Eigen::MatrixXd Vxx_tmp_;
324  std::vector<Eigen::VectorXd> Vx_;
325  std::vector<Eigen::MatrixXd> Qxx_;
326  std::vector<Eigen::MatrixXd> Qxu_;
327  std::vector<Eigen::MatrixXd> Quu_;
328  std::vector<Eigen::VectorXd> Qx_;
329  std::vector<Eigen::VectorXd> Qu_;
330  std::vector<MatrixXdRowMajor> K_;
331  std::vector<Eigen::VectorXd> k_;
332  std::vector<Eigen::VectorXd> fs_;
333 
334  Eigen::VectorXd xnext_;
335  MatrixXdRowMajor FxTVxx_p_;
336  std::vector<MatrixXdRowMajor> FuTVxx_p_;
337  Eigen::VectorXd fTVxx_p_;
338  std::vector<Eigen::LLT<Eigen::MatrixXd> > Quu_llt_;
339  std::vector<Eigen::VectorXd> Quuk_;
340  std::vector<double> alphas_;
341  double th_grad_;
342  double th_gaptol_;
343  double th_stepdec_;
344  double th_stepinc_;
346 };
347 
348 } // namespace crocoddyl
349 
350 #endif // CROCODDYL_CORE_SOLVERS_DDP_HPP_
double reg_max_
Maximum allowed regularization value.
Definition: ddp.hpp:314
std::vector< Eigen::VectorXd > fs_
Gaps/defects between shooting nodes.
Definition: ddp.hpp:332
double get_reg_max() const
Return the maximum regularization value.
Definition: ddp.cpp:449
void set_reg_max(const double regmax)
Modify the maximum regularization value.
Definition: ddp.cpp:524
SolverDDP(boost::shared_ptr< ShootingProblem > problem)
Initialize the DDP solver.
Definition: ddp.cpp:20
virtual void backwardPass()
Run the backward pass (Riccati sweep)
Definition: ddp.cpp:212
double get_th_stepdec() const
Return the step-length threshold used to decrease regularization.
Definition: ddp.cpp:455
void set_th_stepinc(const double th_step)
Modify the step-length threshold used to increase regularization.
Definition: ddp.cpp:568
void set_th_gaptol(const double th_gaptol)
Modify the threshold for accepting a gap as non-zero.
Definition: ddp.cpp:584
std::vector< MatrixXdRowMajor > FuTVxx_p_
fuTVxx_p_
Definition: ddp.hpp:336
const std::vector< Eigen::VectorXd > & get_k() const
Return the feedforward gains .
Definition: ddp.cpp:479
const std::vector< Eigen::VectorXd > & get_Qx() const
Return the Jacobian of the Hamiltonian function .
Definition: ddp.cpp:473
const std::vector< Eigen::VectorXd > & get_Vx() const
Return the Hessian of the Value function .
Definition: ddp.cpp:465
double th_gaptol_
Threshold limit to check non-zero gaps.
Definition: ddp.hpp:342
std::vector< MatrixXdRowMajor > K_
Feedback gains.
Definition: ddp.hpp:330
const std::vector< Eigen::MatrixXd > & get_Qxu() const
Return the Hessian of the Hamiltonian function .
Definition: ddp.cpp:469
virtual void allocateData()
Allocate all the internal data needed for the solver.
Definition: ddp.cpp:380
std::vector< Eigen::VectorXd > xs_try_
State trajectory computed by line-search procedure.
Definition: ddp.hpp:317
double th_stepdec_
Step-length threshold used to decrease regularization.
Definition: ddp.hpp:343
const std::vector< MatrixXdRowMajor > & get_K() const
Return the feedback gains .
Definition: ddp.cpp:477
const std::vector< Eigen::MatrixXd > & get_Quu() const
Return the Hessian of the Hamiltonian function .
Definition: ddp.cpp:471
double reg_min_
Minimum allowed regularization value.
Definition: ddp.hpp:313
const std::vector< double > & get_alphas() const
Return the set of step lengths using by the line-search procedure.
Definition: ddp.cpp:453
double get_th_grad() const
Return the tolerance of the expected gradient used for testing the step.
Definition: ddp.cpp:459
Abstract class for optimal control solvers.
Definition: solver-base.hpp:50
std::vector< Eigen::VectorXd > k_
Feed-forward terms.
Definition: ddp.hpp:331
const std::vector< Eigen::MatrixXd > & get_Qxx() const
Return the Hessian of the Hamiltonian function .
Definition: ddp.cpp:467
DEPRECATED("Use get_reg_incfactor() or get_reg_decfactor()", double get_regfactor() const ;) double get_reg_min() const
Return the regularization factor used to decrease / increase it.
std::vector< Eigen::VectorXd > Vx_
Gradient of the Value function.
Definition: ddp.hpp:324
void increaseRegularization()
Increase the state and control regularization values by a regfactor_ factor.
Definition: ddp.cpp:364
virtual double stoppingCriteria()
Return a positive value that quantifies the algorithm termination.
Definition: ddp.cpp:141
Eigen::VectorXd fTVxx_p_
fTVxx_p term
Definition: ddp.hpp:337
double reg_incfactor_
Regularization factor used to increase the damping value.
Definition: ddp.hpp:311
Eigen::VectorXd xnext_
Next state.
Definition: ddp.hpp:334
std::vector< Eigen::MatrixXd > Qxx_
Hessian of the Hamiltonian.
Definition: ddp.hpp:325
void set_reg_decfactor(const double reg_factor)
Modify the regularization factor used to decrease the damping value.
Definition: ddp.cpp:491
void decreaseRegularization()
Decrease the state and control regularization values by a regfactor_ factor.
Definition: ddp.cpp:372
void set_alphas(const std::vector< double > &alphas)
Modify the set of step lengths using by the line-search procedure.
Definition: ddp.cpp:540
double get_reg_incfactor() const
Return the regularization factor used to increase the damping value.
Definition: ddp.cpp:439
const std::vector< Eigen::VectorXd > & get_fs() const
Return the gaps .
Definition: ddp.cpp:481
Eigen::MatrixXd Vxx_tmp_
Temporary variable for ensuring symmetry of Vxx.
Definition: ddp.hpp:323
std::vector< Eigen::MatrixXd > Vxx_
Hessian of the Value function.
Definition: ddp.hpp:322
MatrixXdRowMajor FxTVxx_p_
fxTVxx_p_
Definition: ddp.hpp:335
void set_th_stepdec(const double th_step)
Modify the step-length threshold used to decrease regularization.
Definition: ddp.cpp:560
double th_stepinc_
Step-length threshold used to increase regularization.
Definition: ddp.hpp:344
double get_th_stepinc() const
Return the step-length threshold used to increase regularization.
Definition: ddp.cpp:457
bool was_feasible_
Label that indicates in the previous iterate was feasible.
Definition: ddp.hpp:345
double th_grad_
Tolerance of the expected gradient used for testing the step.
Definition: ddp.hpp:341
virtual void computeGains(const std::size_t t)
Compute the feedforward and feedback terms using a Cholesky decomposition.
Definition: ddp.cpp:339
virtual double tryStep(const double steplength=1)
Try a predefined step length and compute its cost improvement.
Definition: ddp.cpp:136
std::vector< double > alphas_
Set of step lengths using by the line-search procedure.
Definition: ddp.hpp:340
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:48
const std::vector< Eigen::VectorXd > & get_Qu() const
Return the Jacobian of the Hamiltonian function .
Definition: ddp.cpp:475
void set_reg_incfactor(const double reg_factor)
Modify the regularization factor used to increase the damping value.
Definition: ddp.cpp:483
double reg_decfactor_
Regularization factor used to decrease the damping value.
Definition: ddp.hpp:312
double get_reg_decfactor() const
Return the regularization factor used to decrease the damping value.
Definition: ddp.cpp:441
const std::vector< Eigen::MatrixXd > & get_Vxx() const
Return the Hessian of the Value function .
Definition: ddp.cpp:463
std::vector< Eigen::VectorXd > Qx_
Gradient of the Hamiltonian.
Definition: ddp.hpp:328
virtual void forwardPass(const double stepLength)
Run the forward pass or rollout.
Definition: ddp.cpp:292
double get_th_gaptol() const
Return the threshold for accepting a gap as non-zero.
Definition: ddp.cpp:461
Differential Dynamic Programming (DDP) solver.
Definition: ddp.hpp:49
std::vector< Eigen::VectorXd > Quuk_
Quuk term.
Definition: ddp.hpp:339
virtual void computeDirection(const bool recalc=true)
Compute the search direction for the current guess .
Definition: ddp.cpp:127
virtual double calcDiff()
Update the Jacobian and Hessian of the optimal control problem.
Definition: ddp.cpp:169
std::vector< Eigen::MatrixXd > Qxu_
Hessian of the Hamiltonian.
Definition: ddp.hpp:326
std::vector< Eigen::VectorXd > us_try_
Control trajectory computed by line-search procedure.
Definition: ddp.hpp:318
virtual const Eigen::Vector2d & expectedImprovement()
Return the expected improvement from a given current search direction.
Definition: ddp.cpp:155
void set_th_grad(const double th_grad)
Modify the tolerance of the expected gradient used for testing the step.
Definition: ddp.cpp:576
std::vector< Eigen::MatrixXd > Quu_
Hessian of the Hamiltonian.
Definition: ddp.hpp:327
std::vector< Eigen::VectorXd > Qu_
Gradient of the Hamiltonian.
Definition: ddp.hpp:329
std::vector< Eigen::LLT< Eigen::MatrixXd > > Quu_llt_
Cholesky LLT solver.
Definition: ddp.hpp:338
double cost_try_
Total cost computed by line-search procedure.
Definition: ddp.hpp:316