| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /////////////////////////////////////////////////////////////////////////////// | ||
| 2 | // BSD 3-Clause License | ||
| 3 | // | ||
| 4 | // Copyright (C) 2019-2021, CNRS-LAAS, University of Edinburgh | ||
| 5 | // Copyright note valid unless otherwise stated in individual files. | ||
| 6 | // All rights reserved. | ||
| 7 | /////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | #include "crocoddyl/core/solvers/box-ddp.hpp" | ||
| 10 | |||
| 11 | namespace crocoddyl { | ||
| 12 | |||
| 13 | ✗ | SolverBoxDDP::SolverBoxDDP(std::shared_ptr<ShootingProblem> problem) | |
| 14 | : SolverDDP(problem), | ||
| 15 | ✗ | qp_(problem->get_runningModels()[0]->get_nu(), 100, 0.1, 1e-5, 0.) { | |
| 16 | ✗ | allocateData(); | |
| 17 | |||
| 18 | ✗ | const std::size_t n_alphas = 10; | |
| 19 | ✗ | alphas_.resize(n_alphas); | |
| 20 | ✗ | for (std::size_t n = 0; n < n_alphas; ++n) { | |
| 21 | ✗ | alphas_[n] = 1. / pow(2., static_cast<double>(n)); | |
| 22 | } | ||
| 23 | // Change the default convergence tolerance since the gradient of the | ||
| 24 | // Lagrangian is smaller than an unconstrained OC problem (i.e. gradient = Qu | ||
| 25 | // - mu^T * C where mu > 0 and C defines the inequality matrix that bounds the | ||
| 26 | // control); and we don't have access to mu from the box QP. | ||
| 27 | ✗ | th_stop_ = 5e-5; | |
| 28 | ✗ | } | |
| 29 | |||
| 30 | ✗ | SolverBoxDDP::~SolverBoxDDP() {} | |
| 31 | |||
| 32 | ✗ | void SolverBoxDDP::resizeData() { | |
| 33 | ✗ | START_PROFILER("SolverBoxDDP::resizeData"); | |
| 34 | ✗ | SolverDDP::resizeData(); | |
| 35 | |||
| 36 | ✗ | const std::size_t T = problem_->get_T(); | |
| 37 | const std::vector<std::shared_ptr<ActionModelAbstract> >& models = | ||
| 38 | ✗ | problem_->get_runningModels(); | |
| 39 | ✗ | for (std::size_t t = 0; t < T; ++t) { | |
| 40 | ✗ | const std::shared_ptr<ActionModelAbstract>& model = models[t]; | |
| 41 | ✗ | const std::size_t nu = model->get_nu(); | |
| 42 | ✗ | Quu_inv_[t].conservativeResize(nu, nu); | |
| 43 | ✗ | du_lb_[t].conservativeResize(nu); | |
| 44 | ✗ | du_ub_[t].conservativeResize(nu); | |
| 45 | } | ||
| 46 | ✗ | STOP_PROFILER("SolverBoxDDP::resizeData"); | |
| 47 | ✗ | } | |
| 48 | |||
| 49 | ✗ | void SolverBoxDDP::allocateData() { | |
| 50 | ✗ | SolverDDP::allocateData(); | |
| 51 | |||
| 52 | ✗ | const std::size_t T = problem_->get_T(); | |
| 53 | ✗ | Quu_inv_.resize(T); | |
| 54 | ✗ | du_lb_.resize(T); | |
| 55 | ✗ | du_ub_.resize(T); | |
| 56 | const std::vector<std::shared_ptr<ActionModelAbstract> >& models = | ||
| 57 | ✗ | problem_->get_runningModels(); | |
| 58 | ✗ | for (std::size_t t = 0; t < T; ++t) { | |
| 59 | ✗ | const std::shared_ptr<ActionModelAbstract>& model = models[t]; | |
| 60 | ✗ | const std::size_t nu = model->get_nu(); | |
| 61 | ✗ | Quu_inv_[t] = Eigen::MatrixXd::Zero(nu, nu); | |
| 62 | ✗ | du_lb_[t] = Eigen::VectorXd::Zero(nu); | |
| 63 | ✗ | du_ub_[t] = Eigen::VectorXd::Zero(nu); | |
| 64 | } | ||
| 65 | ✗ | } | |
| 66 | |||
| 67 | ✗ | void SolverBoxDDP::computeGains(const std::size_t t) { | |
| 68 | ✗ | START_PROFILER("SolverBoxDDP::computeGains"); | |
| 69 | ✗ | const std::size_t nu = problem_->get_runningModels()[t]->get_nu(); | |
| 70 | ✗ | if (nu > 0) { | |
| 71 | ✗ | if (!problem_->get_runningModels()[t]->get_has_control_limits() || | |
| 72 | ✗ | !is_feasible_) { | |
| 73 | // No control limits on this model: Use vanilla DDP | ||
| 74 | ✗ | SolverDDP::computeGains(t); | |
| 75 | ✗ | return; | |
| 76 | } | ||
| 77 | |||
| 78 | ✗ | du_lb_[t] = problem_->get_runningModels()[t]->get_u_lb() - us_[t]; | |
| 79 | ✗ | du_ub_[t] = problem_->get_runningModels()[t]->get_u_ub() - us_[t]; | |
| 80 | |||
| 81 | ✗ | START_PROFILER("SolverBoxDDP::boxQP"); | |
| 82 | const BoxQPSolution& boxqp_sol = | ||
| 83 | ✗ | qp_.solve(Quu_[t], Qu_[t], du_lb_[t], du_ub_[t], k_[t]); | |
| 84 | ✗ | START_PROFILER("SolverBoxDDP::boxQP"); | |
| 85 | |||
| 86 | // Compute controls | ||
| 87 | ✗ | START_PROFILER("SolverBoxDDP::Quu_invproj"); | |
| 88 | ✗ | Quu_inv_[t].setZero(); | |
| 89 | ✗ | for (std::size_t i = 0; i < boxqp_sol.free_idx.size(); ++i) { | |
| 90 | ✗ | for (std::size_t j = 0; j < boxqp_sol.free_idx.size(); ++j) { | |
| 91 | ✗ | Quu_inv_[t](boxqp_sol.free_idx[i], boxqp_sol.free_idx[j]) = | |
| 92 | ✗ | boxqp_sol.Hff_inv(i, j); | |
| 93 | } | ||
| 94 | } | ||
| 95 | ✗ | STOP_PROFILER("SolverBoxDDP::Quu_invproj"); | |
| 96 | ✗ | START_PROFILER("SolverBoxDDP::Quu_invproj_Qxu"); | |
| 97 | ✗ | K_[t].noalias() = Quu_inv_[t] * Qxu_[t].transpose(); | |
| 98 | ✗ | STOP_PROFILER("SolverBoxDDP::Quu_invproj_Qxu"); | |
| 99 | ✗ | k_[t] = -boxqp_sol.x; | |
| 100 | |||
| 101 | // The box-QP clamped the gradient direction; this is important for | ||
| 102 | // accounting the algorithm advancement (i.e. stopping criteria) | ||
| 103 | ✗ | START_PROFILER("SolverBoxDDP::Qu_proj"); | |
| 104 | ✗ | for (std::size_t i = 0; i < boxqp_sol.clamped_idx.size(); ++i) { | |
| 105 | ✗ | Qu_[t](boxqp_sol.clamped_idx[i]) = 0.; | |
| 106 | } | ||
| 107 | ✗ | STOP_PROFILER("SolverBoxDDP::Qu_proj"); | |
| 108 | } | ||
| 109 | ✗ | STOP_PROFILER("SolverBoxDDP::computeGains"); | |
| 110 | } | ||
| 111 | |||
| 112 | ✗ | void SolverBoxDDP::forwardPass(double steplength) { | |
| 113 | ✗ | if (steplength > 1. || steplength < 0.) { | |
| 114 | ✗ | throw_pretty("Invalid argument: " | |
| 115 | << "invalid step length, value is between 0. to 1."); | ||
| 116 | } | ||
| 117 | ✗ | START_PROFILER("SolverBoxDDP::forwardPass"); | |
| 118 | ✗ | cost_try_ = 0.; | |
| 119 | ✗ | xnext_ = problem_->get_x0(); | |
| 120 | ✗ | const std::size_t T = problem_->get_T(); | |
| 121 | const std::vector<std::shared_ptr<ActionModelAbstract> >& models = | ||
| 122 | ✗ | problem_->get_runningModels(); | |
| 123 | const std::vector<std::shared_ptr<ActionDataAbstract> >& datas = | ||
| 124 | ✗ | problem_->get_runningDatas(); | |
| 125 | ✗ | for (std::size_t t = 0; t < T; ++t) { | |
| 126 | ✗ | const std::shared_ptr<ActionModelAbstract>& m = models[t]; | |
| 127 | ✗ | const std::shared_ptr<ActionDataAbstract>& d = datas[t]; | |
| 128 | ✗ | const std::size_t nu = m->get_nu(); | |
| 129 | |||
| 130 | ✗ | xs_try_[t] = xnext_; | |
| 131 | ✗ | m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]); | |
| 132 | ✗ | if (nu != 0) { | |
| 133 | ✗ | us_try_[t].noalias() = us_[t] - k_[t] * steplength - K_[t] * dx_[t]; | |
| 134 | ✗ | if (m->get_has_control_limits()) { // clamp control | |
| 135 | ✗ | us_try_[t] = us_try_[t].cwiseMax(m->get_u_lb()).cwiseMin(m->get_u_ub()); | |
| 136 | } | ||
| 137 | ✗ | m->calc(d, xs_try_[t], us_try_[t]); | |
| 138 | } else { | ||
| 139 | ✗ | m->calc(d, xs_try_[t]); | |
| 140 | } | ||
| 141 | ✗ | xnext_ = d->xnext; | |
| 142 | ✗ | cost_try_ += d->cost; | |
| 143 | |||
| 144 | ✗ | if (raiseIfNaN(cost_try_)) { | |
| 145 | ✗ | STOP_PROFILER("SolverBoxDDP::forwardPass"); | |
| 146 | ✗ | throw_pretty("forward_error"); | |
| 147 | } | ||
| 148 | ✗ | if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) { | |
| 149 | ✗ | STOP_PROFILER("SolverBoxDDP::forwardPass"); | |
| 150 | ✗ | throw_pretty("forward_error"); | |
| 151 | } | ||
| 152 | } | ||
| 153 | |||
| 154 | ✗ | const std::shared_ptr<ActionModelAbstract>& m = problem_->get_terminalModel(); | |
| 155 | ✗ | const std::shared_ptr<ActionDataAbstract>& d = problem_->get_terminalData(); | |
| 156 | ✗ | if ((is_feasible_) || (steplength == 1)) { | |
| 157 | ✗ | xs_try_.back() = xnext_; | |
| 158 | } else { | ||
| 159 | ✗ | m->get_state()->integrate(xnext_, fs_.back() * (steplength - 1), | |
| 160 | ✗ | xs_try_.back()); | |
| 161 | } | ||
| 162 | ✗ | m->calc(d, xs_try_.back()); | |
| 163 | ✗ | cost_try_ += d->cost; | |
| 164 | |||
| 165 | ✗ | if (raiseIfNaN(cost_try_)) { | |
| 166 | ✗ | STOP_PROFILER("SolverBoxDDP::forwardPass"); | |
| 167 | ✗ | throw_pretty("forward_error"); | |
| 168 | } | ||
| 169 | ✗ | } | |
| 170 | |||
| 171 | ✗ | const std::vector<Eigen::MatrixXd>& SolverBoxDDP::get_Quu_inv() const { | |
| 172 | ✗ | return Quu_inv_; | |
| 173 | } | ||
| 174 | |||
| 175 | } // namespace crocoddyl | ||
| 176 |