crocoddyl  1.6.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
ddp.cpp
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 #include <iostream>
10 #include "crocoddyl/core/utils/exception.hpp"
11 #include "crocoddyl/core/solvers/ddp.hpp"
12 
13 namespace crocoddyl {
14 
15 SolverDDP::SolverDDP(boost::shared_ptr<ShootingProblem> problem)
16  : SolverAbstract(problem),
17  regfactor_(10.),
18  regmin_(1e-9),
19  regmax_(1e9),
20  cost_try_(0.),
21  th_grad_(1e-12),
22  th_gaptol_(1e-16),
23  th_stepdec_(0.5),
24  th_stepinc_(0.01),
25  was_feasible_(false) {
26  allocateData();
27 
28  const std::size_t& n_alphas = 10;
29  alphas_.resize(n_alphas);
30  for (std::size_t n = 0; n < n_alphas; ++n) {
31  alphas_[n] = 1. / pow(2., static_cast<double>(n));
32  }
33  if (th_stepinc_ < alphas_[n_alphas - 1]) {
34  th_stepinc_ = alphas_[n_alphas - 1];
35  std::cerr << "Warning: th_stepinc has higher value than lowest alpha value, set to "
36  << std::to_string(alphas_[n_alphas - 1]) << std::endl;
37  }
38 }
39 
40 SolverDDP::~SolverDDP() {}
41 
42 bool SolverDDP::solve(const std::vector<Eigen::VectorXd>& init_xs, const std::vector<Eigen::VectorXd>& init_us,
43  const std::size_t& maxiter, const bool& is_feasible, const double& reginit) {
44  xs_try_[0] = problem_->get_x0(); // it is needed in case that init_xs[0] is infeasible
45  setCandidate(init_xs, init_us, is_feasible);
46 
47  if (std::isnan(reginit)) {
48  xreg_ = regmin_;
49  ureg_ = regmin_;
50  } else {
51  xreg_ = reginit;
52  ureg_ = reginit;
53  }
54  was_feasible_ = false;
55 
56  bool recalcDiff = true;
57  for (iter_ = 0; iter_ < maxiter; ++iter_) {
58  while (true) {
59  try {
60  computeDirection(recalcDiff);
61  } catch (std::exception& e) {
62  recalcDiff = false;
64  if (xreg_ == regmax_) {
65  return false;
66  } else {
67  continue;
68  }
69  }
70  break;
71  }
73 
74  // We need to recalculate the derivatives when the step length passes
75  recalcDiff = false;
76  for (std::vector<double>::const_iterator it = alphas_.begin(); it != alphas_.end(); ++it) {
77  steplength_ = *it;
78 
79  try {
81  } catch (std::exception& e) {
82  continue;
83  }
84  dVexp_ = steplength_ * (d_[0] + 0.5 * steplength_ * d_[1]);
85 
86  if (dVexp_ >= 0) { // descend direction
87  if (d_[0] < th_grad_ || !is_feasible_ || dV_ > th_acceptstep_ * dVexp_) {
90  cost_ = cost_try_;
91  recalcDiff = true;
92  break;
93  }
94  }
95  }
96 
97  if (steplength_ > th_stepdec_) {
99  }
100  if (steplength_ <= th_stepinc_) {
102  if (xreg_ == regmax_) {
103  return false;
104  }
105  }
107 
108  const std::size_t& n_callbacks = callbacks_.size();
109  for (std::size_t c = 0; c < n_callbacks; ++c) {
110  CallbackAbstract& callback = *callbacks_[c];
111  callback(*this);
112  }
113 
114  if (was_feasible_ && stop_ < th_stop_) {
115  return true;
116  }
117  }
118  return false;
119 }
120 
121 void SolverDDP::computeDirection(const bool& recalcDiff) {
122  if (recalcDiff) {
123  calcDiff();
124  }
125  backwardPass();
126 }
127 
128 double SolverDDP::tryStep(const double& steplength) {
129  forwardPass(steplength);
130  return cost_ - cost_try_;
131 }
132 
134  stop_ = 0.;
135  const std::size_t& T = this->problem_->get_T();
136  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
137  for (std::size_t t = 0; t < T; ++t) {
138  const std::size_t& nu = models[t]->get_nu();
139  if (nu != 0) {
140  stop_ += Qu_[t].head(nu).squaredNorm();
141  }
142  }
143  return stop_;
144 }
145 
146 const Eigen::Vector2d& SolverDDP::expectedImprovement() {
147  d_.fill(0);
148  const std::size_t& T = this->problem_->get_T();
149  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
150  for (std::size_t t = 0; t < T; ++t) {
151  const std::size_t& nu = models[t]->get_nu();
152  if (nu != 0) {
153  d_[0] += Qu_[t].head(nu).dot(k_[t].head(nu));
154  d_[1] -= k_[t].head(nu).dot(Quuk_[t].head(nu));
155  }
156  }
157  return d_;
158 }
159 
161  if (iter_ == 0) problem_->calc(xs_, us_);
162  cost_ = problem_->calcDiff(xs_, us_);
163 
164  if (!is_feasible_) {
165  const Eigen::VectorXd& x0 = problem_->get_x0();
166  problem_->get_runningModels()[0]->get_state()->diff(xs_[0], x0, fs_[0]);
167  bool could_be_feasible = true;
168  if (fs_[0].lpNorm<Eigen::Infinity>() >= th_gaptol_) {
169  could_be_feasible = false;
170  }
171  const std::size_t& T = problem_->get_T();
172  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
173  const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas = problem_->get_runningDatas();
174  for (std::size_t t = 0; t < T; ++t) {
175  const boost::shared_ptr<ActionModelAbstract>& model = models[t];
176  const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
177  model->get_state()->diff(xs_[t + 1], d->xnext, fs_[t + 1]);
178  if (could_be_feasible) {
179  if (fs_[t + 1].lpNorm<Eigen::Infinity>() >= th_gaptol_) {
180  could_be_feasible = false;
181  }
182  }
183  }
184  is_feasible_ = could_be_feasible;
185 
186  } else if (!was_feasible_) { // closing the gaps
187  for (std::vector<Eigen::VectorXd>::iterator it = fs_.begin(); it != fs_.end(); ++it) {
188  it->setZero();
189  }
190  }
191  return cost_;
192 }
193 
195  const boost::shared_ptr<ActionDataAbstract>& d_T = problem_->get_terminalData();
196  Vxx_.back() = d_T->Lxx;
197  Vx_.back() = d_T->Lx;
198 
199  if (!std::isnan(xreg_)) {
200  Vxx_.back().diagonal().array() += xreg_;
201  }
202 
203  if (!is_feasible_) {
204  Vx_.back().noalias() += Vxx_.back() * fs_.back();
205  }
206  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
207  const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas = problem_->get_runningDatas();
208  for (int t = static_cast<int>(problem_->get_T()) - 1; t >= 0; --t) {
209  const boost::shared_ptr<ActionModelAbstract>& m = models[t];
210  const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
211  const Eigen::MatrixXd& Vxx_p = Vxx_[t + 1];
212  const Eigen::VectorXd& Vx_p = Vx_[t + 1];
213  const std::size_t& nu = m->get_nu();
214 
215  Qxx_[t] = d->Lxx;
216  Qx_[t] = d->Lx;
217  FxTVxx_p_.noalias() = d->Fx.transpose() * Vxx_p;
218  Qxx_[t].noalias() += FxTVxx_p_ * d->Fx;
219  Qx_[t].noalias() += d->Fx.transpose() * Vx_p;
220  if (nu != 0) {
221  Qxu_[t].leftCols(nu) = d->Lxu;
222  Quu_[t].topLeftCorner(nu, nu) = d->Luu;
223  Qu_[t].head(nu) = d->Lu;
224  FuTVxx_p_[t].topRows(nu).noalias() = d->Fu.transpose() * Vxx_p;
225  Qxu_[t].leftCols(nu).noalias() += FxTVxx_p_ * d->Fu;
226  Quu_[t].topLeftCorner(nu, nu).noalias() += FuTVxx_p_[t].topRows(nu) * d->Fu;
227  Qu_[t].head(nu).noalias() += d->Fu.transpose() * Vx_p;
228 
229  if (!std::isnan(ureg_)) {
230  Quu_[t].diagonal().head(nu).array() += ureg_;
231  }
232  }
233 
234  computeGains(t);
235 
236  Vx_[t] = Qx_[t];
237  Vxx_[t] = Qxx_[t];
238  if (nu != 0) {
239  if (std::isnan(ureg_)) {
240  Vx_[t].noalias() -= K_[t].topRows(nu).transpose() * Qu_[t].head(nu);
241  } else {
242  Quuk_[t].head(nu).noalias() = Quu_[t].topLeftCorner(nu, nu) * k_[t].head(nu);
243  Vx_[t].noalias() += K_[t].topRows(nu).transpose() * Quuk_[t].head(nu);
244  Vx_[t].noalias() -= 2 * (K_[t].topRows(nu).transpose() * Qu_[t].head(nu));
245  }
246  Vxx_[t].noalias() -= Qxu_[t].leftCols(nu) * K_[t].topRows(nu);
247  }
248  Vxx_[t] = 0.5 * (Vxx_[t] + Vxx_[t].transpose()).eval();
249 
250  if (!std::isnan(xreg_)) {
251  Vxx_[t].diagonal().array() += xreg_;
252  }
253 
254  // Compute and store the Vx gradient at end of the interval (rollout state)
255  if (!is_feasible_) {
256  Vx_[t].noalias() += Vxx_[t] * fs_[t];
257  }
258 
259  if (raiseIfNaN(Vx_[t].lpNorm<Eigen::Infinity>())) {
260  throw_pretty("backward_error");
261  }
262  if (raiseIfNaN(Vxx_[t].lpNorm<Eigen::Infinity>())) {
263  throw_pretty("backward_error");
264  }
265  }
266 }
267 
268 void SolverDDP::forwardPass(const double& steplength) {
269  if (steplength > 1. || steplength < 0.) {
270  throw_pretty("Invalid argument: "
271  << "invalid step length, value is between 0. to 1.");
272  }
273  cost_try_ = 0.;
274  const std::size_t& T = problem_->get_T();
275  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
276  const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas = problem_->get_runningDatas();
277  for (std::size_t t = 0; t < T; ++t) {
278  const boost::shared_ptr<ActionModelAbstract>& m = models[t];
279  const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
280 
281  m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]);
282  if (m->get_nu() != 0) {
283  const std::size_t& nu = m->get_nu();
284 
285  us_try_[t].head(nu).noalias() = us_[t].head(nu);
286  us_try_[t].head(nu).noalias() -= k_[t].head(nu) * steplength;
287  us_try_[t].head(nu).noalias() -= K_[t].topRows(nu) * dx_[t];
288  m->calc(d, xs_try_[t], us_try_[t].head(nu));
289  } else {
290  m->calc(d, xs_try_[t]);
291  }
292  xs_try_[t + 1] = d->xnext;
293  cost_try_ += d->cost;
294 
295  if (raiseIfNaN(cost_try_)) {
296  throw_pretty("forward_error");
297  }
298  if (raiseIfNaN(xs_try_[t + 1].lpNorm<Eigen::Infinity>())) {
299  throw_pretty("forward_error");
300  }
301  }
302 
303  const boost::shared_ptr<ActionModelAbstract>& m = problem_->get_terminalModel();
304  const boost::shared_ptr<ActionDataAbstract>& d = problem_->get_terminalData();
305  m->calc(d, xs_try_.back());
306  cost_try_ += d->cost;
307 
308  if (raiseIfNaN(cost_try_)) {
309  throw_pretty("forward_error");
310  }
311 }
312 
313 void SolverDDP::computeGains(const std::size_t& t) {
314  const std::size_t& nu = problem_->get_runningModels()[t]->get_nu();
315  if (nu > 0) {
316  Quu_llt_[t].compute(Quu_[t].topLeftCorner(nu, nu));
317  const Eigen::ComputationInfo& info = Quu_llt_[t].info();
318  if (info != Eigen::Success) {
319  throw_pretty("backward_error");
320  }
321  K_[t].topRows(nu).noalias() = Qxu_[t].leftCols(nu).transpose();
322 
323  Eigen::Block<Eigen::MatrixXd> K = K_[t].topRows(nu);
324  Quu_llt_[t].solveInPlace(K);
325  k_[t].head(nu) = Qu_[t].head(nu);
326  Eigen::VectorBlock<Eigen::VectorXd, Eigen::Dynamic> k = k_[t].head(nu);
327  Quu_llt_[t].solveInPlace(k);
328  }
329 }
330 
332  xreg_ *= regfactor_;
333  if (xreg_ > regmax_) {
334  xreg_ = regmax_;
335  }
336  ureg_ = xreg_;
337 }
338 
340  xreg_ /= regfactor_;
341  if (xreg_ < regmin_) {
342  xreg_ = regmin_;
343  }
344  ureg_ = xreg_;
345 }
346 
348  const std::size_t& T = problem_->get_T();
349  Vxx_.resize(T + 1);
350  Vx_.resize(T + 1);
351  Qxx_.resize(T);
352  Qxu_.resize(T);
353  Quu_.resize(T);
354  Qx_.resize(T);
355  Qu_.resize(T);
356  K_.resize(T);
357  k_.resize(T);
358  fs_.resize(T + 1);
359 
360  xs_try_.resize(T + 1);
361  us_try_.resize(T);
362  dx_.resize(T);
363 
364  FuTVxx_p_.resize(T);
365  Quu_llt_.resize(T);
366  Quuk_.resize(T);
367 
368  const std::size_t& ndx = problem_->get_ndx();
369  const std::size_t& nu = problem_->get_nu_max();
370  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
371  for (std::size_t t = 0; t < T; ++t) {
372  const boost::shared_ptr<ActionModelAbstract>& model = models[t];
373  Vxx_[t] = Eigen::MatrixXd::Zero(ndx, ndx);
374  Vx_[t] = Eigen::VectorXd::Zero(ndx);
375  Qxx_[t] = Eigen::MatrixXd::Zero(ndx, ndx);
376  Qxu_[t] = Eigen::MatrixXd::Zero(ndx, nu);
377  Quu_[t] = Eigen::MatrixXd::Zero(nu, nu);
378  Qx_[t] = Eigen::VectorXd::Zero(ndx);
379  Qu_[t] = Eigen::VectorXd::Zero(nu);
380  K_[t] = Eigen::MatrixXd::Zero(nu, ndx);
381  k_[t] = Eigen::VectorXd::Zero(nu);
382  fs_[t] = Eigen::VectorXd::Zero(ndx);
383 
384  if (t == 0) {
385  xs_try_[t] = problem_->get_x0();
386  } else {
387  xs_try_[t] = model->get_state()->zero();
388  }
389  us_try_[t] = Eigen::VectorXd::Zero(nu);
390  dx_[t] = Eigen::VectorXd::Zero(ndx);
391 
392  FuTVxx_p_[t] = Eigen::MatrixXd::Zero(nu, ndx);
393  Quu_llt_[t] = Eigen::LLT<Eigen::MatrixXd>(model->get_nu());
394  Quuk_[t] = Eigen::VectorXd(nu);
395  }
396  Vxx_.back() = Eigen::MatrixXd::Zero(ndx, ndx);
397  Vx_.back() = Eigen::VectorXd::Zero(ndx);
398  xs_try_.back() = problem_->get_terminalModel()->get_state()->zero();
399  fs_.back() = Eigen::VectorXd::Zero(ndx);
400 
401  FxTVxx_p_ = Eigen::MatrixXd::Zero(ndx, ndx);
402  fTVxx_p_ = Eigen::VectorXd::Zero(ndx);
403 }
404 
405 const double& SolverDDP::get_regfactor() const { return regfactor_; }
406 
407 const double& SolverDDP::get_regmin() const { return regmin_; }
408 
409 const double& SolverDDP::get_regmax() const { return regmax_; }
410 
411 const std::vector<double>& SolverDDP::get_alphas() const { return alphas_; }
412 
413 const double& SolverDDP::get_th_stepdec() const { return th_stepdec_; }
414 
415 const double& SolverDDP::get_th_stepinc() const { return th_stepinc_; }
416 
417 const double& SolverDDP::get_th_grad() const { return th_grad_; }
418 
419 const double& SolverDDP::get_th_gaptol() const { return th_gaptol_; }
420 
421 const std::vector<Eigen::MatrixXd>& SolverDDP::get_Vxx() const { return Vxx_; }
422 
423 const std::vector<Eigen::VectorXd>& SolverDDP::get_Vx() const { return Vx_; }
424 
425 const std::vector<Eigen::MatrixXd>& SolverDDP::get_Qxx() const { return Qxx_; }
426 
427 const std::vector<Eigen::MatrixXd>& SolverDDP::get_Qxu() const { return Qxu_; }
428 
429 const std::vector<Eigen::MatrixXd>& SolverDDP::get_Quu() const { return Quu_; }
430 
431 const std::vector<Eigen::VectorXd>& SolverDDP::get_Qx() const { return Qx_; }
432 
433 const std::vector<Eigen::VectorXd>& SolverDDP::get_Qu() const { return Qu_; }
434 
435 const std::vector<Eigen::MatrixXd>& SolverDDP::get_K() const { return K_; }
436 
437 const std::vector<Eigen::VectorXd>& SolverDDP::get_k() const { return k_; }
438 
439 const std::vector<Eigen::VectorXd>& SolverDDP::get_fs() const { return fs_; }
440 
441 void SolverDDP::set_regfactor(const double& regfactor) {
442  if (regfactor <= 1.) {
443  throw_pretty("Invalid argument: "
444  << "regfactor value is higher than 1.");
445  }
446  regfactor_ = regfactor;
447 }
448 
449 void SolverDDP::set_regmin(const double& regmin) {
450  if (0. > regmin) {
451  throw_pretty("Invalid argument: "
452  << "regmin value has to be positive.");
453  }
454  regmin_ = regmin;
455 }
456 
457 void SolverDDP::set_regmax(const double& regmax) {
458  if (0. > regmax) {
459  throw_pretty("Invalid argument: "
460  << "regmax value has to be positive.");
461  }
462  regmax_ = regmax;
463 }
464 
465 void SolverDDP::set_alphas(const std::vector<double>& alphas) {
466  double prev_alpha = alphas[0];
467  if (prev_alpha != 1.) {
468  std::cerr << "Warning: alpha[0] should be 1" << std::endl;
469  }
470  for (std::size_t i = 1; i < alphas.size(); ++i) {
471  double alpha = alphas[i];
472  if (0. >= alpha) {
473  throw_pretty("Invalid argument: "
474  << "alpha values has to be positive.");
475  }
476  if (alpha >= prev_alpha) {
477  throw_pretty("Invalid argument: "
478  << "alpha values are monotonously decreasing.");
479  }
480  prev_alpha = alpha;
481  }
482  alphas_ = alphas;
483 }
484 
485 void SolverDDP::set_th_stepdec(const double& th_stepdec) {
486  if (0. >= th_stepdec || th_stepdec > 1.) {
487  throw_pretty("Invalid argument: "
488  << "th_stepdec value should between 0 and 1.");
489  }
490  th_stepdec_ = th_stepdec;
491 }
492 
493 void SolverDDP::set_th_stepinc(const double& th_stepinc) {
494  if (0. >= th_stepinc || th_stepinc > 1.) {
495  throw_pretty("Invalid argument: "
496  << "th_stepinc value should between 0 and 1.");
497  }
498  th_stepinc_ = th_stepinc;
499 }
500 
501 void SolverDDP::set_th_grad(const double& th_grad) {
502  if (0. > th_grad) {
503  throw_pretty("Invalid argument: "
504  << "th_grad value has to be positive.");
505  }
506  th_grad_ = th_grad;
507 }
508 
509 void SolverDDP::set_th_gaptol(const double& th_gaptol) {
510  if (0. > th_gaptol) {
511  throw_pretty("Invalid argument: "
512  << "th_gaptol value has to be positive.");
513  }
514  th_gaptol_ = th_gaptol;
515 }
516 
517 } // namespace crocoddyl
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
bool is_feasible_
Label that indicates is the iteration is feasible.
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
double ureg_
Current control regularization values.
double steplength_
Current applied step-length.
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
void setCandidate(const std::vector< Eigen::VectorXd > &xs_warm=DEFAULT_VECTOR, const std::vector< Eigen::VectorXd > &us_warm=DEFAULT_VECTOR, const bool &is_feasible=false)
Set the solver candidate warm-point values .
Definition: solver-base.cpp:42
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
double xreg_
Current state regularization value.
std::vector< boost::shared_ptr< CallbackAbstract > > callbacks_
Callback functions.
const std::vector< Eigen::MatrixXd > & get_Quu() const
Return the Hessian of the Hamiltonian function .
Definition: ddp.cpp:429
double th_acceptstep_
Threshold used for accepting step.
std::vector< Eigen::VectorXd > us_
Control trajectory.
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
double dV_
Cost reduction obtained by tryStep()
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::Vector2d d_
LQ approximation of the expected improvement.
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
boost::shared_ptr< ShootingProblem > problem_
optimal control problem
std::vector< Eigen::MatrixXd > Qxx_
Hessian of the Hamiltonian.
Definition: ddp.hpp:296
double dVexp_
Expected cost reduction.
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
std::vector< Eigen::VectorXd > xs_
State trajectory.
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
double cost_
Total cost.
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
double th_stop_
Tolerance for stopping the algorithm.
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
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
Abstract class for solver callbacks.
virtual double tryStep(const double &steplength=1)
Try a predefined step length and compute its cost improvement.
Definition: ddp.cpp:128
std::size_t iter_
Number of iteration performed by the solver.
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
double stop_
Value computed by stoppingCriteria()
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