| Directory: | ./ |
|---|---|
| File: | src/core/solvers/fddp.cpp |
| Date: | 2025-01-16 08:47:40 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 137 | 172 | 79.7% |
| Branches: | 123 | 462 | 26.6% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /////////////////////////////////////////////////////////////////////////////// | ||
| 2 | // BSD 3-Clause License | ||
| 3 | // | ||
| 4 | // Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh | ||
| 5 | // Heriot-Watt University | ||
| 6 | // Copyright note valid unless otherwise stated in individual files. | ||
| 7 | // All rights reserved. | ||
| 8 | /////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | #ifdef CROCODDYL_WITH_MULTITHREADING | ||
| 11 | #include <omp.h> | ||
| 12 | #endif // CROCODDYL_WITH_MULTITHREADING | ||
| 13 | |||
| 14 | #include "crocoddyl/core/solvers/fddp.hpp" | ||
| 15 | #include "crocoddyl/core/utils/exception.hpp" | ||
| 16 | |||
| 17 | namespace crocoddyl { | ||
| 18 | |||
| 19 | 25 | SolverFDDP::SolverFDDP(boost::shared_ptr<ShootingProblem> problem) | |
| 20 |
1/2✓ Branch 2 taken 25 times.
✗ Branch 3 not taken.
|
25 | : SolverDDP(problem), dg_(0), dq_(0), dv_(0), th_acceptnegstep_(2) {} |
| 21 | |||
| 22 | 62 | SolverFDDP::~SolverFDDP() {} | |
| 23 | |||
| 24 | 16 | bool SolverFDDP::solve(const std::vector<Eigen::VectorXd>& init_xs, | |
| 25 | const std::vector<Eigen::VectorXd>& init_us, | ||
| 26 | const std::size_t maxiter, const bool is_feasible, | ||
| 27 | const double init_reg) { | ||
| 28 |
3/16✗ Branch 2 not taken.
✓ Branch 3 taken 16 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✓ Branch 15 taken 16 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 16 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
|
16 | START_PROFILER("SolverFDDP::solve"); |
| 29 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 16 times.
|
16 | if (problem_->is_updated()) { |
| 30 | ✗ | resizeData(); | |
| 31 | } | ||
| 32 | 16 | xs_try_[0] = | |
| 33 | 32 | problem_->get_x0(); // it is needed in case that init_xs[0] is infeasible | |
| 34 | 16 | setCandidate(init_xs, init_us, is_feasible); | |
| 35 | |||
| 36 |
1/2✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
|
16 | if (std::isnan(init_reg)) { |
| 37 | 16 | preg_ = reg_min_; | |
| 38 | 16 | dreg_ = reg_min_; | |
| 39 | } else { | ||
| 40 | ✗ | preg_ = init_reg; | |
| 41 | ✗ | dreg_ = init_reg; | |
| 42 | } | ||
| 43 | 16 | was_feasible_ = false; | |
| 44 | |||
| 45 | 16 | bool recalcDiff = true; | |
| 46 |
2/2✓ Branch 0 taken 50 times.
✓ Branch 1 taken 6 times.
|
56 | for (iter_ = 0; iter_ < maxiter; ++iter_) { |
| 47 | while (true) { | ||
| 48 | try { | ||
| 49 |
1/2✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
|
50 | computeDirection(recalcDiff); |
| 50 | ✗ | } catch (std::exception& e) { | |
| 51 | ✗ | recalcDiff = false; | |
| 52 | ✗ | increaseRegularization(); | |
| 53 | ✗ | if (preg_ == reg_max_) { | |
| 54 | ✗ | return false; | |
| 55 | } else { | ||
| 56 | ✗ | continue; | |
| 57 | } | ||
| 58 | } | ||
| 59 | 50 | break; | |
| 60 | } | ||
| 61 | 50 | updateExpectedImprovement(); | |
| 62 | |||
| 63 | // We need to recalculate the derivatives when the step length passes | ||
| 64 | 50 | recalcDiff = false; | |
| 65 | 50 | for (std::vector<double>::const_iterator it = alphas_.begin(); | |
| 66 |
1/2✓ Branch 3 taken 50 times.
✗ Branch 4 not taken.
|
50 | it != alphas_.end(); ++it) { |
| 67 | 50 | steplength_ = *it; | |
| 68 | |||
| 69 | try { | ||
| 70 |
1/2✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
|
50 | dV_ = tryStep(steplength_); |
| 71 | ✗ | } catch (std::exception& e) { | |
| 72 | ✗ | continue; | |
| 73 | } | ||
| 74 |
1/2✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
|
50 | expectedImprovement(); |
| 75 |
2/4✓ Branch 1 taken 50 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50 times.
✗ Branch 5 not taken.
|
50 | dVexp_ = steplength_ * (d_[0] + 0.5 * steplength_ * d_[1]); |
| 76 | |||
| 77 |
2/2✓ Branch 0 taken 38 times.
✓ Branch 1 taken 12 times.
|
50 | if (dVexp_ >= 0) { // descend direction |
| 78 |
5/8✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✓ Branch 5 taken 8 times.
✓ Branch 6 taken 30 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 38 times.
✗ Branch 9 not taken.
|
38 | if (std::abs(d_[0]) < th_grad_ || dV_ > th_acceptstep_ * dVexp_) { |
| 79 | 38 | was_feasible_ = is_feasible_; | |
| 80 |
4/6✓ Branch 0 taken 4 times.
✓ Branch 1 taken 34 times.
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 38 times.
✗ Branch 6 not taken.
|
38 | setCandidate(xs_try_, us_try_, (was_feasible_) || (steplength_ == 1)); |
| 81 | 38 | cost_ = cost_try_; | |
| 82 | 38 | recalcDiff = true; | |
| 83 | 38 | break; | |
| 84 | } | ||
| 85 | } else { // reducing the gaps by allowing a small increment in the cost | ||
| 86 | // value | ||
| 87 |
2/4✓ Branch 0 taken 12 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
|
12 | if (!is_feasible_ && dV_ > th_acceptnegstep_ * dVexp_) { |
| 88 | 12 | was_feasible_ = is_feasible_; | |
| 89 |
3/6✓ Branch 0 taken 12 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
|
12 | setCandidate(xs_try_, us_try_, (was_feasible_) || (steplength_ == 1)); |
| 90 | 12 | cost_ = cost_try_; | |
| 91 | 12 | recalcDiff = true; | |
| 92 | 12 | break; | |
| 93 | } | ||
| 94 | } | ||
| 95 | } | ||
| 96 | |||
| 97 |
1/2✓ Branch 0 taken 50 times.
✗ Branch 1 not taken.
|
50 | if (steplength_ > th_stepdec_) { |
| 98 | 50 | decreaseRegularization(); | |
| 99 | } | ||
| 100 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 50 times.
|
50 | if (steplength_ <= th_stepinc_) { |
| 101 | ✗ | increaseRegularization(); | |
| 102 | ✗ | if (preg_ == reg_max_) { | |
| 103 | ✗ | STOP_PROFILER("SolverFDDP::solve"); | |
| 104 | ✗ | return false; | |
| 105 | } | ||
| 106 | } | ||
| 107 | 50 | stoppingCriteria(); | |
| 108 | |||
| 109 | 50 | const std::size_t n_callbacks = callbacks_.size(); | |
| 110 |
2/2✓ Branch 0 taken 22 times.
✓ Branch 1 taken 50 times.
|
72 | for (std::size_t c = 0; c < n_callbacks; ++c) { |
| 111 | 22 | CallbackAbstract& callback = *callbacks_[c]; | |
| 112 | 22 | callback(*this); | |
| 113 | } | ||
| 114 | |||
| 115 |
4/4✓ Branch 0 taken 34 times.
✓ Branch 1 taken 16 times.
✓ Branch 2 taken 10 times.
✓ Branch 3 taken 24 times.
|
50 | if (was_feasible_ && stop_ < th_stop_) { |
| 116 |
3/16✗ Branch 2 not taken.
✓ Branch 3 taken 10 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✓ Branch 15 taken 10 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 10 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
|
10 | STOP_PROFILER("SolverFDDP::solve"); |
| 117 | 10 | return true; | |
| 118 | } | ||
| 119 | } | ||
| 120 |
3/16✗ Branch 2 not taken.
✓ Branch 3 taken 6 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✓ Branch 15 taken 6 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 6 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
|
6 | STOP_PROFILER("SolverFDDP::solve"); |
| 121 | 6 | return false; | |
| 122 | } | ||
| 123 | |||
| 124 | 52 | const Eigen::Vector2d& SolverFDDP::expectedImprovement() { | |
| 125 | 52 | dv_ = 0; | |
| 126 | 52 | const std::size_t T = this->problem_->get_T(); | |
| 127 |
2/2✓ Branch 0 taken 16 times.
✓ Branch 1 taken 36 times.
|
52 | if (!is_feasible_) { |
| 128 | // NB: The dimension of vectors xs_try_ and xs_ are T+1, whereas the | ||
| 129 | // dimension of dx_ is T. Here, we are re-using the final element of dx_ for | ||
| 130 | // the computation of the difference at the terminal node. Using the access | ||
| 131 | // iterator back() this re-use of the final element is fine. Cf. the | ||
| 132 | // discussion at https://github.com/loco-3d/crocoddyl/issues/1022 | ||
| 133 |
3/6✓ Branch 8 taken 16 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 16 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 16 times.
✗ Branch 16 not taken.
|
32 | problem_->get_terminalModel()->get_state()->diff(xs_try_.back(), xs_.back(), |
| 134 | 16 | dx_.back()); | |
| 135 |
2/4✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 16 times.
✗ Branch 8 not taken.
|
16 | fTVxx_p_.noalias() = Vxx_.back() * dx_.back(); |
| 136 | 16 | dv_ -= fs_.back().dot(fTVxx_p_); | |
| 137 | const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = | ||
| 138 | 16 | problem_->get_runningModels(); | |
| 139 | |||
| 140 |
2/2✓ Branch 0 taken 178 times.
✓ Branch 1 taken 16 times.
|
194 | for (std::size_t t = 0; t < T; ++t) { |
| 141 |
3/6✓ Branch 8 taken 178 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 178 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 178 times.
✗ Branch 16 not taken.
|
178 | models[t]->get_state()->diff(xs_try_[t], xs_[t], dx_[t]); |
| 142 |
2/4✓ Branch 4 taken 178 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 178 times.
✗ Branch 8 not taken.
|
178 | fTVxx_p_.noalias() = Vxx_[t] * dx_[t]; |
| 143 | 178 | dv_ -= fs_[t].dot(fTVxx_p_); | |
| 144 | } | ||
| 145 | } | ||
| 146 | 52 | d_[0] = dg_ + dv_; | |
| 147 | 52 | d_[1] = dq_ - 2 * dv_; | |
| 148 | 52 | return d_; | |
| 149 | } | ||
| 150 | |||
| 151 | 50 | void SolverFDDP::updateExpectedImprovement() { | |
| 152 | 50 | dg_ = 0; | |
| 153 | 50 | dq_ = 0; | |
| 154 | 50 | const std::size_t T = this->problem_->get_T(); | |
| 155 |
2/2✓ Branch 0 taken 16 times.
✓ Branch 1 taken 34 times.
|
50 | if (!is_feasible_) { |
| 156 | 16 | dg_ -= Vx_.back().dot(fs_.back()); | |
| 157 |
2/4✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 16 times.
✗ Branch 8 not taken.
|
16 | fTVxx_p_.noalias() = Vxx_.back() * fs_.back(); |
| 158 | 16 | dq_ += fs_.back().dot(fTVxx_p_); | |
| 159 | } | ||
| 160 | const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = | ||
| 161 | 50 | problem_->get_runningModels(); | |
| 162 |
2/2✓ Branch 0 taken 624 times.
✓ Branch 1 taken 50 times.
|
674 | for (std::size_t t = 0; t < T; ++t) { |
| 163 | 624 | const std::size_t nu = models[t]->get_nu(); | |
| 164 |
1/2✓ Branch 0 taken 624 times.
✗ Branch 1 not taken.
|
624 | if (nu != 0) { |
| 165 | 624 | dg_ += Qu_[t].dot(k_[t]); | |
| 166 | 624 | dq_ -= k_[t].dot(Quuk_[t]); | |
| 167 | } | ||
| 168 |
2/2✓ Branch 0 taken 178 times.
✓ Branch 1 taken 446 times.
|
624 | if (!is_feasible_) { |
| 169 | 178 | dg_ -= Vx_[t].dot(fs_[t]); | |
| 170 |
2/4✓ Branch 4 taken 178 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 178 times.
✗ Branch 8 not taken.
|
178 | fTVxx_p_.noalias() = Vxx_[t] * fs_[t]; |
| 171 | 178 | dq_ += fs_[t].dot(fTVxx_p_); | |
| 172 | } | ||
| 173 | } | ||
| 174 | 50 | } | |
| 175 | |||
| 176 | 43 | void SolverFDDP::forwardPass(const double steplength) { | |
| 177 |
2/4✓ Branch 0 taken 43 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 43 times.
|
43 | if (steplength > 1. || steplength < 0.) { |
| 178 | ✗ | throw_pretty("Invalid argument: " | |
| 179 | << "invalid step length, value is between 0. to 1."); | ||
| 180 | } | ||
| 181 |
3/16✗ Branch 2 not taken.
✓ Branch 3 taken 43 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✓ Branch 15 taken 43 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 43 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
|
43 | START_PROFILER("SolverFDDP::forwardPass"); |
| 182 | 43 | cost_try_ = 0.; | |
| 183 | 43 | xnext_ = problem_->get_x0(); | |
| 184 | 43 | const std::size_t T = problem_->get_T(); | |
| 185 | const std::vector<boost::shared_ptr<ActionModelAbstract> >& models = | ||
| 186 | 43 | problem_->get_runningModels(); | |
| 187 | const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas = | ||
| 188 | 43 | problem_->get_runningDatas(); | |
| 189 |
4/4✓ Branch 0 taken 15 times.
✓ Branch 1 taken 28 times.
✓ Branch 2 taken 13 times.
✓ Branch 3 taken 2 times.
|
43 | if ((is_feasible_) || (steplength == 1)) { |
| 190 |
2/2✓ Branch 0 taken 522 times.
✓ Branch 1 taken 41 times.
|
563 | for (std::size_t t = 0; t < T; ++t) { |
| 191 | 522 | const boost::shared_ptr<ActionModelAbstract>& m = models[t]; | |
| 192 | 522 | const boost::shared_ptr<ActionDataAbstract>& d = datas[t]; | |
| 193 | 522 | const std::size_t nu = m->get_nu(); | |
| 194 | |||
| 195 | 522 | xs_try_[t] = xnext_; | |
| 196 |
3/6✓ Branch 7 taken 522 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 522 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 522 times.
✗ Branch 15 not taken.
|
522 | m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]); |
| 197 |
1/2✓ Branch 0 taken 522 times.
✗ Branch 1 not taken.
|
522 | if (nu != 0) { |
| 198 |
5/10✓ Branch 5 taken 522 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 522 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 522 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 522 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 522 times.
✗ Branch 20 not taken.
|
522 | us_try_[t].noalias() = us_[t] - k_[t] * steplength - K_[t] * dx_[t]; |
| 199 |
2/4✓ Branch 5 taken 522 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 522 times.
✗ Branch 9 not taken.
|
522 | m->calc(d, xs_try_[t], us_try_[t]); |
| 200 | } else { | ||
| 201 | ✗ | m->calc(d, xs_try_[t]); | |
| 202 | } | ||
| 203 | 522 | xnext_ = d->xnext; | |
| 204 | 522 | cost_try_ += d->cost; | |
| 205 | |||
| 206 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 522 times.
|
522 | if (raiseIfNaN(cost_try_)) { |
| 207 | ✗ | STOP_PROFILER("SolverFDDP::forwardPass"); | |
| 208 | ✗ | throw_pretty("forward_error"); | |
| 209 | } | ||
| 210 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 522 times.
|
522 | if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) { |
| 211 | ✗ | STOP_PROFILER("SolverFDDP::forwardPass"); | |
| 212 | ✗ | throw_pretty("forward_error"); | |
| 213 | } | ||
| 214 | } | ||
| 215 | |||
| 216 | const boost::shared_ptr<ActionModelAbstract>& m = | ||
| 217 | 41 | problem_->get_terminalModel(); | |
| 218 | const boost::shared_ptr<ActionDataAbstract>& d = | ||
| 219 | 41 | problem_->get_terminalData(); | |
| 220 | 41 | xs_try_.back() = xnext_; | |
| 221 |
1/2✓ Branch 4 taken 41 times.
✗ Branch 5 not taken.
|
41 | m->calc(d, xs_try_.back()); |
| 222 | 41 | cost_try_ += d->cost; | |
| 223 | |||
| 224 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 41 times.
|
41 | if (raiseIfNaN(cost_try_)) { |
| 225 | ✗ | STOP_PROFILER("SolverFDDP::forwardPass"); | |
| 226 | ✗ | throw_pretty("forward_error"); | |
| 227 | } | ||
| 228 | 41 | } else { | |
| 229 |
2/2✓ Branch 0 taken 8 times.
✓ Branch 1 taken 2 times.
|
10 | for (std::size_t t = 0; t < T; ++t) { |
| 230 | 8 | const boost::shared_ptr<ActionModelAbstract>& m = models[t]; | |
| 231 | 8 | const boost::shared_ptr<ActionDataAbstract>& d = datas[t]; | |
| 232 | 8 | const std::size_t nu = m->get_nu(); | |
| 233 |
4/8✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 8 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 8 times.
✗ Branch 17 not taken.
|
8 | m->get_state()->integrate(xnext_, fs_[t] * (steplength - 1), xs_try_[t]); |
| 234 |
3/6✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 8 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 8 times.
✗ Branch 15 not taken.
|
8 | m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]); |
| 235 |
1/2✓ Branch 0 taken 8 times.
✗ Branch 1 not taken.
|
8 | if (nu != 0) { |
| 236 |
5/10✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 8 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 8 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 8 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 8 times.
✗ Branch 20 not taken.
|
8 | us_try_[t].noalias() = us_[t] - k_[t] * steplength - K_[t] * dx_[t]; |
| 237 |
2/4✓ Branch 5 taken 8 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 8 times.
✗ Branch 9 not taken.
|
8 | m->calc(d, xs_try_[t], us_try_[t]); |
| 238 | } else { | ||
| 239 | ✗ | m->calc(d, xs_try_[t]); | |
| 240 | } | ||
| 241 | 8 | xnext_ = d->xnext; | |
| 242 | 8 | cost_try_ += d->cost; | |
| 243 | |||
| 244 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 8 times.
|
8 | if (raiseIfNaN(cost_try_)) { |
| 245 | ✗ | STOP_PROFILER("SolverFDDP::forwardPass"); | |
| 246 | ✗ | throw_pretty("forward_error"); | |
| 247 | } | ||
| 248 |
1/2✗ Branch 2 not taken.
✓ Branch 3 taken 8 times.
|
8 | if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) { |
| 249 | ✗ | STOP_PROFILER("SolverFDDP::forwardPass"); | |
| 250 | ✗ | throw_pretty("forward_error"); | |
| 251 | } | ||
| 252 | } | ||
| 253 | |||
| 254 | const boost::shared_ptr<ActionModelAbstract>& m = | ||
| 255 | 2 | problem_->get_terminalModel(); | |
| 256 | const boost::shared_ptr<ActionDataAbstract>& d = | ||
| 257 | 2 | problem_->get_terminalData(); | |
| 258 |
4/8✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 2 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 2 times.
✗ Branch 16 not taken.
|
4 | m->get_state()->integrate(xnext_, fs_.back() * (steplength - 1), |
| 259 | 2 | xs_try_.back()); | |
| 260 |
1/2✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
2 | m->calc(d, xs_try_.back()); |
| 261 | 2 | cost_try_ += d->cost; | |
| 262 | |||
| 263 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
|
2 | if (raiseIfNaN(cost_try_)) { |
| 264 | ✗ | STOP_PROFILER("SolverFDDP::forwardPass"); | |
| 265 | ✗ | throw_pretty("forward_error"); | |
| 266 | } | ||
| 267 | } | ||
| 268 |
3/16✗ Branch 2 not taken.
✓ Branch 3 taken 43 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✓ Branch 15 taken 43 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 43 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
|
43 | STOP_PROFILER("SolverFDDP::forwardPass"); |
| 269 | 43 | } | |
| 270 | |||
| 271 | ✗ | double SolverFDDP::get_th_acceptnegstep() const { return th_acceptnegstep_; } | |
| 272 | |||
| 273 | ✗ | void SolverFDDP::set_th_acceptnegstep(const double th_acceptnegstep) { | |
| 274 | ✗ | if (0. > th_acceptnegstep) { | |
| 275 | ✗ | throw_pretty( | |
| 276 | "Invalid argument: " << "th_acceptnegstep value has to be positive."); | ||
| 277 | } | ||
| 278 | ✗ | th_acceptnegstep_ = th_acceptnegstep; | |
| 279 | } | ||
| 280 | |||
| 281 | } // namespace crocoddyl | ||
| 282 |