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