10 #include "crocoddyl/core/solvers/ddp.hpp"
14 #include "crocoddyl/core/utils/exception.hpp"
18 SolverDDP::SolverDDP(boost::shared_ptr<ShootingProblem> problem)
19 : SolverAbstract(problem),
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));
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 "
39 << std::to_string(alphas_[n_alphas - 1]) << std::endl;
43 SolverDDP::~SolverDDP() {}
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()) {
55 setCandidate(init_xs, init_us, is_feasible);
57 if (std::isnan(init_reg)) {
64 was_feasible_ =
false;
66 bool recalcDiff =
true;
67 for (iter_ = 0; iter_ < maxiter; ++iter_) {
70 computeDirection(recalcDiff);
71 }
catch (std::exception& e) {
73 increaseRegularization();
74 if (preg_ == reg_max_) {
82 expectedImprovement();
86 for (std::vector<double>::const_iterator it = alphas_.begin();
87 it != alphas_.end(); ++it) {
91 dV_ = tryStep(steplength_);
92 }
catch (std::exception& e) {
95 dVexp_ = steplength_ * (d_[0] + 0.5 * steplength_ * d_[1]);
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);
109 if (steplength_ > th_stepdec_) {
110 decreaseRegularization();
112 if (steplength_ <= th_stepinc_) {
113 increaseRegularization();
114 if (preg_ == reg_max_) {
115 STOP_PROFILER(
"SolverDDP::solve");
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];
127 if (was_feasible_ && stop_ < th_stop_) {
128 STOP_PROFILER(
"SolverDDP::solve");
132 STOP_PROFILER(
"SolverDDP::solve");
136 void SolverDDP::computeDirection(
const bool recalcDiff) {
137 START_PROFILER(
"SolverDDP::computeDirection");
142 STOP_PROFILER(
"SolverDDP::computeDirection");
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_;
152 double SolverDDP::stoppingCriteria() {
157 stop_ = std::abs(d_[0] + 0.5 * d_[1]);
161 const Eigen::Vector2d& SolverDDP::expectedImprovement() {
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();
169 d_[0] += Qu_[t].dot(k_[t]);
170 d_[1] -= k_[t].dot(Quuk_[t]);
176 void SolverDDP::resizeData() {
177 START_PROFILER(
"SolverDDP::resizeData");
178 SolverAbstract::resizeData();
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);
196 FuTVxx_p_[t].setZero();
199 STOP_PROFILER(
"SolverDDP::resizeData");
202 double SolverDDP::calcDiff() {
203 START_PROFILER(
"SolverDDP::calcDiff");
205 problem_->calc(xs_, us_);
207 cost_ = problem_->calcDiff(xs_, us_);
209 ffeas_ = computeDynamicFeasibility();
210 gfeas_ = computeInequalityFeasibility();
211 hfeas_ = computeEqualityFeasibility();
212 STOP_PROFILER(
"SolverDDP::calcDiff");
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;
223 if (!std::isnan(preg_)) {
224 Vxx_.back().diagonal().array() += preg_;
228 Vx_.back().noalias() += Vxx_.back() * fs_.back();
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];
240 computeActionValueFunction(t, m, d);
246 computeValueFunction(t, m);
248 if (raiseIfNaN(Vx_[t].lpNorm<Eigen::Infinity>())) {
249 throw_pretty(
"backward_error");
251 if (raiseIfNaN(Vxx_[t].lpNorm<Eigen::Infinity>())) {
252 throw_pretty(
"backward_error");
255 STOP_PROFILER(
"SolverDDP::backwardPass");
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.");
263 START_PROFILER(
"SolverDDP::forwardPass");
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];
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]);
281 m->calc(d, xs_try_[t]);
283 xs_try_[t + 1] = d->xnext;
284 cost_try_ += d->cost;
286 if (raiseIfNaN(cost_try_)) {
287 STOP_PROFILER(
"SolverDDP::forwardPass");
288 throw_pretty(
"forward_error");
290 if (raiseIfNaN(xs_try_[t + 1].lpNorm<Eigen::Infinity>())) {
291 STOP_PROFILER(
"SolverDDP::forwardPass");
292 throw_pretty(
"forward_error");
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;
302 if (raiseIfNaN(cost_try_)) {
303 STOP_PROFILER(
"SolverDDP::forwardPass");
304 throw_pretty(
"forward_error");
306 STOP_PROFILER(
"SolverDDP::forwardPass");
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];
319 FxTVxx_p_.noalias() = data->Fx.transpose() * Vxx_p;
320 START_PROFILER(
"SolverDDP::Qx");
322 Qx_[t].noalias() += data->Fx.transpose() * Vx_p;
323 STOP_PROFILER(
"SolverDDP::Qx");
324 START_PROFILER(
"SolverDDP::Qxx");
326 Qxx_[t].noalias() += FxTVxx_p_ * data->Fx;
327 STOP_PROFILER(
"SolverDDP::Qxx");
329 FuTVxx_p_[t].noalias() = data->Fu.transpose() * Vxx_p;
330 START_PROFILER(
"SolverDDP::Qu");
332 Qu_[t].noalias() += data->Fu.transpose() * Vx_p;
333 STOP_PROFILER(
"SolverDDP::Qu");
334 START_PROFILER(
"SolverDDP::Quu");
336 Quu_[t].noalias() += FuTVxx_p_[t] * data->Fu;
337 STOP_PROFILER(
"SolverDDP::Quu");
338 START_PROFILER(
"SolverDDP::Qxu");
340 Qxu_[t].noalias() += FxTVxx_p_ * data->Fu;
341 STOP_PROFILER(
"SolverDDP::Qxu");
342 if (!std::isnan(preg_)) {
343 Quu_[t].diagonal().array() += preg_;
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();
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");
365 Vxx_tmp_ = 0.5 * (Vxx_[t] + Vxx_[t].transpose());
368 if (!std::isnan(preg_)) {
369 Vxx_[t].diagonal().array() += preg_;
374 Vx_[t].noalias() += Vxx_[t] * fs_[t];
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();
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");
393 K_[t] = Qxu_[t].transpose();
395 START_PROFILER(
"SolverDDP::Quu_inv_Qux");
396 Quu_llt_[t].solveInPlace(K_[t]);
397 STOP_PROFILER(
"SolverDDP::Quu_inv_Qux");
399 Quu_llt_[t].solveInPlace(k_[t]);
401 STOP_PROFILER(
"SolverDDP::computeGains");
404 void SolverDDP::increaseRegularization() {
405 preg_ *= reg_incfactor_;
406 if (preg_ > reg_max_) {
412 void SolverDDP::decreaseRegularization() {
413 preg_ /= reg_decfactor_;
414 if (preg_ < reg_min_) {
420 void SolverDDP::allocateData() {
421 const std::size_t T = problem_->get_T();
432 xs_try_.resize(T + 1);
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);
457 xs_try_[t] = problem_->get_x0();
459 xs_try_[t] = model->get_state()->zero();
461 us_try_[t] = Eigen::VectorXd::Zero(nu);
462 dx_[t] = Eigen::VectorXd::Zero(ndx);
464 FuTVxx_p_[t] = MatrixXdRowMajor::Zero(nu, ndx);
465 Quu_llt_[t] = Eigen::LLT<Eigen::MatrixXd>(nu);
466 Quuk_[t] = Eigen::VectorXd(nu);
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();
473 FxTVxx_p_ = MatrixXdRowMajor::Zero(ndx, ndx);
474 fTVxx_p_ = Eigen::VectorXd::Zero(ndx);
477 double SolverDDP::get_reg_incfactor()
const {
return reg_incfactor_; }
479 double SolverDDP::get_reg_decfactor()
const {
return reg_decfactor_; }
481 double SolverDDP::get_regfactor()
const {
return reg_incfactor_; }
483 double SolverDDP::get_reg_min()
const {
return reg_min_; }
485 double SolverDDP::get_regmin()
const {
return reg_min_; }
487 double SolverDDP::get_reg_max()
const {
return reg_max_; }
489 double SolverDDP::get_regmax()
const {
return reg_max_; }
491 const std::vector<double>& SolverDDP::get_alphas()
const {
return alphas_; }
493 double SolverDDP::get_th_stepdec()
const {
return th_stepdec_; }
495 double SolverDDP::get_th_stepinc()
const {
return th_stepinc_; }
497 double SolverDDP::get_th_grad()
const {
return th_grad_; }
499 const std::vector<Eigen::MatrixXd>& SolverDDP::get_Vxx()
const {
return Vxx_; }
501 const std::vector<Eigen::VectorXd>& SolverDDP::get_Vx()
const {
return Vx_; }
503 const std::vector<Eigen::MatrixXd>& SolverDDP::get_Qxx()
const {
return Qxx_; }
505 const std::vector<Eigen::MatrixXd>& SolverDDP::get_Qxu()
const {
return Qxu_; }
507 const std::vector<Eigen::MatrixXd>& SolverDDP::get_Quu()
const {
return Quu_; }
509 const std::vector<Eigen::VectorXd>& SolverDDP::get_Qx()
const {
return Qx_; }
511 const std::vector<Eigen::VectorXd>& SolverDDP::get_Qu()
const {
return Qu_; }
513 const std::vector<typename MathBaseTpl<double>::MatrixXsRowMajor>&
514 SolverDDP::get_K()
const {
518 const std::vector<Eigen::VectorXd>& SolverDDP::get_k()
const {
return k_; }
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.");
525 reg_incfactor_ = regfactor;
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.");
533 reg_decfactor_ = regfactor;
536 void SolverDDP::set_regfactor(
const double regfactor) {
537 if (regfactor <= 1.) {
538 throw_pretty(
"Invalid argument: "
539 <<
"regfactor value is higher than 1.");
541 set_reg_incfactor(regfactor);
542 set_reg_decfactor(regfactor);
545 void SolverDDP::set_reg_min(
const double regmin) {
547 throw_pretty(
"Invalid argument: "
548 <<
"regmin value has to be positive.");
553 void SolverDDP::set_regmin(
const double regmin) {
555 throw_pretty(
"Invalid argument: "
556 <<
"regmin value has to be positive.");
561 void SolverDDP::set_reg_max(
const double regmax) {
563 throw_pretty(
"Invalid argument: "
564 <<
"regmax value has to be positive.");
569 void SolverDDP::set_regmax(
const double regmax) {
571 throw_pretty(
"Invalid argument: "
572 <<
"regmax value has to be positive.");
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;
582 for (std::size_t i = 1; i < alphas.size(); ++i) {
583 double alpha = alphas[i];
585 throw_pretty(
"Invalid argument: "
586 <<
"alpha values has to be positive.");
588 if (alpha >= prev_alpha) {
589 throw_pretty(
"Invalid argument: "
590 <<
"alpha values are monotonously decreasing.");
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.");
602 th_stepdec_ = th_stepdec;
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.");
610 th_stepinc_ = th_stepinc;
613 void SolverDDP::set_th_grad(
const double th_grad) {
615 throw_pretty(
"Invalid argument: "
616 <<
"th_grad value has to be positive.");