crocoddyl  1.9.0 Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
ddp.hpp
3 //
4 // Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh, University of Oxford
5 // Copyright note valid unless otherwise stated in individual files.
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  virtual void resizeData();
71
80  virtual double calcDiff();
81
109  virtual void backwardPass();
110
125  virtual void forwardPass(const double stepLength);
126
140  virtual void computeGains(const std::size_t t);
141
145  void increaseRegularization();
146
150  void decreaseRegularization();
151
155  virtual void allocateData();
156
160  double get_reg_incfactor() const;
161
165  double get_reg_decfactor() const;
166
170  DEPRECATED("Use get_reg_incfactor() or get_reg_decfactor()", double get_regfactor() const;)
171
175  double get_reg_min() const;
176  DEPRECATED("Use get_reg_min()", double get_regmin() const);
177
181  double get_reg_max() const;
182  DEPRECATED("Use get_reg_max()", double get_regmax() const);
183
187  const std::vector<double>& get_alphas() const;
188
192  double get_th_stepdec() const;
193
197  double get_th_stepinc() const;
198
203
207  const std::vector<Eigen::MatrixXd>& get_Vxx() const;
208
212  const std::vector<Eigen::VectorXd>& get_Vx() const;
213
217  const std::vector<Eigen::MatrixXd>& get_Qxx() const;
218
222  const std::vector<Eigen::MatrixXd>& get_Qxu() const;
223
227  const std::vector<Eigen::MatrixXd>& get_Quu() const;
228
232  const std::vector<Eigen::VectorXd>& get_Qx() const;
233
237  const std::vector<Eigen::VectorXd>& get_Qu() const;
238
242  const std::vector<MatrixXdRowMajor>& get_K() const;
243
247  const std::vector<Eigen::VectorXd>& get_k() const;
248
252  void set_reg_incfactor(const double reg_factor);
253
257  void set_reg_decfactor(const double reg_factor);
258
262  DEPRECATED("Use set_reg_incfactor() or set_reg_decfactor()", void set_regfactor(const double reg_factor);)
263
267  void set_reg_min(const double regmin);
268  DEPRECATED("Use set_reg_min()", void set_regmin(const double regmin));
269
273  void set_reg_max(const double regmax);
274  DEPRECATED("Use set_reg_max()", void set_regmax(const double regmax));
275
279  void set_alphas(const std::vector<double>& alphas);
280
284  void set_th_stepdec(const double th_step);
285
289  void set_th_stepinc(const double th_step);
290
295
296  protected:
297  double reg_incfactor_;
298  double reg_decfactor_;
299  double reg_min_;
300  double reg_max_;
301
302  double cost_try_;
303  std::vector<Eigen::VectorXd> xs_try_;
304  std::vector<Eigen::VectorXd> us_try_;
305  std::vector<Eigen::VectorXd> dx_;
306
307  // allocate data
308  std::vector<Eigen::MatrixXd> Vxx_;
309  Eigen::MatrixXd Vxx_tmp_;
310  std::vector<Eigen::VectorXd> Vx_;
311  std::vector<Eigen::MatrixXd> Qxx_;
312  std::vector<Eigen::MatrixXd> Qxu_;
313  std::vector<Eigen::MatrixXd> Quu_;
314  std::vector<Eigen::VectorXd> Qx_;
315  std::vector<Eigen::VectorXd> Qu_;
316  std::vector<MatrixXdRowMajor> K_;
317  std::vector<Eigen::VectorXd> k_;
318
319  Eigen::VectorXd xnext_;
320  MatrixXdRowMajor FxTVxx_p_;
321  std::vector<MatrixXdRowMajor>
323  Eigen::VectorXd fTVxx_p_;
324  std::vector<Eigen::LLT<Eigen::MatrixXd> > Quu_llt_;
325  std::vector<Eigen::VectorXd> Quuk_;
326  std::vector<double> alphas_;
328  double th_stepdec_;
329  double th_stepinc_;
330 };
331
332 } // namespace crocoddyl
333
334 #endif // CROCODDYL_CORE_SOLVERS_DDP_HPP_
Tolerance of the expected gradient used for testing the step.
Definition: ddp.hpp:327
crocoddyl::SolverDDP::computeGains
virtual void computeGains(const std::size_t t)
Compute the feedforward and feedback terms using a Cholesky decomposition.
Definition: ddp.cpp:337
crocoddyl::SolverDDP::fTVxx_p_
Eigen::VectorXd fTVxx_p_
Store the value of .
Definition: ddp.hpp:323
crocoddyl::SolverDDP::set_th_stepdec
void set_th_stepdec(const double th_step)
Modify the step-length threshold used to decrease regularization.
Definition: ddp.cpp:549
crocoddyl::SolverDDP::set_reg_incfactor
void set_reg_incfactor(const double reg_factor)
Modify the regularization factor used to increase the damping value.
Definition: ddp.cpp:472
crocoddyl::SolverDDP::get_Qu
const std::vector< Eigen::VectorXd > & get_Qu() const
Return the Jacobian of the Hamiltonian function .
Definition: ddp.cpp:466
crocoddyl::SolverDDP::th_stepinc_
double th_stepinc_
Step-length threshold used to increase regularization.
Definition: ddp.hpp:329
crocoddyl::SolverDDP::Vxx_tmp_
Eigen::MatrixXd Vxx_tmp_
Temporary variable for ensuring symmetry of Vxx.
Definition: ddp.hpp:309
crocoddyl::MathBaseTpl
Definition: mathbase.hpp:18
crocoddyl::SolverDDP::set_reg_decfactor
void set_reg_decfactor(const double reg_factor)
Modify the regularization factor used to decrease the damping value.
Definition: ddp.cpp:480
crocoddyl::SolverDDP::set_alphas
void set_alphas(const std::vector< double > &alphas)
Modify the set of step lengths using by the line-search procedure.
Definition: ddp.cpp:529
crocoddyl::SolverDDP::get_reg_incfactor
double get_reg_incfactor() const
Return the regularization factor used to increase the damping value.
Definition: ddp.cpp:432
crocoddyl::SolverAbstract
Abstract class for optimal control solvers.
Definition: solver-base.hpp:51
crocoddyl::SolverDDP::get_Vxx
const std::vector< Eigen::MatrixXd > & get_Vxx() const
Return the Hessian of the Value function .
Definition: ddp.cpp:454
crocoddyl::SolverDDP::forwardPass
virtual void forwardPass(const double stepLength)
Run the forward pass or rollout.
Definition: ddp.cpp:289
crocoddyl::SolverDDP::calcDiff
virtual double calcDiff()
Update the Jacobian, Hessian and feasibility of the optimal control problem.
Definition: ddp.cpp:194
crocoddyl::SolverDDP::alphas_
std::vector< double > alphas_
Set of step lengths using by the line-search procedure.
Definition: ddp.hpp:326
crocoddyl::SolverDDP::FxTVxx_p_
MatrixXdRowMajor FxTVxx_p_
Store the value of .
Definition: ddp.hpp:320
crocoddyl::SolverDDP::solve
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:42
crocoddyl::SolverDDP::get_th_stepinc
double get_th_stepinc() const
Return the step-length threshold used to increase regularization.
Definition: ddp.cpp:450
crocoddyl::SolverDDP::reg_decfactor_
double reg_decfactor_
Regularization factor used to decrease the damping value.
Definition: ddp.hpp:298
crocoddyl::SolverDDP::get_reg_decfactor
double get_reg_decfactor() const
Return the regularization factor used to decrease the damping value.
Definition: ddp.cpp:434
crocoddyl::SolverDDP::dx_
std::vector< Eigen::VectorXd > dx_
State error during the roll-out/forward-pass (size T)
Definition: ddp.hpp:305
crocoddyl::SolverDDP::Quuk_
std::vector< Eigen::VectorXd > Quuk_
Store the values of.
Definition: ddp.hpp:325
crocoddyl::SolverDDP::tryStep
virtual double tryStep(const double steplength=1)
Try a predefined step length and compute its cost improvement .
Definition: ddp.cpp:137
crocoddyl::SolverDDP::Qu_
std::vector< Eigen::VectorXd > Qu_
Definition: ddp.hpp:315
crocoddyl::SolverDDP::Qxu_
std::vector< Eigen::MatrixXd > Qxu_
Hessian of the Hamiltonian .
Definition: ddp.hpp:312
crocoddyl::SolverDDP::Qx_
std::vector< Eigen::VectorXd > Qx_
Definition: ddp.hpp:314
crocoddyl::SolverDDP::Quu_
std::vector< Eigen::MatrixXd > Quu_
Hessian of the Hamiltonian .
Definition: ddp.hpp:313
crocoddyl::SolverDDP::cost_try_
double cost_try_
Total cost computed by line-search procedure.
Definition: ddp.hpp:302
crocoddyl::SolverDDP::backwardPass
virtual void backwardPass()
Run the backward pass (Riccati sweep)
Definition: ddp.cpp:206
crocoddyl::SolverDDP::reg_max_
double reg_max_
Maximum allowed regularization value.
Definition: ddp.hpp:300
crocoddyl::SolverDDP::computeDirection
virtual void computeDirection(const bool recalc=true)
Compute the search direction for the current guess .
Definition: ddp.cpp:128
crocoddyl::SolverDDP::set_th_stepinc
void set_th_stepinc(const double th_step)
Modify the step-length threshold used to increase regularization.
Definition: ddp.cpp:557
crocoddyl::SolverDDP::us_try_
std::vector< Eigen::VectorXd > us_try_
Control trajectory computed by line-search procedure.
Definition: ddp.hpp:304
crocoddyl::SolverDDP::expectedImprovement
virtual const Eigen::Vector2d & expectedImprovement()
Return the expected improvement from a given current search direction .
Definition: ddp.cpp:158
crocoddyl::SolverDDP::SolverDDP
SolverDDP(boost::shared_ptr< ShootingProblem > problem)
Initialize the DDP solver.
Definition: ddp.cpp:16
crocoddyl::SolverDDP::get_reg_max
double get_reg_max() const
Return the maximum regularization value.
Definition: ddp.cpp:442
crocoddyl::SolverDDP::set_reg_max
void set_reg_max(const double regmax)
Modify the maximum regularization value.
Definition: ddp.cpp:513
crocoddyl::SolverDDP::K_
std::vector< MatrixXdRowMajor > K_
Feedback gains .
Definition: ddp.hpp:316
crocoddyl::SolverDDP
Differential Dynamic Programming (DDP) solver.
Definition: ddp.hpp:49
Modify the tolerance of the expected gradient used for testing the step.
Definition: ddp.cpp:565
crocoddyl::SolverDDP::Quu_llt_
std::vector< Eigen::LLT< Eigen::MatrixXd > > Quu_llt_
Cholesky LLT solver.
Definition: ddp.hpp:324
crocoddyl::SolverDDP::get_Qx
const std::vector< Eigen::VectorXd > & get_Qx() const
Return the Jacobian of the Hamiltonian function .
Definition: ddp.cpp:464
crocoddyl::SolverDDP::get_th_stepdec
double get_th_stepdec() const
Return the step-length threshold used to decrease regularization.
Definition: ddp.cpp:448
crocoddyl::SolverDDP::DEPRECATED
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.
crocoddyl::SolverDDP::resizeData
virtual void resizeData()
Resizing the solver data.
Definition: ddp.cpp:172
crocoddyl::SolverDDP::get_alphas
const std::vector< double > & get_alphas() const
Return the set of step lengths using by the line-search procedure.
Definition: ddp.cpp:446
crocoddyl::SolverDDP::reg_min_
double reg_min_
Minimum allowed regularization value.
Definition: ddp.hpp:299
crocoddyl::SolverDDP::xs_try_
std::vector< Eigen::VectorXd > xs_try_
State trajectory computed by line-search procedure.
Definition: ddp.hpp:303
crocoddyl::SolverDDP::allocateData
virtual void allocateData()
Allocate all the internal data needed for the solver.
Definition: ddp.cpp:376
crocoddyl::SolverDDP::FuTVxx_p_
std::vector< MatrixXdRowMajor > FuTVxx_p_
Store the values of per each running node.
Definition: ddp.hpp:322
crocoddyl::SolverDDP::Vx_
std::vector< Eigen::VectorXd > Vx_
Gradient of the Value function .
Definition: ddp.hpp:310
crocoddyl::SolverDDP::increaseRegularization
void increaseRegularization()
Increase the state and control regularization values by a regfactor_ factor.
Definition: ddp.cpp:360
crocoddyl::SolverDDP::get_k
const std::vector< Eigen::VectorXd > & get_k() const
Return the feedforward gains .
Definition: ddp.cpp:470
crocoddyl::SolverDDP::get_Vx
const std::vector< Eigen::VectorXd > & get_Vx() const
Return the Hessian of the Value function .
Definition: ddp.cpp:456
crocoddyl::SolverDDP::get_Quu
const std::vector< Eigen::MatrixXd > & get_Quu() const
Return the Hessian of the Hamiltonian function .
Definition: ddp.cpp:462
crocoddyl::SolverDDP::xnext_
Eigen::VectorXd xnext_
Next state .
Definition: ddp.hpp:319
crocoddyl::SolverDDP::decreaseRegularization
void decreaseRegularization()
Decrease the state and control regularization values by a regfactor_ factor.
Definition: ddp.cpp:368
Return the tolerance of the expected gradient used for testing the step.
Definition: ddp.cpp:452
crocoddyl::SolverDDP::get_Qxu
const std::vector< Eigen::MatrixXd > & get_Qxu() const
Return the Hessian of the Hamiltonian function .
Definition: ddp.cpp:460
crocoddyl::SolverDDP::Qxx_
std::vector< Eigen::MatrixXd > Qxx_
Hessian of the Hamiltonian .
Definition: ddp.hpp:311
crocoddyl::SolverDDP::th_stepdec_
double th_stepdec_
Step-length threshold used to decrease regularization.
Definition: ddp.hpp:328
crocoddyl::SolverDDP::get_Qxx
const std::vector< Eigen::MatrixXd > & get_Qxx() const
Return the Hessian of the Hamiltonian function .
Definition: ddp.cpp:458
crocoddyl::SolverDDP::Vxx_
std::vector< Eigen::MatrixXd > Vxx_
Hessian of the Value function .
Definition: ddp.hpp:308
crocoddyl::SolverDDP::stoppingCriteria
virtual double stoppingCriteria()
Return a positive value that quantifies the algorithm termination.
Definition: ddp.cpp:144
crocoddyl::SolverDDP::get_K
const std::vector< MatrixXdRowMajor > & get_K() const
Return the feedback gains .
Definition: ddp.cpp:468
crocoddyl::SolverDDP::reg_incfactor_
double reg_incfactor_
Regularization factor used to increase the damping value.
Definition: ddp.hpp:297
crocoddyl::SolverDDP::k_
std::vector< Eigen::VectorXd > k_
Feed-forward terms .
Definition: ddp.hpp:317