GCC Code Coverage Report

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