GCC Code Coverage Report

Directory: ./
File: src/core/solvers/ddp.cpp
Date: 2025-02-24 23:41:29
Exec Total Coverage
Lines: 271 391 69.3%
Branches: 265 1218 21.8%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh,
5 // University of Oxford, Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 #include "crocoddyl/core/solvers/ddp.hpp"
11
12 #include <iostream>
13
14 #include "crocoddyl/core/utils/exception.hpp"
15
16 namespace crocoddyl {
17
18 49 SolverDDP::SolverDDP(std::shared_ptr<ShootingProblem> problem)
19 : SolverAbstract(problem),
20 49 reg_incfactor_(10.),
21 49 reg_decfactor_(10.),
22 49 reg_min_(1e-9),
23 49 reg_max_(1e9),
24 49 cost_try_(0.),
25 49 th_grad_(1e-12),
26 49 th_stepdec_(0.5),
27
5/10
✓ Branch 2 taken 49 times.
✗ Branch 3 not taken.
✓ Branch 10 taken 49 times.
✗ Branch 11 not taken.
✓ Branch 21 taken 49 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 49 times.
✗ Branch 25 not taken.
✓ Branch 28 taken 49 times.
✗ Branch 29 not taken.
 49 th_stepinc_(0.01) {
28
1/2
✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
 49 allocateData();
29
30 49 const std::size_t n_alphas = 10;
31
1/2
✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
 49 alphas_.resize(n_alphas);
32
2/2
✓ Branch 0 taken 490 times.
✓ Branch 1 taken 49 times.
 539 for (std::size_t n = 0; n < n_alphas; ++n) {
33 490 alphas_[n] = 1. / pow(2., static_cast<double>(n));
34 }
35
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 49 times.
 49 if (th_stepinc_ < alphas_[n_alphas - 1]) {
36 ✗ th_stepinc_ = alphas_[n_alphas - 1];
37 std::cerr << "Warning: th_stepinc has higher value than lowest alpha "
38 "value, set to "
39 ✗ << std::to_string(alphas_[n_alphas - 1]) << std::endl;
40 }
41 49 }
42
43 118 SolverDDP::~SolverDDP() {}
44
45 16 bool SolverDDP::solve(const std::vector<Eigen::VectorXd>& init_xs,
46 const std::vector<Eigen::VectorXd>& init_us,
47 const std::size_t maxiter, const bool is_feasible,
48 const double init_reg) {
49
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("SolverDDP::solve");
50
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 16 times.
 16 if (problem_->is_updated()) {
51 ✗ resizeData();
52 }
53 16 xs_try_[0] =
54 32 problem_->get_x0(); // it is needed in case that init_xs[0] is infeasible
55 16 setCandidate(init_xs, init_us, is_feasible);
56
57
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 if (std::isnan(init_reg)) {
58 16 preg_ = reg_min_;
59 16 dreg_ = reg_min_;
60 } else {
61 ✗ preg_ = init_reg;
62 ✗ dreg_ = init_reg;
63 }
64 16 was_feasible_ = false;
65
66 16 bool recalcDiff = true;
67
2/2
✓ Branch 0 taken 43 times.
✓ Branch 1 taken 5 times.
 48 for (iter_ = 0; iter_ < maxiter; ++iter_) {
68 while (true) {
69 try {
70
1/2
✓ Branch 1 taken 43 times.
✗ Branch 2 not taken.
 43 computeDirection(recalcDiff);
71 ✗ } catch (std::exception& e) {
72 ✗ recalcDiff = false;
73 ✗ increaseRegularization();
74 ✗ if (preg_ == reg_max_) {
75 ✗ return false;
76 } else {
77 ✗ continue;
78 }
79 }
80 43 break;
81 }
82 43 expectedImprovement();
83
84 // We need to recalculate the derivatives when the step length passes
85 43 recalcDiff = false;
86 43 for (std::vector<double>::const_iterator it = alphas_.begin();
87
1/2
✓ Branch 3 taken 43 times.
✗ Branch 4 not taken.
 43 it != alphas_.end(); ++it) {
88 43 steplength_ = *it;
89
90 try {
91
1/2
✓ Branch 1 taken 43 times.
✗ Branch 2 not taken.
 43 dV_ = tryStep(steplength_);
92 ✗ } catch (std::exception& e) {
93 ✗ continue;
94 }
95
2/4
✓ Branch 1 taken 43 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 43 times.
✗ Branch 5 not taken.
 43 dVexp_ = steplength_ * (d_[0] + 0.5 * steplength_ * d_[1]);
96
97
1/2
✓ Branch 0 taken 43 times.
✗ Branch 1 not taken.
 43 if (dVexp_ >= 0) { // descend direction
98
6/8
✓ Branch 1 taken 43 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 32 times.
✓ Branch 5 taken 11 times.
✓ Branch 6 taken 19 times.
✓ Branch 7 taken 13 times.
✓ Branch 8 taken 43 times.
✗ Branch 9 not taken.
 62 if (std::abs(d_[0]) < th_grad_ || !is_feasible_ ||
99
1/2
✓ Branch 0 taken 19 times.
✗ Branch 1 not taken.
 19 dV_ > th_acceptstep_ * dVexp_) {
100 43 was_feasible_ = is_feasible_;
101
1/2
✓ Branch 1 taken 43 times.
✗ Branch 2 not taken.
 43 setCandidate(xs_try_, us_try_, true);
102 43 cost_ = cost_try_;
103 43 recalcDiff = true;
104 43 break;
105 }
106 }
107 }
108
109
1/2
✓ Branch 0 taken 43 times.
✗ Branch 1 not taken.
 43 if (steplength_ > th_stepdec_) {
110 43 decreaseRegularization();
111 }
112
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 43 times.
 43 if (steplength_ <= th_stepinc_) {
113 ✗ increaseRegularization();
114 ✗ if (preg_ == reg_max_) {
115 ✗ STOP_PROFILER("SolverDDP::solve");
116 ✗ return false;
117 }
118 }
119 43 stoppingCriteria();
120
121 43 const std::size_t n_callbacks = callbacks_.size();
122
2/2
✓ Branch 0 taken 22 times.
✓ Branch 1 taken 43 times.
 65 for (std::size_t c = 0; c < n_callbacks; ++c) {
123 22 CallbackAbstract& callback = *callbacks_[c];
124 22 callback(*this);
125 }
126
127
4/4
✓ Branch 0 taken 27 times.
✓ Branch 1 taken 16 times.
✓ Branch 2 taken 11 times.
✓ Branch 3 taken 16 times.
 43 if (was_feasible_ && stop_ < th_stop_) {
128
3/16
✗ Branch 2 not taken.
✓ Branch 3 taken 11 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 11 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 11 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
 11 STOP_PROFILER("SolverDDP::solve");
129 11 return true;
130 }
131 }
132
3/16
✗ Branch 2 not taken.
✓ Branch 3 taken 5 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 5 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 5 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
 5 STOP_PROFILER("SolverDDP::solve");
133 5 return false;
134 }
135
136 101 void SolverDDP::computeDirection(const bool recalcDiff) {
137
3/16
✗ Branch 2 not taken.
✓ Branch 3 taken 101 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 101 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 101 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
 101 START_PROFILER("SolverDDP::computeDirection");
138
1/2
✓ Branch 0 taken 101 times.
✗ Branch 1 not taken.
 101 if (recalcDiff) {
139 101 calcDiff();
140 }
141 101 backwardPass();
142
3/16
✗ Branch 2 not taken.
✓ Branch 3 taken 101 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 101 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 101 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
 101 STOP_PROFILER("SolverDDP::computeDirection");
143 101 }
144
145 103 double SolverDDP::tryStep(const double steplength) {
146
3/16
✗ Branch 2 not taken.
✓ Branch 3 taken 103 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 103 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 103 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
 103 START_PROFILER("SolverDDP::tryStep");
147 103 forwardPass(steplength);
148
3/16
✗ Branch 2 not taken.
✓ Branch 3 taken 103 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 103 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 103 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
 103 STOP_PROFILER("SolverDDP::tryStep");
149 103 return cost_ - cost_try_;
150 }
151
152 97 double SolverDDP::stoppingCriteria() {
153 // This stopping criteria represents the expected reduction in the value
154 // function. If this reduction is less than a certain threshold, then the
155 // algorithm reaches the local minimum. For more details, see C. Mastalli et
156 // al. "Inverse-dynamics MPC via Nullspace Resolution".
157 97 stop_ = std::abs(d_[0] + 0.5 * d_[1]);
158 97 return stop_;
159 }
160
161 45 const Eigen::Vector2d& SolverDDP::expectedImprovement() {
162
1/2
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
 45 d_.fill(0);
163 45 const std::size_t T = this->problem_->get_T();
164 const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
165 45 problem_->get_runningModels();
166
2/2
✓ Branch 0 taken 485 times.
✓ Branch 1 taken 45 times.
 530 for (std::size_t t = 0; t < T; ++t) {
167 485 const std::size_t nu = models[t]->get_nu();
168
1/2
✓ Branch 0 taken 485 times.
✗ Branch 1 not taken.
 485 if (nu != 0) {
169 485 d_[0] += Qu_[t].dot(k_[t]);
170 485 d_[1] -= k_[t].dot(Quuk_[t]);
171 }
172 }
173 45 return d_;
174 }
175
176 ✗ void SolverDDP::resizeData() {
177 ✗ START_PROFILER("SolverDDP::resizeData");
178 ✗ SolverAbstract::resizeData();
179
180 ✗ const std::size_t T = problem_->get_T();
181 ✗ const std::size_t ndx = problem_->get_ndx();
182 const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
183 ✗ problem_->get_runningModels();
184 ✗ for (std::size_t t = 0; t < T; ++t) {
185 ✗ const std::shared_ptr<ActionModelAbstract>& model = models[t];
186 ✗ const std::size_t nu = model->get_nu();
187 ✗ Qxu_[t].conservativeResize(ndx, nu);
188 ✗ Quu_[t].conservativeResize(nu, nu);
189 ✗ Qu_[t].conservativeResize(nu);
190 ✗ K_[t].conservativeResize(nu, ndx);
191 ✗ k_[t].conservativeResize(nu);
192 ✗ us_try_[t].conservativeResize(nu);
193 ✗ FuTVxx_p_[t].conservativeResize(nu, ndx);
194 ✗ Quuk_[t].conservativeResize(nu);
195 ✗ if (nu != 0) {
196 ✗ FuTVxx_p_[t].setZero();
197 }
198 }
199 ✗ STOP_PROFILER("SolverDDP::resizeData");
200 }
201
202 101 double SolverDDP::calcDiff() {
203
3/16
✗ Branch 2 not taken.
✓ Branch 3 taken 101 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 101 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 101 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
 101 START_PROFILER("SolverDDP::calcDiff");
204
2/2
✓ Branch 0 taken 40 times.
✓ Branch 1 taken 61 times.
 101 if (iter_ == 0) {
205 40 problem_->calc(xs_, us_);
206 }
207 101 cost_ = problem_->calcDiff(xs_, us_);
208
209 101 ffeas_ = computeDynamicFeasibility();
210 101 gfeas_ = computeInequalityFeasibility();
211 101 hfeas_ = computeEqualityFeasibility();
212
3/16
✗ Branch 2 not taken.
✓ Branch 3 taken 101 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 101 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 101 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
 101 STOP_PROFILER("SolverDDP::calcDiff");
213 101 return cost_;
214 }
215
216 101 void SolverDDP::backwardPass() {
217
3/16
✗ Branch 2 not taken.
✓ Branch 3 taken 101 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 101 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 101 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
 101 START_PROFILER("SolverDDP::backwardPass");
218 101 const std::shared_ptr<ActionDataAbstract>& d_T = problem_->get_terminalData();
219 101 Vxx_.back() = d_T->Lxx;
220 101 Vx_.back() = d_T->Lx;
221
222
1/2
✓ Branch 1 taken 101 times.
✗ Branch 2 not taken.
 101 if (!std::isnan(preg_)) {
223
2/4
✓ Branch 3 taken 101 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 101 times.
✗ Branch 7 not taken.
 101 Vxx_.back().diagonal().array() += preg_;
224 }
225
226
2/2
✓ Branch 0 taken 41 times.
✓ Branch 1 taken 60 times.
 101 if (!is_feasible_) {
227
2/4
✓ Branch 5 taken 41 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 41 times.
✗ Branch 9 not taken.
 41 Vx_.back().noalias() += Vxx_.back() * fs_.back();
228 }
229 const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
230 101 problem_->get_runningModels();
231 const std::vector<std::shared_ptr<ActionDataAbstract> >& datas =
232 101 problem_->get_runningDatas();
233
2/2
✓ Branch 2 taken 1109 times.
✓ Branch 3 taken 101 times.
 1210 for (int t = static_cast<int>(problem_->get_T()) - 1; t >= 0; --t) {
234 1109 const std::shared_ptr<ActionModelAbstract>& m = models[t];
235 1109 const std::shared_ptr<ActionDataAbstract>& d = datas[t];
236
237 // Compute the linear-quadratic approximation of the control Hamiltonian
238 // function
239 1109 computeActionValueFunction(t, m, d);
240
241 // Compute the feedforward and feedback gains
242 1109 computeGains(t);
243
244 // Compute the linear-quadratic approximation of the Value function
245 1109 computeValueFunction(t, m);
246
247
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 1109 times.
 1109 if (raiseIfNaN(Vx_[t].lpNorm<Eigen::Infinity>())) {
248 ✗ throw_pretty("backward_error");
249 }
250
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 1109 times.
 1109 if (raiseIfNaN(Vxx_[t].lpNorm<Eigen::Infinity>())) {
251 ✗ throw_pretty("backward_error");
252 }
253 }
254
3/16
✗ Branch 2 not taken.
✓ Branch 3 taken 101 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 101 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 101 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
 101 STOP_PROFILER("SolverDDP::backwardPass");
255 101 }
256
257 36 void SolverDDP::forwardPass(const double steplength) {
258
2/4
✓ Branch 0 taken 36 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 36 times.
 36 if (steplength > 1. || steplength < 0.) {
259 ✗ throw_pretty("Invalid argument: "
260 << "invalid step length, value is between 0. to 1.");
261 }
262
3/16
✗ Branch 2 not taken.
✓ Branch 3 taken 36 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 36 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 36 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
 36 START_PROFILER("SolverDDP::forwardPass");
263 36 cost_try_ = 0.;
264 36 const std::size_t T = problem_->get_T();
265 const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
266 36 problem_->get_runningModels();
267 const std::vector<std::shared_ptr<ActionDataAbstract> >& datas =
268 36 problem_->get_runningDatas();
269
2/2
✓ Branch 0 taken 411 times.
✓ Branch 1 taken 36 times.
 447 for (std::size_t t = 0; t < T; ++t) {
270 411 const std::shared_ptr<ActionModelAbstract>& m = models[t];
271 411 const std::shared_ptr<ActionDataAbstract>& d = datas[t];
272
273
3/6
✓ Branch 7 taken 411 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 411 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 411 times.
✗ Branch 15 not taken.
 411 m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]);
274
1/2
✓ Branch 2 taken 411 times.
✗ Branch 3 not taken.
 411 if (m->get_nu() != 0) {
275
1/2
✓ Branch 4 taken 411 times.
✗ Branch 5 not taken.
 411 us_try_[t].noalias() = us_[t];
276
2/4
✓ Branch 4 taken 411 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 411 times.
✗ Branch 8 not taken.
 411 us_try_[t].noalias() -= k_[t] * steplength;
277
2/4
✓ Branch 5 taken 411 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 411 times.
✗ Branch 9 not taken.
 411 us_try_[t].noalias() -= K_[t] * dx_[t];
278
2/4
✓ Branch 5 taken 411 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 411 times.
✗ Branch 9 not taken.
 411 m->calc(d, xs_try_[t], us_try_[t]);
279 } else {
280 ✗ m->calc(d, xs_try_[t]);
281 }
282 411 xs_try_[t + 1] = d->xnext;
283 411 cost_try_ += d->cost;
284
285
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 411 times.
 411 if (raiseIfNaN(cost_try_)) {
286 ✗ STOP_PROFILER("SolverDDP::forwardPass");
287 ✗ throw_pretty("forward_error");
288 }
289
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 411 times.
 411 if (raiseIfNaN(xs_try_[t + 1].lpNorm<Eigen::Infinity>())) {
290 ✗ STOP_PROFILER("SolverDDP::forwardPass");
291 ✗ throw_pretty("forward_error");
292 }
293 }
294
295 36 const std::shared_ptr<ActionModelAbstract>& m = problem_->get_terminalModel();
296 36 const std::shared_ptr<ActionDataAbstract>& d = problem_->get_terminalData();
297
1/2
✓ Branch 4 taken 36 times.
✗ Branch 5 not taken.
 36 m->calc(d, xs_try_.back());
298 36 cost_try_ += d->cost;
299
300
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 36 times.
 36 if (raiseIfNaN(cost_try_)) {
301 ✗ STOP_PROFILER("SolverDDP::forwardPass");
302 ✗ throw_pretty("forward_error");
303 }
304
3/16
✗ Branch 2 not taken.
✓ Branch 3 taken 36 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 36 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 36 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
 36 STOP_PROFILER("SolverDDP::forwardPass");
305 36 }
306
307 1109 void SolverDDP::computeActionValueFunction(
308 const std::size_t t, const std::shared_ptr<ActionModelAbstract>& model,
309 const std::shared_ptr<ActionDataAbstract>& data) {
310
1/14
✗ Branch 2 not taken.
✓ Branch 3 taken 1109 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
 1109 assert_pretty(t < problem_->get_T(),
311 "Invalid argument: t should be between 0 and " +
312 std::to_string(problem_->get_T()););
313 1109 const std::size_t nu = model->get_nu();
314 1109 const Eigen::MatrixXd& Vxx_p = Vxx_[t + 1];
315 1109 const Eigen::VectorXd& Vx_p = Vx_[t + 1];
316
317
3/6
✓ Branch 3 taken 1109 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1109 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 1109 times.
✗ Branch 10 not taken.
 1109 FxTVxx_p_.noalias() = data->Fx.transpose() * Vxx_p;
318
3/16
✗ Branch 2 not taken.
✓ Branch 3 taken 1109 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 1109 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 1109 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
 1109 START_PROFILER("SolverDDP::Qx");
319 1109 Qx_[t] = data->Lx;
320
3/6
✓ Branch 3 taken 1109 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 1109 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1109 times.
✗ Branch 11 not taken.
 1109 Qx_[t].noalias() += data->Fx.transpose() * Vx_p;
321
3/16
✗ Branch 2 not taken.
✓ Branch 3 taken 1109 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 1109 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 1109 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
 1109 STOP_PROFILER("SolverDDP::Qx");
322
3/16
✗ Branch 2 not taken.
✓ Branch 3 taken 1109 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 1109 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 1109 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
 1109 START_PROFILER("SolverDDP::Qxx");
323 1109 Qxx_[t] = data->Lxx;
324
2/4
✓ Branch 4 taken 1109 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1109 times.
✗ Branch 8 not taken.
 1109 Qxx_[t].noalias() += FxTVxx_p_ * data->Fx;
325
3/16
✗ Branch 2 not taken.
✓ Branch 3 taken 1109 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 1109 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 1109 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
 1109 STOP_PROFILER("SolverDDP::Qxx");
326
1/2
✓ Branch 0 taken 1109 times.
✗ Branch 1 not taken.
 1109 if (nu != 0) {
327
3/6
✓ Branch 3 taken 1109 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 1109 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1109 times.
✗ Branch 11 not taken.
 1109 FuTVxx_p_[t].noalias() = data->Fu.transpose() * Vxx_p;
328
3/16
✗ Branch 2 not taken.
✓ Branch 3 taken 1109 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 1109 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 1109 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
 1109 START_PROFILER("SolverDDP::Qu");
329 1109 Qu_[t] = data->Lu;
330
3/6
✓ Branch 3 taken 1109 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 1109 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1109 times.
✗ Branch 11 not taken.
 1109 Qu_[t].noalias() += data->Fu.transpose() * Vx_p;
331
3/16
✗ Branch 2 not taken.
✓ Branch 3 taken 1109 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 1109 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 1109 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
 1109 STOP_PROFILER("SolverDDP::Qu");
332
3/16
✗ Branch 2 not taken.
✓ Branch 3 taken 1109 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 1109 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 1109 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
 1109 START_PROFILER("SolverDDP::Quu");
333 1109 Quu_[t] = data->Luu;
334
2/4
✓ Branch 5 taken 1109 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1109 times.
✗ Branch 9 not taken.
 1109 Quu_[t].noalias() += FuTVxx_p_[t] * data->Fu;
335
3/16
✗ Branch 2 not taken.
✓ Branch 3 taken 1109 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 1109 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 1109 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
 1109 STOP_PROFILER("SolverDDP::Quu");
336
3/16
✗ Branch 2 not taken.
✓ Branch 3 taken 1109 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 1109 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 1109 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
 1109 START_PROFILER("SolverDDP::Qxu");
337 1109 Qxu_[t] = data->Lxu;
338
2/4
✓ Branch 4 taken 1109 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1109 times.
✗ Branch 8 not taken.
 1109 Qxu_[t].noalias() += FxTVxx_p_ * data->Fu;
339
3/16
✗ Branch 2 not taken.
✓ Branch 3 taken 1109 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 1109 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 1109 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
 1109 STOP_PROFILER("SolverDDP::Qxu");
340
1/2
✓ Branch 1 taken 1109 times.
✗ Branch 2 not taken.
 1109 if (!std::isnan(preg_)) {
341
2/4
✓ Branch 3 taken 1109 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1109 times.
✗ Branch 7 not taken.
 1109 Quu_[t].diagonal().array() += preg_;
342 }
343 }
344 1109 }
345
346 1109 void SolverDDP::computeValueFunction(
347 const std::size_t t, const std::shared_ptr<ActionModelAbstract>& model) {
348
1/14
✗ Branch 2 not taken.
✓ Branch 3 taken 1109 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
 1109 assert_pretty(t < problem_->get_T(),
349 "Invalid argument: t should be between 0 and " +
350 std::to_string(problem_->get_T()););
351 1109 const std::size_t nu = model->get_nu();
352 1109 Vx_[t] = Qx_[t];
353 1109 Vxx_[t] = Qxx_[t];
354
1/2
✓ Branch 0 taken 1109 times.
✗ Branch 1 not taken.
 1109 if (nu != 0) {
355
3/16
✗ Branch 2 not taken.
✓ Branch 3 taken 1109 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 1109 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 1109 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
 1109 START_PROFILER("SolverDDP::Vx");
356
2/4
✓ Branch 5 taken 1109 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1109 times.
✗ Branch 9 not taken.
 1109 Quuk_[t].noalias() = Quu_[t] * k_[t];
357
3/6
✓ Branch 4 taken 1109 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1109 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1109 times.
✗ Branch 12 not taken.
 1109 Vx_[t].noalias() -= K_[t].transpose() * Qu_[t];
358
3/16
✗ Branch 2 not taken.
✓ Branch 3 taken 1109 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 1109 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 1109 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
 1109 STOP_PROFILER("SolverDDP::Vx");
359
3/16
✗ Branch 2 not taken.
✓ Branch 3 taken 1109 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 1109 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 1109 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
 1109 START_PROFILER("SolverDDP::Vxx");
360
2/4
✓ Branch 5 taken 1109 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1109 times.
✗ Branch 9 not taken.
 1109 Vxx_[t].noalias() -= Qxu_[t] * K_[t];
361
3/16
✗ Branch 2 not taken.
✓ Branch 3 taken 1109 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 1109 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 1109 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
 1109 STOP_PROFILER("SolverDDP::Vxx");
362 }
363
3/6
✓ Branch 4 taken 1109 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1109 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1109 times.
✗ Branch 11 not taken.
 1109 Vxx_tmp_ = 0.5 * (Vxx_[t] + Vxx_[t].transpose());
364 1109 Vxx_[t] = Vxx_tmp_;
365
366
1/2
✓ Branch 1 taken 1109 times.
✗ Branch 2 not taken.
 1109 if (!std::isnan(preg_)) {
367
2/4
✓ Branch 3 taken 1109 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1109 times.
✗ Branch 7 not taken.
 1109 Vxx_[t].diagonal().array() += preg_;
368 }
369
370 // Compute and store the Vx gradient at end of the interval (rollout state)
371
2/2
✓ Branch 0 taken 406 times.
✓ Branch 1 taken 703 times.
 1109 if (!is_feasible_) {
372
2/4
✓ Branch 5 taken 406 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 406 times.
✗ Branch 9 not taken.
 406 Vx_[t].noalias() += Vxx_[t] * fs_[t];
373 }
374 1109 }
375
376 1109 void SolverDDP::computeGains(const std::size_t t) {
377
1/14
✗ Branch 2 not taken.
✓ Branch 3 taken 1109 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
 1109 assert_pretty(t < problem_->get_T(),
378 "Invalid argument: t should be between 0 and " +
379 std::to_string(problem_->get_T()));
380
3/16
✗ Branch 2 not taken.
✓ Branch 3 taken 1109 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 1109 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 1109 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
 1109 START_PROFILER("SolverDDP::computeGains");
381 1109 const std::size_t nu = problem_->get_runningModels()[t]->get_nu();
382
1/2
✓ Branch 0 taken 1109 times.
✗ Branch 1 not taken.
 1109 if (nu > 0) {
383
4/18
✓ Branch 1 taken 1109 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1109 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✓ Branch 17 taken 1109 times.
✗ Branch 19 not taken.
✓ Branch 20 taken 1109 times.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
 1109 START_PROFILER("SolverDDP::Quu_inv");
384
1/2
✓ Branch 3 taken 1109 times.
✗ Branch 4 not taken.
 1109 Quu_llt_[t].compute(Quu_[t]);
385
4/18
✓ Branch 1 taken 1109 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1109 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✓ Branch 17 taken 1109 times.
✗ Branch 19 not taken.
✓ Branch 20 taken 1109 times.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
 1109 STOP_PROFILER("SolverDDP::Quu_inv");
386 1109 const Eigen::ComputationInfo& info = Quu_llt_[t].info();
387
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1109 times.
 1109 if (info != Eigen::Success) {
388 ✗ STOP_PROFILER("SolverDDP::computeGains");
389 ✗ throw_pretty("backward_error");
390 }
391
2/4
✓ Branch 2 taken 1109 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1109 times.
✗ Branch 7 not taken.
 1109 K_[t] = Qxu_[t].transpose();
392
393
4/18
✓ Branch 1 taken 1109 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1109 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✓ Branch 17 taken 1109 times.
✗ Branch 19 not taken.
✓ Branch 20 taken 1109 times.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
 1109 START_PROFILER("SolverDDP::Quu_inv_Qux");
394
1/2
✓ Branch 3 taken 1109 times.
✗ Branch 4 not taken.
 1109 Quu_llt_[t].solveInPlace(K_[t]);
395
4/18
✓ Branch 1 taken 1109 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1109 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
✓ Branch 17 taken 1109 times.
✗ Branch 19 not taken.
✓ Branch 20 taken 1109 times.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
 1109 STOP_PROFILER("SolverDDP::Quu_inv_Qux");
396
1/2
✓ Branch 3 taken 1109 times.
✗ Branch 4 not taken.
 1109 k_[t] = Qu_[t];
397
1/2
✓ Branch 3 taken 1109 times.
✗ Branch 4 not taken.
 1109 Quu_llt_[t].solveInPlace(k_[t]);
398 }
399
3/16
✗ Branch 2 not taken.
✓ Branch 3 taken 1109 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 1109 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 1109 times.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
 1109 STOP_PROFILER("SolverDDP::computeGains");
400 1109 }
401
402 ✗ void SolverDDP::increaseRegularization() {
403 ✗ preg_ *= reg_incfactor_;
404 ✗ if (preg_ > reg_max_) {
405 ✗ preg_ = reg_max_;
406 }
407 ✗ dreg_ = preg_;
408 }
409
410 91 void SolverDDP::decreaseRegularization() {
411 91 preg_ /= reg_decfactor_;
412
1/2
✓ Branch 0 taken 91 times.
✗ Branch 1 not taken.
 91 if (preg_ < reg_min_) {
413 91 preg_ = reg_min_;
414 }
415 91 dreg_ = preg_;
416 91 }
417
418 61 void SolverDDP::allocateData() {
419 61 const std::size_t T = problem_->get_T();
420 61 Vxx_.resize(T + 1);
421 61 Vx_.resize(T + 1);
422 61 Qxx_.resize(T);
423 61 Qxu_.resize(T);
424 61 Quu_.resize(T);
425 61 Qx_.resize(T);
426 61 Qu_.resize(T);
427 61 K_.resize(T);
428 61 k_.resize(T);
429
430 61 xs_try_.resize(T + 1);
431 61 us_try_.resize(T);
432 61 dx_.resize(T);
433
434 61 FuTVxx_p_.resize(T);
435 61 Quu_llt_.resize(T);
436 61 Quuk_.resize(T);
437
438 61 const std::size_t ndx = problem_->get_ndx();
439 const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
440 61 problem_->get_runningModels();
441
2/2
✓ Branch 0 taken 589 times.
✓ Branch 1 taken 61 times.
 650 for (std::size_t t = 0; t < T; ++t) {
442 589 const std::shared_ptr<ActionModelAbstract>& model = models[t];
443 589 const std::size_t nu = model->get_nu();
444
2/4
✓ Branch 1 taken 589 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 589 times.
✗ Branch 6 not taken.
 589 Vxx_[t] = Eigen::MatrixXd::Zero(ndx, ndx);
445
2/4
✓ Branch 1 taken 589 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 589 times.
✗ Branch 6 not taken.
 589 Vx_[t] = Eigen::VectorXd::Zero(ndx);
446
2/4
✓ Branch 1 taken 589 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 589 times.
✗ Branch 6 not taken.
 589 Qxx_[t] = Eigen::MatrixXd::Zero(ndx, ndx);
447
2/4
✓ Branch 1 taken 589 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 589 times.
✗ Branch 6 not taken.
 589 Qxu_[t] = Eigen::MatrixXd::Zero(ndx, nu);
448
2/4
✓ Branch 1 taken 589 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 589 times.
✗ Branch 6 not taken.
 589 Quu_[t] = Eigen::MatrixXd::Zero(nu, nu);
449
2/4
✓ Branch 1 taken 589 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 589 times.
✗ Branch 6 not taken.
 589 Qx_[t] = Eigen::VectorXd::Zero(ndx);
450
2/4
✓ Branch 1 taken 589 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 589 times.
✗ Branch 6 not taken.
 589 Qu_[t] = Eigen::VectorXd::Zero(nu);
451
2/4
✓ Branch 1 taken 589 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 589 times.
✗ Branch 6 not taken.
 589 K_[t] = MatrixXdRowMajor::Zero(nu, ndx);
452
2/4
✓ Branch 1 taken 589 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 589 times.
✗ Branch 6 not taken.
 589 k_[t] = Eigen::VectorXd::Zero(nu);
453
454
2/2
✓ Branch 0 taken 61 times.
✓ Branch 1 taken 528 times.
 589 if (t == 0) {
455
1/2
✓ Branch 4 taken 61 times.
✗ Branch 5 not taken.
 61 xs_try_[t] = problem_->get_x0();
456 } else {
457
1/2
✓ Branch 4 taken 528 times.
✗ Branch 5 not taken.
 528 xs_try_[t] = model->get_state()->zero();
458 }
459
2/4
✓ Branch 1 taken 589 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 589 times.
✗ Branch 6 not taken.
 589 us_try_[t] = Eigen::VectorXd::Zero(nu);
460
2/4
✓ Branch 1 taken 589 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 589 times.
✗ Branch 6 not taken.
 589 dx_[t] = Eigen::VectorXd::Zero(ndx);
461
462
2/4
✓ Branch 1 taken 589 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 589 times.
✗ Branch 6 not taken.
 589 FuTVxx_p_[t] = MatrixXdRowMajor::Zero(nu, ndx);
463
1/2
✓ Branch 1 taken 589 times.
✗ Branch 2 not taken.
 589 Quu_llt_[t] = Eigen::LLT<Eigen::MatrixXd>(nu);
464
1/2
✓ Branch 1 taken 589 times.
✗ Branch 2 not taken.
 589 Quuk_[t] = Eigen::VectorXd(nu);
465 }
466
1/2
✓ Branch 3 taken 61 times.
✗ Branch 4 not taken.
 61 Vxx_.back() = Eigen::MatrixXd::Zero(ndx, ndx);
467
1/2
✓ Branch 2 taken 61 times.
✗ Branch 3 not taken.
 61 Vxx_tmp_ = Eigen::MatrixXd::Zero(ndx, ndx);
468
1/2
✓ Branch 3 taken 61 times.
✗ Branch 4 not taken.
 61 Vx_.back() = Eigen::VectorXd::Zero(ndx);
469 61 xs_try_.back() = problem_->get_terminalModel()->get_state()->zero();
470
471
1/2
✓ Branch 2 taken 61 times.
✗ Branch 3 not taken.
 61 FxTVxx_p_ = MatrixXdRowMajor::Zero(ndx, ndx);
472
1/2
✓ Branch 2 taken 61 times.
✗ Branch 3 not taken.
 61 fTVxx_p_ = Eigen::VectorXd::Zero(ndx);
473 61 }
474
475 ✗ double SolverDDP::get_reg_incfactor() const { return reg_incfactor_; }
476
477 ✗ double SolverDDP::get_reg_decfactor() const { return reg_decfactor_; }
478
479 ✗ double SolverDDP::get_regfactor() const { return reg_incfactor_; }
480
481 ✗ double SolverDDP::get_reg_min() const { return reg_min_; }
482
483 ✗ double SolverDDP::get_regmin() const { return reg_min_; }
484
485 ✗ double SolverDDP::get_reg_max() const { return reg_max_; }
486
487 ✗ double SolverDDP::get_regmax() const { return reg_max_; }
488
489 ✗ const std::vector<double>& SolverDDP::get_alphas() const { return alphas_; }
490
491 ✗ double SolverDDP::get_th_stepdec() const { return th_stepdec_; }
492
493 ✗ double SolverDDP::get_th_stepinc() const { return th_stepinc_; }
494
495 ✗ double SolverDDP::get_th_grad() const { return th_grad_; }
496
497 4 const std::vector<Eigen::MatrixXd>& SolverDDP::get_Vxx() const { return Vxx_; }
498
499 4 const std::vector<Eigen::VectorXd>& SolverDDP::get_Vx() const { return Vx_; }
500
501 4 const std::vector<Eigen::MatrixXd>& SolverDDP::get_Qxx() const { return Qxx_; }
502
503 4 const std::vector<Eigen::MatrixXd>& SolverDDP::get_Qxu() const { return Qxu_; }
504
505 4 const std::vector<Eigen::MatrixXd>& SolverDDP::get_Quu() const { return Quu_; }
506
507 4 const std::vector<Eigen::VectorXd>& SolverDDP::get_Qx() const { return Qx_; }
508
509 4 const std::vector<Eigen::VectorXd>& SolverDDP::get_Qu() const { return Qu_; }
510
511 const std::vector<typename MathBaseTpl<double>::MatrixXsRowMajor>&
512 ✗ SolverDDP::get_K() const {
513 ✗ return K_;
514 }
515
516 4 const std::vector<Eigen::VectorXd>& SolverDDP::get_k() const { return k_; }
517
518 ✗ void SolverDDP::set_reg_incfactor(const double regfactor) {
519 ✗ if (regfactor <= 1.) {
520 ✗ throw_pretty(
521 "Invalid argument: " << "reg_incfactor value is higher than 1.");
522 }
523 ✗ reg_incfactor_ = regfactor;
524 }
525
526 ✗ void SolverDDP::set_reg_decfactor(const double regfactor) {
527 ✗ if (regfactor <= 1.) {
528 ✗ throw_pretty(
529 "Invalid argument: " << "reg_decfactor value is higher than 1.");
530 }
531 ✗ reg_decfactor_ = regfactor;
532 }
533
534 ✗ void SolverDDP::set_regfactor(const double regfactor) {
535 ✗ if (regfactor <= 1.) {
536 ✗ throw_pretty("Invalid argument: " << "regfactor value is higher than 1.");
537 }
538 ✗ set_reg_incfactor(regfactor);
539 ✗ set_reg_decfactor(regfactor);
540 }
541
542 ✗ void SolverDDP::set_reg_min(const double regmin) {
543 ✗ if (0. > regmin) {
544 ✗ throw_pretty("Invalid argument: " << "regmin value has to be positive.");
545 }
546 ✗ reg_min_ = regmin;
547 }
548
549 ✗ void SolverDDP::set_regmin(const double regmin) {
550 ✗ if (0. > regmin) {
551 ✗ throw_pretty("Invalid argument: " << "regmin value has to be positive.");
552 }
553 ✗ reg_min_ = regmin;
554 }
555
556 ✗ void SolverDDP::set_reg_max(const double regmax) {
557 ✗ if (0. > regmax) {
558 ✗ throw_pretty("Invalid argument: " << "regmax value has to be positive.");
559 }
560 ✗ reg_max_ = regmax;
561 }
562
563 ✗ void SolverDDP::set_regmax(const double regmax) {
564 ✗ if (0. > regmax) {
565 ✗ throw_pretty("Invalid argument: " << "regmax value has to be positive.");
566 }
567 ✗ reg_max_ = regmax;
568 }
569
570 ✗ void SolverDDP::set_alphas(const std::vector<double>& alphas) {
571 ✗ double prev_alpha = alphas[0];
572 ✗ if (prev_alpha != 1.) {
573 ✗ std::cerr << "Warning: alpha[0] should be 1" << std::endl;
574 }
575 ✗ for (std::size_t i = 1; i < alphas.size(); ++i) {
576 ✗ double alpha = alphas[i];
577 ✗ if (0. >= alpha) {
578 ✗ throw_pretty("Invalid argument: " << "alpha values has to be positive.");
579 }
580 ✗ if (alpha >= prev_alpha) {
581 ✗ throw_pretty(
582 "Invalid argument: " << "alpha values are monotonously decreasing.");
583 }
584 ✗ prev_alpha = alpha;
585 }
586 ✗ alphas_ = alphas;
587 }
588
589 ✗ void SolverDDP::set_th_stepdec(const double th_stepdec) {
590 ✗ if (0. >= th_stepdec || th_stepdec > 1.) {
591 ✗ throw_pretty(
592 "Invalid argument: " << "th_stepdec value should between 0 and 1.");
593 }
594 ✗ th_stepdec_ = th_stepdec;
595 }
596
597 ✗ void SolverDDP::set_th_stepinc(const double th_stepinc) {
598 ✗ if (0. >= th_stepinc || th_stepinc > 1.) {
599 ✗ throw_pretty(
600 "Invalid argument: " << "th_stepinc value should between 0 and 1.");
601 }
602 ✗ th_stepinc_ = th_stepinc;
603 }
604
605 ✗ void SolverDDP::set_th_grad(const double th_grad) {
606 ✗ if (0. > th_grad) {
607 ✗ throw_pretty("Invalid argument: " << "th_grad value has to be positive.");
608 }
609 ✗ th_grad_ = th_grad;
610 }
611
612 } // namespace crocoddyl
613