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