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) {
44 setCandidate(init_xs, init_us, is_feasible);
46 if (std::isnan(reginit)) {
55 bool recalcDiff =
true;
59 computeDirection(recalcDiff);
60 }
catch (std::exception& e) {
62 increaseRegularization();
71 expectedImprovement();
75 for (std::vector<double>::const_iterator it =
alphas_.begin(); it !=
alphas_.end(); ++it) {
80 }
catch (std::exception& e) {
97 decreaseRegularization();
100 increaseRegularization();
107 const std::size_t& n_callbacks =
callbacks_.size();
108 for (std::size_t c = 0; c < n_callbacks; ++c) {
120 void SolverDDP::computeDirection(
const bool& recalcDiff) {
127 double SolverDDP::tryStep(
const double& steplength) {
128 forwardPass(steplength);
132 double SolverDDP::stoppingCriteria() {
134 const std::size_t& T = this->
problem_->get_T();
135 for (std::size_t t = 0; t < T; ++t) {
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) {
146 d_[1] -=
k_[t].dot(Quuk_[t]);
151 double SolverDDP::calcDiff() {
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]);
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_)) {
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_)) {
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]);
221 if (!std::isnan(
xreg_)) {
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]);
252 us_try_[t].noalias() -=
k_[t] * steplength;
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();
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() {
298 void SolverDDP::decreaseRegularization() {
306 void SolverDDP::allocateData() {
307 const std::size_t& T =
problem_->get_T();
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);
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);
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.");
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.");
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.");
460 void SolverDDP::set_th_grad(
const double& th_grad) {
462 throw_pretty(
"Invalid argument: "
463 <<
"th_grad value has to be positive.");