crocoddyl  1.8.0
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  if (std::isnan(ureg_)) {
265  Vx_[t].noalias() -= K_[t].topRows(nu).transpose() * Qu_[t].head(nu);
266  } else {
267  Quuk_[t].head(nu).noalias() = Quu_[t].topLeftCorner(nu, nu) * k_[t].head(nu);
268  Vx_[t].noalias() += K_[t].topRows(nu).transpose() * Quuk_[t].head(nu);
269  Vx_[t].noalias() -= 2 * (K_[t].topRows(nu).transpose() * Qu_[t].head(nu));
270  }
271  START_PROFILER("SolverDDP::Vxx");
272  Vxx_[t].noalias() -= Qxu_[t].leftCols(nu) * K_[t].topRows(nu);
273  STOP_PROFILER("SolverDDP::Vxx");
274  }
275  Vxx_tmp_ = 0.5 * (Vxx_[t] + Vxx_[t].transpose());
276  Vxx_[t] = Vxx_tmp_;
277 
278  if (!std::isnan(xreg_)) {
279  Vxx_[t].diagonal().array() += xreg_;
280  }
281 
282  // Compute and store the Vx gradient at end of the interval (rollout state)
283  if (!is_feasible_) {
284  Vx_[t].noalias() += Vxx_[t] * fs_[t];
285  }
286 
287  if (raiseIfNaN(Vx_[t].lpNorm<Eigen::Infinity>())) {
288  throw_pretty("backward_error");
289  }
290  if (raiseIfNaN(Vxx_[t].lpNorm<Eigen::Infinity>())) {
291  throw_pretty("backward_error");
292  }
293  }
294  STOP_PROFILER("SolverDDP::backwardPass");
295 }
296 
297 void SolverDDP::forwardPass(const double steplength) {
298  START_PROFILER("SolverDDP::forwardPass");
299  if (steplength > 1. || steplength < 0.) {
300  throw_pretty("Invalid argument: "
301  << "invalid step length, value is between 0. to 1.");
302  }
303  cost_try_ = 0.;
304  const std::size_t T = problem_->get_T();
305  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
306  const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas = problem_->get_runningDatas();
307  for (std::size_t t = 0; t < T; ++t) {
308  const boost::shared_ptr<ActionModelAbstract>& m = models[t];
309  const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
310 
311  m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]);
312  if (m->get_nu() != 0) {
313  const std::size_t nu = m->get_nu();
314 
315  us_try_[t].head(nu).noalias() = us_[t].head(nu);
316  us_try_[t].head(nu).noalias() -= k_[t].head(nu) * steplength;
317  us_try_[t].head(nu).noalias() -= K_[t].topRows(nu) * dx_[t];
318  m->calc(d, xs_try_[t], us_try_[t].head(nu));
319  } else {
320  m->calc(d, xs_try_[t]);
321  }
322  xs_try_[t + 1] = d->xnext;
323  cost_try_ += d->cost;
324 
325  if (raiseIfNaN(cost_try_)) {
326  throw_pretty("forward_error");
327  }
328  if (raiseIfNaN(xs_try_[t + 1].lpNorm<Eigen::Infinity>())) {
329  throw_pretty("forward_error");
330  }
331  }
332 
333  const boost::shared_ptr<ActionModelAbstract>& m = problem_->get_terminalModel();
334  const boost::shared_ptr<ActionDataAbstract>& d = problem_->get_terminalData();
335  m->calc(d, xs_try_.back());
336  cost_try_ += d->cost;
337 
338  if (raiseIfNaN(cost_try_)) {
339  throw_pretty("forward_error");
340  }
341  STOP_PROFILER("SolverDDP::forwardPass");
342 }
343 
344 void SolverDDP::computeGains(const std::size_t t) {
345  START_PROFILER("SolverDDP::computeGains");
346  const std::size_t nu = problem_->get_runningModels()[t]->get_nu();
347  if (nu > 0) {
348  START_PROFILER("SolverDDP::Quu_inv");
349  Quu_llt_[t].compute(Quu_[t].topLeftCorner(nu, nu));
350  STOP_PROFILER("SolverDDP::Quu_inv");
351  const Eigen::ComputationInfo& info = Quu_llt_[t].info();
352  if (info != Eigen::Success) {
353  throw_pretty("backward_error");
354  }
355  K_[t].topRows(nu) = Qxu_[t].leftCols(nu).transpose();
356 
357  Eigen::Block<MatrixXdRowMajor, Eigen::Dynamic, Eigen::internal::traits<MatrixXdRowMajor>::ColsAtCompileTime, true>
358  K = K_[t].topRows(nu);
359  START_PROFILER("SolverDDP::Quu_inv_Qux");
360  Quu_llt_[t].solveInPlace(K);
361  STOP_PROFILER("SolverDDP::Quu_inv_Qux");
362  k_[t].head(nu) = Qu_[t].head(nu);
363  Eigen::VectorBlock<Eigen::VectorXd, Eigen::Dynamic> k = k_[t].head(nu);
364  Quu_llt_[t].solveInPlace(k);
365  }
366  STOP_PROFILER("SolverDDP::computeGains");
367 }
368 
371  if (xreg_ > reg_max_) {
372  xreg_ = reg_max_;
373  }
374  ureg_ = xreg_;
375 }
376 
379  if (xreg_ < reg_min_) {
380  xreg_ = reg_min_;
381  }
382  ureg_ = xreg_;
383 }
384 
386  const std::size_t T = problem_->get_T();
387  Vxx_.resize(T + 1);
388  Vx_.resize(T + 1);
389  Qxx_.resize(T);
390  Qxu_.resize(T);
391  Quu_.resize(T);
392  Qx_.resize(T);
393  Qu_.resize(T);
394  K_.resize(T);
395  k_.resize(T);
396  fs_.resize(T + 1);
397 
398  xs_try_.resize(T + 1);
399  us_try_.resize(T);
400  dx_.resize(T);
401 
402  FuTVxx_p_.resize(T);
403  Quu_llt_.resize(T);
404  Quuk_.resize(T);
405 
406  const std::size_t ndx = problem_->get_ndx();
407  const std::size_t nu = problem_->get_nu_max();
408  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = problem_->get_runningModels();
409  for (std::size_t t = 0; t < T; ++t) {
410  const boost::shared_ptr<ActionModelAbstract>& model = models[t];
411  Vxx_[t] = Eigen::MatrixXd::Zero(ndx, ndx);
412  Vx_[t] = Eigen::VectorXd::Zero(ndx);
413  Qxx_[t] = Eigen::MatrixXd::Zero(ndx, ndx);
414  Qxu_[t] = Eigen::MatrixXd::Zero(ndx, nu);
415  Quu_[t] = Eigen::MatrixXd::Zero(nu, nu);
416  Qx_[t] = Eigen::VectorXd::Zero(ndx);
417  Qu_[t] = Eigen::VectorXd::Zero(nu);
418  K_[t] = MatrixXdRowMajor::Zero(nu, ndx);
419  k_[t] = Eigen::VectorXd::Zero(nu);
420  fs_[t] = Eigen::VectorXd::Zero(ndx);
421 
422  if (t == 0) {
423  xs_try_[t] = problem_->get_x0();
424  } else {
425  xs_try_[t] = model->get_state()->zero();
426  }
427  us_try_[t] = Eigen::VectorXd::Zero(nu);
428  dx_[t] = Eigen::VectorXd::Zero(ndx);
429 
430  FuTVxx_p_[t] = MatrixXdRowMajor::Zero(nu, ndx);
431  Quu_llt_[t] = Eigen::LLT<Eigen::MatrixXd>(model->get_nu());
432  Quuk_[t] = Eigen::VectorXd(nu);
433  }
434  Vxx_.back() = Eigen::MatrixXd::Zero(ndx, ndx);
435  Vxx_tmp_ = Eigen::MatrixXd::Zero(ndx, ndx);
436  Vx_.back() = Eigen::VectorXd::Zero(ndx);
437  xs_try_.back() = problem_->get_terminalModel()->get_state()->zero();
438  fs_.back() = Eigen::VectorXd::Zero(ndx);
439 
440  FxTVxx_p_ = MatrixXdRowMajor::Zero(ndx, ndx);
441  fTVxx_p_ = Eigen::VectorXd::Zero(ndx);
442 }
443 
445 
447 
448 double SolverDDP::get_regfactor() const { return reg_incfactor_; }
449 
450 double SolverDDP::get_reg_min() const { return reg_min_; }
451 
452 double SolverDDP::get_regmin() const { return reg_min_; }
453 
454 double SolverDDP::get_reg_max() const { return reg_max_; }
455 
456 double SolverDDP::get_regmax() const { return reg_max_; }
457 
458 const std::vector<double>& SolverDDP::get_alphas() const { return alphas_; }
459 
460 double SolverDDP::get_th_stepdec() const { return th_stepdec_; }
461 
462 double SolverDDP::get_th_stepinc() const { return th_stepinc_; }
463 
464 double SolverDDP::get_th_grad() const { return th_grad_; }
465 
466 double SolverDDP::get_th_gaptol() const { return th_gaptol_; }
467 
468 const std::vector<Eigen::MatrixXd>& SolverDDP::get_Vxx() const { return Vxx_; }
469 
470 const std::vector<Eigen::VectorXd>& SolverDDP::get_Vx() const { return Vx_; }
471 
472 const std::vector<Eigen::MatrixXd>& SolverDDP::get_Qxx() const { return Qxx_; }
473 
474 const std::vector<Eigen::MatrixXd>& SolverDDP::get_Qxu() const { return Qxu_; }
475 
476 const std::vector<Eigen::MatrixXd>& SolverDDP::get_Quu() const { return Quu_; }
477 
478 const std::vector<Eigen::VectorXd>& SolverDDP::get_Qx() const { return Qx_; }
479 
480 const std::vector<Eigen::VectorXd>& SolverDDP::get_Qu() const { return Qu_; }
481 
482 const std::vector<typename MathBaseTpl<double>::MatrixXsRowMajor>& SolverDDP::get_K() const { return K_; }
483 
484 const std::vector<Eigen::VectorXd>& SolverDDP::get_k() const { return k_; }
485 
486 const std::vector<Eigen::VectorXd>& SolverDDP::get_fs() const { return fs_; }
487 
488 void SolverDDP::set_reg_incfactor(const double regfactor) {
489  if (regfactor <= 1.) {
490  throw_pretty("Invalid argument: "
491  << "reg_incfactor value is higher than 1.");
492  }
493  reg_incfactor_ = regfactor;
494 }
495 
496 void SolverDDP::set_reg_decfactor(const double regfactor) {
497  if (regfactor <= 1.) {
498  throw_pretty("Invalid argument: "
499  << "reg_decfactor value is higher than 1.");
500  }
501  reg_decfactor_ = regfactor;
502 }
503 
504 void SolverDDP::set_regfactor(const double regfactor) {
505  if (regfactor <= 1.) {
506  throw_pretty("Invalid argument: "
507  << "regfactor value is higher than 1.");
508  }
509  set_reg_incfactor(regfactor);
510  set_reg_decfactor(regfactor);
511 }
512 
513 void SolverDDP::set_reg_min(const double regmin) {
514  if (0. > regmin) {
515  throw_pretty("Invalid argument: "
516  << "regmin value has to be positive.");
517  }
518  reg_min_ = regmin;
519 }
520 
521 void SolverDDP::set_regmin(const double regmin) {
522  if (0. > regmin) {
523  throw_pretty("Invalid argument: "
524  << "regmin value has to be positive.");
525  }
526  reg_min_ = regmin;
527 }
528 
529 void SolverDDP::set_reg_max(const double regmax) {
530  if (0. > regmax) {
531  throw_pretty("Invalid argument: "
532  << "regmax value has to be positive.");
533  }
534  reg_max_ = regmax;
535 }
536 
537 void SolverDDP::set_regmax(const double regmax) {
538  if (0. > regmax) {
539  throw_pretty("Invalid argument: "
540  << "regmax value has to be positive.");
541  }
542  reg_max_ = regmax;
543 }
544 
545 void SolverDDP::set_alphas(const std::vector<double>& alphas) {
546  double prev_alpha = alphas[0];
547  if (prev_alpha != 1.) {
548  std::cerr << "Warning: alpha[0] should be 1" << std::endl;
549  }
550  for (std::size_t i = 1; i < alphas.size(); ++i) {
551  double alpha = alphas[i];
552  if (0. >= alpha) {
553  throw_pretty("Invalid argument: "
554  << "alpha values has to be positive.");
555  }
556  if (alpha >= prev_alpha) {
557  throw_pretty("Invalid argument: "
558  << "alpha values are monotonously decreasing.");
559  }
560  prev_alpha = alpha;
561  }
562  alphas_ = alphas;
563 }
564 
565 void SolverDDP::set_th_stepdec(const double th_stepdec) {
566  if (0. >= th_stepdec || th_stepdec > 1.) {
567  throw_pretty("Invalid argument: "
568  << "th_stepdec value should between 0 and 1.");
569  }
570  th_stepdec_ = th_stepdec;
571 }
572 
573 void SolverDDP::set_th_stepinc(const double th_stepinc) {
574  if (0. >= th_stepinc || th_stepinc > 1.) {
575  throw_pretty("Invalid argument: "
576  << "th_stepinc value should between 0 and 1.");
577  }
578  th_stepinc_ = th_stepinc;
579 }
580 
581 void SolverDDP::set_th_grad(const double th_grad) {
582  if (0. > th_grad) {
583  throw_pretty("Invalid argument: "
584  << "th_grad value has to be positive.");
585  }
586  th_grad_ = th_grad;
587 }
588 
589 void SolverDDP::set_th_gaptol(const double th_gaptol) {
590  if (0. > th_gaptol) {
591  throw_pretty("Invalid argument: "
592  << "th_gaptol value has to be positive.");
593  }
594  th_gaptol_ = th_gaptol;
595 }
596 
597 } // 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:454
void set_reg_max(const double regmax)
Modify the maximum regularization value.
Definition: ddp.cpp:529
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:460
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:573
void set_th_gaptol(const double th_gaptol)
Modify the threshold for accepting a gap as non-zero.
Definition: ddp.cpp:589
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:484
const std::vector< Eigen::VectorXd > & get_Qx() const
Return the Jacobian of the Hamiltonian function .
Definition: ddp.cpp:478
const std::vector< Eigen::VectorXd > & get_Vx() const
Return the Hessian of the Value function .
Definition: ddp.cpp:470
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:474
virtual void allocateData()
Allocate all the internal data needed for the solver.
Definition: ddp.cpp:385
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:482
const std::vector< Eigen::MatrixXd > & get_Quu() const
Return the Hessian of the Hamiltonian function .
Definition: ddp.cpp:476
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:458
double get_th_grad() const
Return the tolerance of the expected gradient used for testing the step.
Definition: ddp.cpp:464
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:472
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:369
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:496
void decreaseRegularization()
Decrease the state and control regularization values by a regfactor_ factor.
Definition: ddp.cpp:377
void set_alphas(const std::vector< double > &alphas)
Modify the set of step lengths using by the line-search procedure.
Definition: ddp.cpp:545
double get_reg_incfactor() const
Return the regularization factor used to increase the damping value.
Definition: ddp.cpp:444
std::vector< Eigen::VectorXd > xs_
State trajectory.
const std::vector< Eigen::VectorXd > & get_fs() const
Return the gaps .
Definition: ddp.cpp:486
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:565
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:462
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:344
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:480
void set_reg_incfactor(const double reg_factor)
Modify the regularization factor used to increase the damping value.
Definition: ddp.cpp:488
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:446
const std::vector< Eigen::MatrixXd > & get_Vxx() const
Return the Hessian of the Value function .
Definition: ddp.cpp:468
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:297
double get_th_gaptol() const
Return the threshold for accepting a gap as non-zero.
Definition: ddp.cpp:466
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:581
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