GCC Code Coverage Report

Directory: ./
File: src/core/solvers/kkt.cpp
Date: 2025-01-16 08:47:40
Exec Total Coverage
Lines: 196 218 89.9%
Branches: 137 260 52.7%
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(boost::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 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<boost::shared_ptr<ActionModelAbstract> >& models =
103 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 670 const std::size_t ndxi = models[t]->get_state()->get_ndx();
106 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 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 60 const std::size_t T = problem_->get_T();
121 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
122 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 boost::shared_ptr<ActionModelAbstract>& m = models[t];
125
126
5/10
✓ Branch 5 taken 600 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 600 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 600 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 600 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 600 times.
✗ Branch 20 not taken.
 600 m->get_state()->integrate(xs_[t], steplength * dxs_[t], xs_try_[t]);
127
1/2
✓ Branch 2 taken 600 times.
✗ Branch 3 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 const boost::shared_ptr<ActionModelAbstract> m =
133 60 problem_->get_terminalModel();
134
5/10
✓ Branch 5 taken 60 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 60 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 60 times.
✗ Branch 13 not taken.
✓ Branch 16 taken 60 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 60 times.
✗ Branch 20 not taken.
 60 m->get_state()->integrate(xs_[T], steplength * dxs_[T], xs_try_[T]);
135
1/2
✓ Branch 2 taken 60 times.
✗ Branch 3 not taken.
 60 cost_try_ = problem_->calc(xs_try_, us_try_);
136 60 return cost_ - cost_try_;
137 60 }
138
139 60 double SolverKKT::stoppingCriteria() {
140 60 const std::size_t T = problem_->get_T();
141 60 std::size_t ix = 0;
142 60 std::size_t iu = 0;
143 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
144 60 problem_->get_runningModels();
145 const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
146 60 problem_->get_runningDatas();
147
2/2
✓ Branch 0 taken 600 times.
✓ Branch 1 taken 60 times.
 660 for (std::size_t t = 0; t < T; ++t) {
148 600 const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
149 600 const std::size_t ndxi = models[t]->get_state()->get_ndx();
150 600 const std::size_t nui = models[t]->get_nu();
151
152
1/2
✓ Branch 3 taken 600 times.
✗ Branch 4 not taken.
 600 dF.segment(ix, ndxi) = lambdas_[t];
153
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];
154
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;
155 600 ix += ndxi;
156 600 iu += nui;
157 }
158 const std::size_t ndxi =
159 60 problem_->get_terminalModel()->get_state()->get_ndx();
160
1/2
✓ Branch 3 taken 60 times.
✗ Branch 4 not taken.
 60 dF.segment(ix, ndxi) = lambdas_.back();
161
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() +
162
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();
163 60 return stop_;
164 }
165
166 60 const Eigen::Vector2d& SolverKKT::expectedImprovement() {
167
1/2
✓ Branch 2 taken 60 times.
✗ Branch 3 not taken.
 60 d_ = Eigen::Vector2d::Zero();
168 // -grad^T.primal
169
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_);
170 // -(hessian.primal)^T.primal
171
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_;
172 60 d_(1) = -kkt_primal_.dot(primal_);
173 60 return d_;
174 }
175
176 21 const Eigen::MatrixXd& SolverKKT::get_kkt() const { return kkt_; }
177
178 7 const Eigen::VectorXd& SolverKKT::get_kktref() const { return kktref_; }
179
180 7 const Eigen::VectorXd& SolverKKT::get_primaldual() const { return primaldual_; }
181
182 7 const std::vector<Eigen::VectorXd>& SolverKKT::get_dxs() const { return dxs_; }
183
184 ✗ const std::vector<Eigen::VectorXd>& SolverKKT::get_dus() const { return dus_; }
185
186 ✗ const std::vector<Eigen::VectorXd>& SolverKKT::get_lambdas() const {
187 ✗ return lambdas_;
188 }
189
190 ✗ std::size_t SolverKKT::get_nx() const { return nx_; }
191
192 14 std::size_t SolverKKT::get_ndx() const { return ndx_; }
193
194 14 std::size_t SolverKKT::get_nu() const { return nu_; }
195
196 67 double SolverKKT::calcDiff() {
197 67 cost_ = problem_->calc(xs_, us_);
198 67 cost_ = problem_->calcDiff(xs_, us_);
199
200 // offset on constraint xnext = f(x,u) due to x0 = ref.
201 const std::size_t cx0 =
202 67 problem_->get_runningModels()[0]->get_state()->get_ndx();
203
204 67 std::size_t ix = 0;
205 67 std::size_t iu = 0;
206 67 const std::size_t T = problem_->get_T();
207
1/2
✓ Branch 2 taken 67 times.
✗ Branch 3 not taken.
 67 kkt_.block(ndx_ + nu_, 0, ndx_, ndx_).setIdentity();
208
2/2
✓ Branch 0 taken 670 times.
✓ Branch 1 taken 67 times.
 737 for (std::size_t t = 0; t < T; ++t) {
209 const boost::shared_ptr<ActionModelAbstract>& m =
210 670 problem_->get_runningModels()[t];
211 const boost::shared_ptr<ActionDataAbstract>& d =
212 670 problem_->get_runningDatas()[t];
213 670 const std::size_t ndxi = m->get_state()->get_ndx();
214 670 const std::size_t nui = m->get_nu();
215
216 // Computing the gap at the initial state
217
2/2
✓ Branch 0 taken 67 times.
✓ Branch 1 taken 603 times.
 670 if (t == 0) {
218
4/8
✓ Branch 4 taken 67 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 67 times.
✗ Branch 9 not taken.
✓ Branch 13 taken 67 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 67 times.
✗ Branch 17 not taken.
 134 m->get_state()->diff(problem_->get_x0(), xs_[0],
219 134 kktref_.segment(ndx_ + nu_, ndxi));
220 }
221
222 // Filling KKT matrix
223
1/2
✓ Branch 3 taken 670 times.
✗ Branch 4 not taken.
 670 kkt_.block(ix, ix, ndxi, ndxi) = d->Lxx;
224
1/2
✓ Branch 3 taken 670 times.
✗ Branch 4 not taken.
 670 kkt_.block(ix, ndx_ + iu, ndxi, nui) = d->Lxu;
225
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();
226
1/2
✓ Branch 3 taken 670 times.
✗ Branch 4 not taken.
 670 kkt_.block(ndx_ + iu, ndx_ + iu, nui, nui) = d->Luu;
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, ix, ndxi, ndxi) = -d->Fx;
228
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;
229
230 // Filling KKT vector
231
1/2
✓ Branch 3 taken 670 times.
✗ Branch 4 not taken.
 670 kktref_.segment(ix, ndxi) = d->Lx;
232
1/2
✓ Branch 3 taken 670 times.
✗ Branch 4 not taken.
 670 kktref_.segment(ndx_ + iu, nui) = d->Lu;
233
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],
234 670 kktref_.segment(ndx_ + nu_ + cx0 + ix, ndxi));
235
236 670 ix += ndxi;
237 670 iu += nui;
238 }
239 const boost::shared_ptr<ActionDataAbstract>& df =
240 67 problem_->get_terminalData();
241 const std::size_t ndxf =
242 67 problem_->get_terminalModel()->get_state()->get_ndx();
243
1/2
✓ Branch 3 taken 67 times.
✗ Branch 4 not taken.
 67 kkt_.block(ix, ix, ndxf, ndxf) = df->Lxx;
244
1/2
✓ Branch 3 taken 67 times.
✗ Branch 4 not taken.
 67 kktref_.segment(ix, ndxf) = df->Lx;
245
1/2
✓ Branch 1 taken 67 times.
✗ Branch 2 not taken.
 67 kkt_.block(0, ndx_ + nu_, ndx_ + nu_, ndx_) =
246
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();
247 67 return cost_;
248 }
249
250 67 void SolverKKT::computePrimalDual() {
251
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_);
252
1/2
✓ Branch 2 taken 67 times.
✗ Branch 3 not taken.
 67 primal_ = primaldual_.segment(0, ndx_ + nu_);
253
1/2
✓ Branch 2 taken 67 times.
✗ Branch 3 not taken.
 67 dual_ = primaldual_.segment(ndx_ + nu_, ndx_);
254 67 }
255
256 ✗ void SolverKKT::increaseRegularization() {
257 ✗ preg_ *= reg_incfactor_;
258 ✗ if (preg_ > reg_max_) {
259 ✗ preg_ = reg_max_;
260 }
261 ✗ dreg_ = preg_;
262 }
263
264 ✗ void SolverKKT::decreaseRegularization() {
265 ✗ preg_ /= reg_decfactor_;
266 ✗ if (preg_ < reg_min_) {
267 ✗ preg_ = reg_min_;
268 }
269 ✗ dreg_ = preg_;
270 }
271
272 40 void SolverKKT::allocateData() {
273 40 const std::size_t T = problem_->get_T();
274 40 dxs_.resize(T + 1);
275 40 dus_.resize(T);
276 40 lambdas_.resize(T + 1);
277 40 xs_try_.resize(T + 1);
278 40 us_try_.resize(T);
279
280 40 nx_ = 0;
281 40 ndx_ = 0;
282 40 nu_ = 0;
283 40 const std::size_t nx = problem_->get_nx();
284 40 const std::size_t ndx = problem_->get_ndx();
285 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
286 40 problem_->get_runningModels();
287
2/2
✓ Branch 0 taken 400 times.
✓ Branch 1 taken 40 times.
 440 for (std::size_t t = 0; t < T; ++t) {
288 400 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
289
2/2
✓ Branch 0 taken 40 times.
✓ Branch 1 taken 360 times.
 400 if (t == 0) {
290 40 xs_try_[t] = problem_->get_x0();
291 } else {
292
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);
293 }
294 400 const std::size_t nu = model->get_nu();
295
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);
296
1/2
✓ Branch 3 taken 400 times.
✗ Branch 4 not taken.
 400 dxs_[t] = Eigen::VectorXd::Zero(ndx);
297
1/2
✓ Branch 3 taken 400 times.
✗ Branch 4 not taken.
 400 dus_[t] = Eigen::VectorXd::Zero(nu);
298
1/2
✓ Branch 3 taken 400 times.
✗ Branch 4 not taken.
 400 lambdas_[t] = Eigen::VectorXd::Zero(ndx);
299 400 nx_ += nx;
300 400 ndx_ += ndx;
301 400 nu_ += nu;
302 }
303 40 nx_ += nx;
304 40 ndx_ += ndx;
305 40 xs_try_.back() = problem_->get_terminalModel()->get_state()->zero();
306
1/2
✓ Branch 3 taken 40 times.
✗ Branch 4 not taken.
 40 dxs_.back() = Eigen::VectorXd::Zero(ndx);
307
1/2
✓ Branch 3 taken 40 times.
✗ Branch 4 not taken.
 40 lambdas_.back() = Eigen::VectorXd::Zero(ndx);
308
309 // Set dimensions for kkt matrix and kkt_ref vector
310 40 kkt_.resize(2 * ndx_ + nu_, 2 * ndx_ + nu_);
311 40 kkt_.setZero();
312 40 kktref_.resize(2 * ndx_ + nu_);
313 40 kktref_.setZero();
314 40 primaldual_.resize(2 * ndx_ + nu_);
315 40 primaldual_.setZero();
316 40 primal_.resize(ndx_ + nu_);
317 40 primal_.setZero();
318 40 kkt_primal_.resize(ndx_ + nu_);
319 40 kkt_primal_.setZero();
320 40 dual_.resize(ndx_);
321 40 dual_.setZero();
322 40 dF.resize(ndx_ + nu_);
323 40 dF.setZero();
324 40 }
325
326 } // namespace crocoddyl
327