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