crocoddyl  1.6.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
ddp.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-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 double& get_th_gaptol() const;
192 
196  const std::vector<Eigen::MatrixXd>& get_Vxx() const;
197 
201  const std::vector<Eigen::VectorXd>& get_Vx() const;
202 
206  const std::vector<Eigen::MatrixXd>& get_Qxx() const;
207 
211  const std::vector<Eigen::MatrixXd>& get_Qxu() const;
212 
216  const std::vector<Eigen::MatrixXd>& get_Quu() const;
217 
221  const std::vector<Eigen::VectorXd>& get_Qx() const;
222 
226  const std::vector<Eigen::VectorXd>& get_Qu() const;
227 
231  const std::vector<Eigen::MatrixXd>& get_K() const;
232 
236  const std::vector<Eigen::VectorXd>& get_k() const;
237 
241  const std::vector<Eigen::VectorXd>& get_fs() const;
242 
246  void set_regfactor(const double& reg_factor);
247 
251  void set_regmin(const double& regmin);
252 
256  void set_regmax(const double& regmax);
257 
261  void set_alphas(const std::vector<double>& alphas);
262 
266  void set_th_stepdec(const double& th_step);
267 
271  void set_th_stepinc(const double& th_step);
272 
276  void set_th_grad(const double& th_grad);
277 
281  void set_th_gaptol(const double& th_gaptol);
282 
283  protected:
284  double regfactor_;
285  double regmin_;
286  double regmax_;
287 
288  double cost_try_;
289  std::vector<Eigen::VectorXd> xs_try_;
290  std::vector<Eigen::VectorXd> us_try_;
291  std::vector<Eigen::VectorXd> dx_;
292 
293  // allocate data
294  std::vector<Eigen::MatrixXd> Vxx_;
295  std::vector<Eigen::VectorXd> Vx_;
296  std::vector<Eigen::MatrixXd> Qxx_;
297  std::vector<Eigen::MatrixXd> Qxu_;
298  std::vector<Eigen::MatrixXd> Quu_;
299  std::vector<Eigen::VectorXd> Qx_;
300  std::vector<Eigen::VectorXd> Qu_;
301  std::vector<Eigen::MatrixXd> K_;
302  std::vector<Eigen::VectorXd> k_;
303  std::vector<Eigen::VectorXd> fs_;
304 
305  Eigen::VectorXd xnext_;
306  Eigen::MatrixXd FxTVxx_p_;
307  std::vector<Eigen::MatrixXd> FuTVxx_p_;
308  Eigen::VectorXd fTVxx_p_;
309  std::vector<Eigen::LLT<Eigen::MatrixXd> > Quu_llt_;
310  std::vector<Eigen::VectorXd> Quuk_;
311  std::vector<double> alphas_;
312  double th_grad_;
313  double th_gaptol_;
314  double th_stepdec_;
315  double th_stepinc_;
317 };
318 
319 } // namespace crocoddyl
320 
321 #endif // CROCODDYL_CORE_SOLVERS_DDP_HPP_
const double & get_regfactor() const
Return the regularization factor used to decrease / increase it.
Definition: ddp.cpp:405
std::vector< Eigen::VectorXd > fs_
Gaps/defects between shooting nodes.
Definition: ddp.hpp:303
void set_th_stepdec(const double &th_step)
Modify the step-length threshold used to decrease regularization.
Definition: ddp.cpp:485
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:194
const double & get_regmax() const
Return the maximum regularization value.
Definition: ddp.cpp:409
const double & get_th_grad() const
Return the tolerance of the expected gradient used for testing the step.
Definition: ddp.cpp:417
std::vector< Eigen::MatrixXd > FuTVxx_p_
fuTVxx_p_
Definition: ddp.hpp:307
const std::vector< Eigen::VectorXd > & get_k() const
Return the feedforward gains .
Definition: ddp.cpp:437
Eigen::MatrixXd FxTVxx_p_
fxTVxx_p_
Definition: ddp.hpp:306
const std::vector< Eigen::VectorXd > & get_Qx() const
Return the Jacobian of the Hamiltonian function .
Definition: ddp.cpp:431
const std::vector< Eigen::VectorXd > & get_Vx() const
Return the Hessian of the Value function .
Definition: ddp.cpp:423
double th_gaptol_
Threshold limit to check non-zero gaps.
Definition: ddp.hpp:313
const std::vector< Eigen::MatrixXd > & get_Qxu() const
Return the Hessian of the Hamiltonian function .
Definition: ddp.cpp:427
const double & get_th_stepinc() const
Return the step-length threshold used to increase regularization.
Definition: ddp.cpp:415
virtual void allocateData()
Allocate all the internal data needed for the solver.
Definition: ddp.cpp:347
std::vector< Eigen::VectorXd > xs_try_
State trajectory computed by line-search procedure.
Definition: ddp.hpp:289
double th_stepdec_
Step-length threshold used to decrease regularization.
Definition: ddp.hpp:314
const std::vector< Eigen::MatrixXd > & get_Quu() const
Return the Hessian of the Hamiltonian function .
Definition: ddp.cpp:429
const std::vector< double > & get_alphas() const
Return the set of step lengths using by the line-search procedure.
Definition: ddp.cpp:411
Abstract class for optimal control solvers.
Definition: solver-base.hpp:50
std::vector< Eigen::VectorXd > k_
Feed-forward terms.
Definition: ddp.hpp:302
const std::vector< Eigen::MatrixXd > & get_Qxx() const
Return the Hessian of the Hamiltonian function .
Definition: ddp.cpp:425
std::vector< Eigen::VectorXd > Vx_
Gradient of the Value function.
Definition: ddp.hpp:295
void increaseRegularization()
Increase the state and control regularization values by a regfactor_ factor.
Definition: ddp.cpp:331
virtual double stoppingCriteria()
Return a positive value that quantifies the algorithm termination.
Definition: ddp.cpp:133
Eigen::VectorXd fTVxx_p_
fTVxx_p term
Definition: ddp.hpp:308
void set_th_grad(const double &th_grad)
Modify the tolerance of the expected gradient used for testing the step.
Definition: ddp.cpp:501
Eigen::VectorXd xnext_
Next state.
Definition: ddp.hpp:305
std::vector< Eigen::MatrixXd > Qxx_
Hessian of the Hamiltonian.
Definition: ddp.hpp:296
virtual void forwardPass(const double &stepLength)
Run the forward pass or rollout.
Definition: ddp.cpp:268
void decreaseRegularization()
Decrease the state and control regularization values by a regfactor_ factor.
Definition: ddp.cpp:339
const double & get_th_gaptol() const
Return the threshold for accepting a gap as non-zero.
Definition: ddp.cpp:419
void set_alphas(const std::vector< double > &alphas)
Modify the set of step lengths using by the line-search procedure.
Definition: ddp.cpp:465
std::vector< Eigen::MatrixXd > K_
Feedback gains.
Definition: ddp.hpp:301
virtual void computeDirection(const bool &recalc=true)
Compute the search direction for the current guess .
Definition: ddp.cpp:121
const std::vector< Eigen::VectorXd > & get_fs() const
Return the gaps .
Definition: ddp.cpp:439
std::vector< Eigen::MatrixXd > Vxx_
Hessian of the Value function.
Definition: ddp.hpp:294
double th_stepinc_
Step-length threshold used to increase regularization.
Definition: ddp.hpp:315
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
bool was_feasible_
Label that indicates in the previous iterate was feasible.
Definition: ddp.hpp:316
double th_grad_
Tolerance of the expected gradient used for testing the step.
Definition: ddp.hpp:312
std::vector< double > alphas_
Set of step lengths using by the line-search procedure.
Definition: ddp.hpp:311
const std::vector< Eigen::VectorXd > & get_Qu() const
Return the Jacobian of the Hamiltonian function .
Definition: ddp.cpp:433
void set_th_gaptol(const double &th_gaptol)
Modify the threshold for accepting a gap as non-zero.
Definition: ddp.cpp:509
const std::vector< Eigen::MatrixXd > & get_Vxx() const
Return the Hessian of the Value function .
Definition: ddp.cpp:421
std::vector< Eigen::VectorXd > Qx_
Gradient of the Hamiltonian.
Definition: ddp.hpp:299
virtual void computeGains(const std::size_t &t)
Compute the feedforward and feedback terms using a Cholesky decomposition.
Definition: ddp.cpp:313
const std::vector< Eigen::MatrixXd > & get_K() const
Return the feedback gains .
Definition: ddp.cpp:435
double regmin_
Minimum allowed regularization value.
Definition: ddp.hpp:285
double regmax_
Maximum allowed regularization value.
Definition: ddp.hpp:286
double regfactor_
Regularization factor used to decrease / increase it.
Definition: ddp.hpp:284
Differential Dynamic Programming (DDP) solver.
Definition: ddp.hpp:48
std::vector< Eigen::VectorXd > Quuk_
Quuk term.
Definition: ddp.hpp:310
virtual double calcDiff()
Update the Jacobian and Hessian of the optimal control problem.
Definition: ddp.cpp:160
virtual double tryStep(const double &steplength=1)
Try a predefined step length and compute its cost improvement.
Definition: ddp.cpp:128
std::vector< Eigen::MatrixXd > Qxu_
Hessian of the Hamiltonian.
Definition: ddp.hpp:297
const double & get_th_stepdec() const
Return the step-length threshold used to decrease regularization.
Definition: ddp.cpp:413
void set_th_stepinc(const double &th_step)
Modify the step-length threshold used to increase regularization.
Definition: ddp.cpp:493
void set_regfactor(const double &reg_factor)
Modify the regularization factor used to decrease / increase it.
Definition: ddp.cpp:441
void set_regmin(const double &regmin)
Modify the minimum regularization value.
Definition: ddp.cpp:449
std::vector< Eigen::VectorXd > us_try_
Control trajectory computed by line-search procedure.
Definition: ddp.hpp:290
virtual const Eigen::Vector2d & expectedImprovement()
Return the expected improvement from a given current search direction.
Definition: ddp.cpp:146
void set_regmax(const double &regmax)
Modify the maximum regularization value.
Definition: ddp.cpp:457
const double & get_regmin() const
Return the minimum regularization value.
Definition: ddp.cpp:407
std::vector< Eigen::MatrixXd > Quu_
Hessian of the Hamiltonian.
Definition: ddp.hpp:298
std::vector< Eigen::VectorXd > Qu_
Gradient of the Hamiltonian.
Definition: ddp.hpp:300
std::vector< Eigen::LLT< Eigen::MatrixXd > > Quu_llt_
Cholesky LLT solver.
Definition: ddp.hpp:309
double cost_try_
Total cost computed by line-search procedure.
Definition: ddp.hpp:288