GCC Code Coverage Report

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