GCC Code Coverage Report

Directory: ./
File: src/core/solvers/kkt.cpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 196 218 89.9%
Branches: 152 290 52.4%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2020, LAAS-CNRS, New York University, Max Planck
5 // Gesellschaft,
6 // University of Edinburgh
7 // Copyright note valid unless otherwise stated in individual files.
8 // All rights reserved.
9 ///////////////////////////////////////////////////////////////////////////////
10
11 #include "crocoddyl/core/solvers/kkt.hpp"
12
13 namespace crocoddyl {
14
15 40 SolverKKT::SolverKKT(std::shared_ptr<ShootingProblem> problem)
16 : SolverAbstract(problem),
17 40 reg_incfactor_(10.),
18 40 reg_decfactor_(10.),
19 40 reg_min_(1e-9),
20 40 reg_max_(1e9),
21 40 cost_try_(0.),
22 40 th_grad_(1e-12),
23
8/16
✓ Branch 2 taken 40 times.
✗ Branch 3 not taken.
✓ Branch 11 taken 40 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 40 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 40 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 40 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 40 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 40 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 40 times.
✗ Branch 31 not taken.
 40 was_feasible_(false) {
24
1/2
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
 40 allocateData();
25 40 const std::size_t n_alphas = 10;
26 40 preg_ = 0.;
27 40 dreg_ = 0.;
28
1/2
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
 40 alphas_.resize(n_alphas);
29
2/2
✓ Branch 0 taken 400 times.
✓ Branch 1 taken 40 times.
 440 for (std::size_t n = 0; n < n_alphas; ++n) {
30 400 alphas_[n] = 1. / pow(2., (double)n);
31 }
32 40 }
33
34 84 SolverKKT::~SolverKKT() {}
35
36 25 bool SolverKKT::solve(const std::vector<Eigen::VectorXd>& init_xs,
37 const std::vector<Eigen::VectorXd>& init_us,
38 const std::size_t maxiter, const bool is_feasible,
39 const double) {
40 25 setCandidate(init_xs, init_us, is_feasible);
41 25 bool recalc = true;
42
1/2
✓ Branch 0 taken 60 times.
✗ Branch 1 not taken.
 60 for (iter_ = 0; iter_ < maxiter; ++iter_) {
43 while (true) {
44 try {
45
1/2
✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
 60 computeDirection(recalc);
46 ✗ } catch (std::exception& e) {
47 ✗ recalc = false;
48 ✗ if (preg_ == reg_max_) {
49 ✗ return false;
50 } else {
51 ✗ continue;
52 }
53 }
54 60 break;
55 }
56
57 60 expectedImprovement();
58 60 for (std::vector<double>::const_iterator it = alphas_.begin();
59
1/2
✓ Branch 3 taken 60 times.
✗ Branch 4 not taken.
 60 it != alphas_.end(); ++it) {
60 60 steplength_ = *it;
61 try {
62
1/2
✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
 60 dV_ = tryStep(steplength_);
63 ✗ } catch (std::exception& e) {
64 ✗ continue;
65 }
66
2/4
✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60 times.
✗ Branch 5 not taken.
 60 dVexp_ = steplength_ * d_[0] + 0.5 * steplength_ * steplength_ * d_[1];
67
7/10
✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 25 times.
✓ Branch 4 taken 35 times.
✓ Branch 5 taken 10 times.
✓ Branch 6 taken 15 times.
✓ Branch 7 taken 10 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 60 times.
✗ Branch 10 not taken.
 60 if (d_[0] < th_grad_ || !is_feasible_ || dV_ > th_acceptstep_ * dVexp_) {
68 60 was_feasible_ = is_feasible_;
69
1/2
✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
 60 setCandidate(xs_try_, us_try_, true);
70 60 cost_ = cost_try_;
71 60 break;
72 }
73 }
74 60 stoppingCriteria();
75 60 const std::size_t n_callbacks = callbacks_.size();
76
1/2
✓ Branch 0 taken 60 times.
✗ Branch 1 not taken.
 60 if (n_callbacks != 0) {
77
2/2
✓ Branch 0 taken 60 times.
✓ Branch 1 taken 60 times.
 120 for (std::size_t c = 0; c < n_callbacks; ++c) {
78 60 CallbackAbstract& callback = *callbacks_[c];
79 60 callback(*this);
80 }
81 }
82
4/4
✓ Branch 0 taken 35 times.
✓ Branch 1 taken 25 times.
✓ Branch 2 taken 25 times.
✓ Branch 3 taken 10 times.
 60 if (was_feasible_ && stop_ < th_stop_) {
83 25 return true;
84 }
85 }
86 ✗ return false;
87 }
88
89 67 void SolverKKT::computeDirection(const bool recalc) {
90
1/2
✓ Branch 2 taken 67 times.
✗ Branch 3 not taken.
 67 const std::size_t T = problem_->get_T();
91
1/2
✓ Branch 0 taken 67 times.
✗ Branch 1 not taken.
 67 if (recalc) {
92
1/2
✓ Branch 1 taken 67 times.
✗ Branch 2 not taken.
 67 calcDiff();
93 }
94
1/2
✓ Branch 1 taken 67 times.
✗ Branch 2 not taken.
 67 computePrimalDual();
95 const Eigen::VectorBlock<Eigen::VectorXd, Eigen::Dynamic> p_x =
96
1/2
✓ Branch 1 taken 67 times.
✗ Branch 2 not taken.
 67 primal_.segment(0, ndx_);
97 const Eigen::VectorBlock<Eigen::VectorXd, Eigen::Dynamic> p_u =
98
1/2
✓ Branch 1 taken 67 times.
✗ Branch 2 not taken.
 67 primal_.segment(ndx_, nu_);
99
100 67 std::size_t ix = 0;
101 67 std::size_t iu = 0;
102 const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
103
1/2
✓ Branch 2 taken 67 times.
✗ Branch 3 not taken.
 67 problem_->get_runningModels();
104
2/2
✓ Branch 0 taken 670 times.
✓ Branch 1 taken 67 times.
 737 for (std::size_t t = 0; t < T; ++t) {
105
2/4
✓ Branch 3 taken 670 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 670 times.
✗ Branch 8 not taken.
 670 const std::size_t ndxi = models[t]->get_state()->get_ndx();
106
1/2
✓ Branch 3 taken 670 times.
✗ Branch 4 not taken.
 670 const std::size_t nui = models[t]->get_nu();
107
2/4
✓ Branch 1 taken 670 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 670 times.
✗ Branch 6 not taken.
 670 dxs_[t] = p_x.segment(ix, ndxi);
108
2/4
✓ Branch 1 taken 670 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 670 times.
✗ Branch 6 not taken.
 670 dus_[t] = p_u.segment(iu, nui);
109
2/4
✓ Branch 1 taken 670 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 670 times.
✗ Branch 6 not taken.
 670 lambdas_[t] = dual_.segment(ix, ndxi);
110 670 ix += ndxi;
111 670 iu += nui;
112 }
113 const std::size_t ndxi =
114
3/6
✓ Branch 2 taken 67 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 67 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 67 times.
✗ Branch 11 not taken.
 67 problem_->get_terminalModel()->get_state()->get_ndx();
115
2/4
✓ Branch 1 taken 67 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 67 times.
✗ Branch 6 not taken.
 67 dxs_.back() = p_x.segment(ix, ndxi);
116
2/4
✓ Branch 1 taken 67 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 67 times.
✗ Branch 6 not taken.
 67 lambdas_.back() = dual_.segment(ix, ndxi);
117 67 }
118
119 60 double SolverKKT::tryStep(const double steplength) {
120
1/2
✓ Branch 2 taken 60 times.
✗ Branch 3 not taken.
 60 const std::size_t T = problem_->get_T();
121 const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
122
1/2
✓ Branch 2 taken 60 times.
✗ Branch 3 not taken.
 60 problem_->get_runningModels();
123
2/2
✓ Branch 0 taken 600 times.
✓ Branch 1 taken 60 times.
 660 for (std::size_t t = 0; t < T; ++t) {
124 600 const std::shared_ptr<ActionModelAbstract>& m = models[t];
125
126
6/12
✓ Branch 2 taken 600 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 600 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 600 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 600 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 600 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 600 times.
✗ Branch 22 not taken.
 600 m->get_state()->integrate(xs_[t], steplength * dxs_[t], xs_try_[t]);
127
2/4
✓ Branch 2 taken 600 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 600 times.
✗ Branch 5 not taken.
 600 if (m->get_nu() != 0) {
128
1/2
✓ Branch 3 taken 600 times.
✗ Branch 4 not taken.
 600 us_try_[t] = us_[t];
129
2/4
✓ Branch 2 taken 600 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 600 times.
✗ Branch 7 not taken.
 600 us_try_[t] += steplength * dus_[t];
130 }
131 }
132
1/2
✓ Branch 2 taken 60 times.
✗ Branch 3 not taken.
 60 const std::shared_ptr<ActionModelAbstract> m = problem_->get_terminalModel();
133
6/12
✓ Branch 2 taken 60 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 60 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 60 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 60 times.
✗ Branch 15 not taken.
✓ Branch 18 taken 60 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 60 times.
✗ Branch 22 not taken.
 60 m->get_state()->integrate(xs_[T], steplength * dxs_[T], xs_try_[T]);
134
1/2
✓ Branch 2 taken 60 times.
✗ Branch 3 not taken.
 60 cost_try_ = problem_->calc(xs_try_, us_try_);
135 60 return cost_ - cost_try_;
136 60 }
137
138 60 double SolverKKT::stoppingCriteria() {
139 60 const std::size_t T = problem_->get_T();
140 60 std::size_t ix = 0;
141 60 std::size_t iu = 0;
142 const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
143 60 problem_->get_runningModels();
144 const std::vector<std::shared_ptr<ActionDataAbstract> >& datas =
145 60 problem_->get_runningDatas();
146
2/2
✓ Branch 0 taken 600 times.
✓ Branch 1 taken 60 times.
 660 for (std::size_t t = 0; t < T; ++t) {
147 600 const std::shared_ptr<ActionDataAbstract>& d = datas[t];
148 600 const std::size_t ndxi = models[t]->get_state()->get_ndx();
149 600 const std::size_t nui = models[t]->get_nu();
150
151
1/2
✓ Branch 3 taken 600 times.
✗ Branch 4 not taken.
 600 dF.segment(ix, ndxi) = lambdas_[t];
152
4/8
✓ Branch 4 taken 600 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 600 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 600 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 600 times.
✗ Branch 14 not taken.
 600 dF.segment(ix, ndxi).noalias() -= d->Fx.transpose() * lambdas_[t + 1];
153
5/10
✓ Branch 4 taken 600 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 600 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 600 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 600 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 600 times.
✗ Branch 17 not taken.
 600 dF.segment(ndx_ + iu, nui).noalias() = -lambdas_[t + 1].transpose() * d->Fu;
154 600 ix += ndxi;
155 600 iu += nui;
156 }
157 const std::size_t ndxi =
158 60 problem_->get_terminalModel()->get_state()->get_ndx();
159
1/2
✓ Branch 3 taken 60 times.
✗ Branch 4 not taken.
 60 dF.segment(ix, ndxi) = lambdas_.back();
160
2/4
✓ Branch 2 taken 60 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 60 times.
✗ Branch 6 not taken.
 60 stop_ = (kktref_.segment(0, ndx_ + nu_) + dF).squaredNorm() +
161
2/4
✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60 times.
✗ Branch 5 not taken.
 60 kktref_.segment(ndx_ + nu_, ndx_).squaredNorm();
162 60 return stop_;
163 }
164
165 60 const Eigen::Vector2d& SolverKKT::expectedImprovement() {
166
1/2
✓ Branch 2 taken 60 times.
✗ Branch 3 not taken.
 60 d_ = Eigen::Vector2d::Zero();
167 // -grad^T.primal
168
2/4
✓ Branch 2 taken 60 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 60 times.
✗ Branch 6 not taken.
 60 d_(0) = -kktref_.segment(0, ndx_ + nu_).dot(primal_);
169 // -(hessian.primal)^T.primal
170
3/6
✓ Branch 2 taken 60 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 60 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 60 times.
✗ Branch 9 not taken.
 60 kkt_primal_.noalias() = kkt_.block(0, 0, ndx_ + nu_, ndx_ + nu_) * primal_;
171 60 d_(1) = -kkt_primal_.dot(primal_);
172 60 return d_;
173 }
174
175 21 const Eigen::MatrixXd& SolverKKT::get_kkt() const { return kkt_; }
176
177 7 const Eigen::VectorXd& SolverKKT::get_kktref() const { return kktref_; }
178
179 7 const Eigen::VectorXd& SolverKKT::get_primaldual() const { return primaldual_; }
180
181 7 const std::vector<Eigen::VectorXd>& SolverKKT::get_dxs() const { return dxs_; }
182
183 ✗ const std::vector<Eigen::VectorXd>& SolverKKT::get_dus() const { return dus_; }
184
185 ✗ const std::vector<Eigen::VectorXd>& SolverKKT::get_lambdas() const {
186 ✗ return lambdas_;
187 }
188
189 ✗ std::size_t SolverKKT::get_nx() const { return nx_; }
190
191 14 std::size_t SolverKKT::get_ndx() const { return ndx_; }
192
193 14 std::size_t SolverKKT::get_nu() const { return nu_; }
194
195 67 double SolverKKT::calcDiff() {
196 67 cost_ = problem_->calc(xs_, us_);
197 67 cost_ = problem_->calcDiff(xs_, us_);
198
199 // offset on constraint xnext = f(x,u) due to x0 = ref.
200 const std::size_t cx0 =
201 67 problem_->get_runningModels()[0]->get_state()->get_ndx();
202
203 67 std::size_t ix = 0;
204 67 std::size_t iu = 0;
205 67 const std::size_t T = problem_->get_T();
206
1/2
✓ Branch 2 taken 67 times.
✗ Branch 3 not taken.
 67 kkt_.block(ndx_ + nu_, 0, ndx_, ndx_).setIdentity();
207
2/2
✓ Branch 0 taken 670 times.
✓ Branch 1 taken 67 times.
 737 for (std::size_t t = 0; t < T; ++t) {
208 const std::shared_ptr<ActionModelAbstract>& m =
209 670 problem_->get_runningModels()[t];
210 const std::shared_ptr<ActionDataAbstract>& d =
211 670 problem_->get_runningDatas()[t];
212 670 const std::size_t ndxi = m->get_state()->get_ndx();
213 670 const std::size_t nui = m->get_nu();
214
215 // Computing the gap at the initial state
216
2/2
✓ Branch 0 taken 67 times.
✓ Branch 1 taken 603 times.
 670 if (t == 0) {
217
5/10
✓ Branch 4 taken 67 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 67 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 67 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 67 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 67 times.
✗ Branch 19 not taken.
 134 m->get_state()->diff(problem_->get_x0(), xs_[0],
218 134 kktref_.segment(ndx_ + nu_, ndxi));
219 }
220
221 // Filling KKT matrix
222
1/2
✓ Branch 3 taken 670 times.
✗ Branch 4 not taken.
 670 kkt_.block(ix, ix, ndxi, ndxi) = d->Lxx;
223
1/2
✓ Branch 3 taken 670 times.
✗ Branch 4 not taken.
 670 kkt_.block(ix, ndx_ + iu, ndxi, nui) = d->Lxu;
224
2/4
✓ Branch 3 taken 670 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 670 times.
✗ Branch 7 not taken.
 670 kkt_.block(ndx_ + iu, ix, nui, ndxi) = d->Lxu.transpose();
225
1/2
✓ Branch 3 taken 670 times.
✗ Branch 4 not taken.
 670 kkt_.block(ndx_ + iu, ndx_ + iu, nui, nui) = d->Luu;
226
2/4
✓ Branch 3 taken 670 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 670 times.
✗ Branch 7 not taken.
 670 kkt_.block(ndx_ + nu_ + cx0 + ix, ix, ndxi, ndxi) = -d->Fx;
227
2/4
✓ Branch 3 taken 670 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 670 times.
✗ Branch 7 not taken.
 670 kkt_.block(ndx_ + nu_ + cx0 + ix, ndx_ + iu, ndxi, nui) = -d->Fu;
228
229 // Filling KKT vector
230
1/2
✓ Branch 3 taken 670 times.
✗ Branch 4 not taken.
 670 kktref_.segment(ix, ndxi) = d->Lx;
231
1/2
✓ Branch 3 taken 670 times.
✗ Branch 4 not taken.
 670 kktref_.segment(ndx_ + iu, nui) = d->Lu;
232
4/8
✓ Branch 4 taken 670 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 670 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 670 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 670 times.
✗ Branch 16 not taken.
 1340 m->get_state()->diff(d->xnext, xs_[t + 1],
233 670 kktref_.segment(ndx_ + nu_ + cx0 + ix, ndxi));
234
235 670 ix += ndxi;
236 670 iu += nui;
237 }
238 67 const std::shared_ptr<ActionDataAbstract>& df = problem_->get_terminalData();
239 const std::size_t ndxf =
240 67 problem_->get_terminalModel()->get_state()->get_ndx();
241
1/2
✓ Branch 3 taken 67 times.
✗ Branch 4 not taken.
 67 kkt_.block(ix, ix, ndxf, ndxf) = df->Lxx;
242
1/2
✓ Branch 3 taken 67 times.
✗ Branch 4 not taken.
 67 kktref_.segment(ix, ndxf) = df->Lx;
243
1/2
✓ Branch 1 taken 67 times.
✗ Branch 2 not taken.
 67 kkt_.block(0, ndx_ + nu_, ndx_ + nu_, ndx_) =
244
2/4
✓ Branch 2 taken 67 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 67 times.
✗ Branch 6 not taken.
 134 kkt_.block(ndx_ + nu_, 0, ndx_, ndx_ + nu_).transpose();
245 67 return cost_;
246 }
247
248 67 void SolverKKT::computePrimalDual() {
249
3/6
✓ Branch 2 taken 67 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 67 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 67 times.
✗ Branch 9 not taken.
 67 primaldual_ = kkt_.lu().solve(-kktref_);
250
1/2
✓ Branch 2 taken 67 times.
✗ Branch 3 not taken.
 67 primal_ = primaldual_.segment(0, ndx_ + nu_);
251
1/2
✓ Branch 2 taken 67 times.
✗ Branch 3 not taken.
 67 dual_ = primaldual_.segment(ndx_ + nu_, ndx_);
252 67 }
253
254 ✗ void SolverKKT::increaseRegularization() {
255 ✗ preg_ *= reg_incfactor_;
256 ✗ if (preg_ > reg_max_) {
257 ✗ preg_ = reg_max_;
258 }
259 ✗ dreg_ = preg_;
260 }
261
262 ✗ void SolverKKT::decreaseRegularization() {
263 ✗ preg_ /= reg_decfactor_;
264 ✗ if (preg_ < reg_min_) {
265 ✗ preg_ = reg_min_;
266 }
267 ✗ dreg_ = preg_;
268 }
269
270 40 void SolverKKT::allocateData() {
271 40 const std::size_t T = problem_->get_T();
272 40 dxs_.resize(T + 1);
273 40 dus_.resize(T);
274 40 lambdas_.resize(T + 1);
275 40 xs_try_.resize(T + 1);
276 40 us_try_.resize(T);
277
278 40 nx_ = 0;
279 40 ndx_ = 0;
280 40 nu_ = 0;
281 40 const std::size_t nx = problem_->get_nx();
282 40 const std::size_t ndx = problem_->get_ndx();
283 const std::vector<std::shared_ptr<ActionModelAbstract> >& models =
284 40 problem_->get_runningModels();
285
2/2
✓ Branch 0 taken 400 times.
✓ Branch 1 taken 40 times.
 440 for (std::size_t t = 0; t < T; ++t) {
286 400 const std::shared_ptr<ActionModelAbstract>& model = models[t];
287
2/2
✓ Branch 0 taken 40 times.
✓ Branch 1 taken 360 times.
 400 if (t == 0) {
288 40 xs_try_[t] = problem_->get_x0();
289 } else {
290
2/4
✓ Branch 1 taken 360 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 360 times.
✗ Branch 6 not taken.
 360 xs_try_[t] = Eigen::VectorXd::Constant(nx, NAN);
291 }
292 400 const std::size_t nu = model->get_nu();
293
2/4
✓ Branch 1 taken 400 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 400 times.
✗ Branch 6 not taken.
 400 us_try_[t] = Eigen::VectorXd::Constant(nu, NAN);
294
1/2
✓ Branch 3 taken 400 times.
✗ Branch 4 not taken.
 400 dxs_[t] = Eigen::VectorXd::Zero(ndx);
295
1/2
✓ Branch 3 taken 400 times.
✗ Branch 4 not taken.
 400 dus_[t] = Eigen::VectorXd::Zero(nu);
296
1/2
✓ Branch 3 taken 400 times.
✗ Branch 4 not taken.
 400 lambdas_[t] = Eigen::VectorXd::Zero(ndx);
297 400 nx_ += nx;
298 400 ndx_ += ndx;
299 400 nu_ += nu;
300 }
301 40 nx_ += nx;
302 40 ndx_ += ndx;
303 40 xs_try_.back() = problem_->get_terminalModel()->get_state()->zero();
304
1/2
✓ Branch 3 taken 40 times.
✗ Branch 4 not taken.
 40 dxs_.back() = Eigen::VectorXd::Zero(ndx);
305
1/2
✓ Branch 3 taken 40 times.
✗ Branch 4 not taken.
 40 lambdas_.back() = Eigen::VectorXd::Zero(ndx);
306
307 // Set dimensions for kkt matrix and kkt_ref vector
308 40 kkt_.resize(2 * ndx_ + nu_, 2 * ndx_ + nu_);
309 40 kkt_.setZero();
310 40 kktref_.resize(2 * ndx_ + nu_);
311 40 kktref_.setZero();
312 40 primaldual_.resize(2 * ndx_ + nu_);
313 40 primaldual_.setZero();
314 40 primal_.resize(ndx_ + nu_);
315 40 primal_.setZero();
316 40 kkt_primal_.resize(ndx_ + nu_);
317 40 kkt_primal_.setZero();
318 40 dual_.resize(ndx_);
319 40 dual_.setZero();
320 40 dF.resize(ndx_ + nu_);
321 40 dF.setZero();
322 40 }
323
324 } // namespace crocoddyl
325