10 #include "crocoddyl/core/utils/exception.hpp" 11 #include "crocoddyl/core/solvers/ddp.hpp" 15 SolverDDP::SolverDDP(boost::shared_ptr<ShootingProblem> problem)
16 : SolverAbstract(problem),
24 was_feasible_(false) {
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));
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;
39 SolverDDP::~SolverDDP() {}
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();
44 setCandidate(init_xs, init_us, is_feasible);
46 if (std::isnan(reginit)) {
53 was_feasible_ =
false;
55 bool recalcDiff =
true;
56 for (iter_ = 0; iter_ < maxiter; ++iter_) {
59 computeDirection(recalcDiff);
60 }
catch (std::exception& e) {
62 increaseRegularization();
63 if (xreg_ == regmax_) {
71 expectedImprovement();
75 for (std::vector<double>::const_iterator it = alphas_.begin(); it != alphas_.end(); ++it) {
79 dV_ = tryStep(steplength_);
80 }
catch (std::exception& e) {
83 dVexp_ = steplength_ * (d_[0] + 0.5 * steplength_ * d_[1]);
86 if (d_[0] < th_grad_ || !is_feasible_ || dV_ > th_acceptstep_ * dVexp_) {
87 was_feasible_ = is_feasible_;
88 setCandidate(xs_try_, us_try_,
true);
96 if (steplength_ > th_stepdec_) {
97 decreaseRegularization();
99 if (steplength_ <= th_stepinc_) {
100 increaseRegularization();
101 if (xreg_ == regmax_) {
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];
113 if (was_feasible_ && stop_ < th_stop_) {
120 void SolverDDP::computeDirection(
const bool& recalcDiff) {
127 double SolverDDP::tryStep(
const double& steplength) {
128 forwardPass(steplength);
129 return cost_ - cost_try_;
132 double SolverDDP::stoppingCriteria() {
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();
141 const Eigen::Vector2d& SolverDDP::expectedImprovement() {
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]);
151 double SolverDDP::calcDiff() {
152 if (iter_ == 0) problem_->calc(xs_, us_);
153 cost_ = problem_->calcDiff(xs_, us_);
155 const Eigen::VectorXd& x0 = problem_->get_x0();
156 problem_->get_runningModels()[0]->get_state()->diff(xs_[0], x0, fs_[0]);
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]);
164 }
else if (!was_feasible_) {
165 for (std::vector<Eigen::VectorXd>::iterator it = fs_.begin(); it != fs_.end(); ++it) {
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;
177 if (!std::isnan(xreg_)) {
178 Vxx_.back().diagonal().array() += xreg_;
182 Vx_.back().noalias() += Vxx_.back() * fs_.back();
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];
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;
203 if (!std::isnan(ureg_)) {
204 Quu_[t].diagonal().array() += ureg_;
210 if (std::isnan(ureg_)) {
211 Vx_[t].noalias() -= K_[t].transpose() * Qu_[t];
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]);
218 Vxx_[t].noalias() -= Qxu_[t] * K_[t];
219 Vxx_[t] = 0.5 * (Vxx_[t] + Vxx_[t].transpose()).eval();
221 if (!std::isnan(xreg_)) {
222 Vxx_[t].diagonal().array() += xreg_;
227 Vx_[t].noalias() += Vxx_[t] * fs_[t];
230 if (raiseIfNaN(Vx_[t].lpNorm<Eigen::Infinity>())) {
231 throw_pretty(
"backward_error");
233 if (raiseIfNaN(Vxx_[t].lpNorm<Eigen::Infinity>())) {
234 throw_pretty(
"backward_error");
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.");
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];
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;
258 if (raiseIfNaN(cost_try_)) {
259 throw_pretty(
"forward_error");
261 if (raiseIfNaN(xs_try_[t + 1].lpNorm<Eigen::Infinity>())) {
262 throw_pretty(
"forward_error");
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;
271 if (raiseIfNaN(cost_try_)) {
272 throw_pretty(
"forward_error");
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");
283 K_[t] = Qxu_[t].transpose();
284 Quu_llt_[t].solveInPlace(K_[t]);
286 Quu_llt_[t].solveInPlace(k_[t]);
290 void SolverDDP::increaseRegularization() {
292 if (xreg_ > regmax_) {
298 void SolverDDP::decreaseRegularization() {
300 if (xreg_ < regmin_) {
306 void SolverDDP::allocateData() {
307 const std::size_t& T = problem_->get_T();
319 xs_try_.resize(T + 1);
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();
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);
345 xs_try_[t] = problem_->get_x0();
347 xs_try_[t] = Eigen::VectorXd::Constant(nx, NAN);
349 us_try_[t] = Eigen::VectorXd::Constant(nu, NAN);
350 dx_[t] = Eigen::VectorXd::Zero(ndx);
352 FuTVxx_p_[t] = Eigen::MatrixXd::Zero(nu, ndx);
353 Quu_llt_[t] = Eigen::LLT<Eigen::MatrixXd>(nu);
354 Quuk_[t] = Eigen::VectorXd(nu);
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);
362 FxTVxx_p_ = Eigen::MatrixXd::Zero(ndx, ndx);
363 fTVxx_p_ = Eigen::VectorXd::Zero(ndx);
366 const double& SolverDDP::get_regfactor()
const {
return regfactor_; }
368 const double& SolverDDP::get_regmin()
const {
return regmin_; }
370 const double& SolverDDP::get_regmax()
const {
return regmax_; }
372 const std::vector<double>& SolverDDP::get_alphas()
const {
return alphas_; }
374 const double& SolverDDP::get_th_stepdec()
const {
return th_stepdec_; }
376 const double& SolverDDP::get_th_stepinc()
const {
return th_stepinc_; }
378 const double& SolverDDP::get_th_grad()
const {
return th_grad_; }
380 const std::vector<Eigen::MatrixXd>& SolverDDP::get_Vxx()
const {
return Vxx_; }
382 const std::vector<Eigen::VectorXd>& SolverDDP::get_Vx()
const {
return Vx_; }
384 const std::vector<Eigen::MatrixXd>& SolverDDP::get_Qxx()
const {
return Qxx_; }
386 const std::vector<Eigen::MatrixXd>& SolverDDP::get_Qxu()
const {
return Qxu_; }
388 const std::vector<Eigen::MatrixXd>& SolverDDP::get_Quu()
const {
return Quu_; }
390 const std::vector<Eigen::VectorXd>& SolverDDP::get_Qx()
const {
return Qx_; }
392 const std::vector<Eigen::VectorXd>& SolverDDP::get_Qu()
const {
return Qu_; }
394 const std::vector<Eigen::MatrixXd>& SolverDDP::get_K()
const {
return K_; }
396 const std::vector<Eigen::VectorXd>& SolverDDP::get_k()
const {
return k_; }
398 const std::vector<Eigen::VectorXd>& SolverDDP::get_fs()
const {
return fs_; }
400 void SolverDDP::set_regfactor(
const double& regfactor) {
401 if (regfactor <= 1.) {
402 throw_pretty(
"Invalid argument: " 403 <<
"regfactor value is higher than 1.");
405 regfactor_ = regfactor;
408 void SolverDDP::set_regmin(
const double& regmin) {
410 throw_pretty(
"Invalid argument: " 411 <<
"regmin value has to be positive.");
416 void SolverDDP::set_regmax(
const double& regmax) {
418 throw_pretty(
"Invalid argument: " 419 <<
"regmax value has to be positive.");
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;
429 for (std::size_t i = 1; i < alphas.size(); ++i) {
430 double alpha = alphas[i];
432 throw_pretty(
"Invalid argument: " 433 <<
"alpha values has to be positive.");
435 if (alpha >= prev_alpha) {
436 throw_pretty(
"Invalid argument: " 437 <<
"alpha values are monotonously decreasing.");
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.");
449 th_stepdec_ = th_stepdec;
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.");
457 th_stepinc_ = th_stepinc;
460 void SolverDDP::set_th_grad(
const double& th_grad) {
462 throw_pretty(
"Invalid argument: " 463 <<
"th_grad value has to be positive.");