GCC Code Coverage Report

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