GCC Code Coverage Report

Directory: ./
File: src/core/solvers/intro.cpp
Date: 2025-01-16 08:47:40
Exec Total Coverage
Lines: 49 311 15.8%
Branches: 49 805 6.1%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021-2023, Heriot-Watt University, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
7 ///////////////////////////////////////////////////////////////////////////////
8
9 #include "crocoddyl/core/solvers/intro.hpp"
10
11 #include <iostream>
12
13 #include "crocoddyl/core/utils/exception.hpp"
14 #include "crocoddyl/core/utils/stop-watch.hpp"
15
16 namespace crocoddyl {
17
18 1 SolverIntro::SolverIntro(boost::shared_ptr<ShootingProblem> problem)
19 : SolverFDDP(problem),
20 1 eq_solver_(LuNull),
21 1 th_feas_(1e-4),
22 1 rho_(0.3),
23 1 upsilon_(0.),
24
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 zero_upsilon_(false) {
25 1 const std::size_t T = problem_->get_T();
26
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Hu_rank_.resize(T);
27
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 KQuu_tmp_.resize(T);
28
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 YZ_.resize(T);
29
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Hy_.resize(T);
30
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Qz_.resize(T);
31
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Qzz_.resize(T);
32
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Qxz_.resize(T);
33
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Quz_.resize(T);
34
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 kz_.resize(T);
35
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Kz_.resize(T);
36
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ks_.resize(T);
37
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Ks_.resize(T);
38
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 QuuinvHuT_.resize(T);
39
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Qzz_llt_.resize(T);
40
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Hu_lu_.resize(T);
41
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Hu_qr_.resize(T);
42
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Hy_lu_.resize(T);
43
44 1 const std::size_t ndx = problem_->get_ndx();
45 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
46 1 problem_->get_runningModels();
47
2/2
✓ Branch 0 taken 10 times.
✓ Branch 1 taken 1 times.
 11 for (std::size_t t = 0; t < T; ++t) {
48 10 const boost::shared_ptr<ActionModelAbstract>& model = models[t];
49 10 const std::size_t nu = model->get_nu();
50
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 10 const std::size_t nh = model->get_nh();
51 10 Hu_rank_[t] = nh;
52
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
 10 KQuu_tmp_[t] = Eigen::MatrixXd::Zero(ndx, nu);
53
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
 10 YZ_[t] = Eigen::MatrixXd::Zero(nu, nu);
54
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
 10 Hy_[t] = Eigen::MatrixXd::Zero(nh, nh);
55
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
 10 Qz_[t] = Eigen::VectorXd::Zero(nh);
56
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
 10 Qzz_[t] = Eigen::MatrixXd::Zero(nh, nh);
57
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
 10 Qxz_[t] = Eigen::MatrixXd::Zero(ndx, nh);
58
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
 10 Quz_[t] = Eigen::MatrixXd::Zero(nu, nh);
59
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
 10 kz_[t] = Eigen::VectorXd::Zero(nu);
60
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
 10 Kz_[t] = Eigen::MatrixXd::Zero(nu, ndx);
61
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
 10 ks_[t] = Eigen::VectorXd::Zero(nh);
62
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
 10 Ks_[t] = Eigen::MatrixXd::Zero(nh, ndx);
63
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 10 times.
✗ Branch 6 not taken.
 10 QuuinvHuT_[t] = Eigen::MatrixXd::Zero(nu, nh);
64
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 Qzz_llt_[t] = Eigen::LLT<Eigen::MatrixXd>(nh);
65
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 Hu_lu_[t] = Eigen::FullPivLU<Eigen::MatrixXd>(nh, nu);
66
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 Hu_qr_[t] = Eigen::ColPivHouseholderQR<Eigen::MatrixXd>(nu, nh);
67
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 Hy_lu_[t] = Eigen::PartialPivLU<Eigen::MatrixXd>(nh);
68 }
69 1 }
70
71 6 SolverIntro::~SolverIntro() {}
72
73 ✗ bool SolverIntro::solve(const std::vector<Eigen::VectorXd>& init_xs,
74 const std::vector<Eigen::VectorXd>& init_us,
75 const std::size_t maxiter, const bool is_feasible,
76 const double init_reg) {
77 ✗ START_PROFILER("SolverIntro::solve");
78 ✗ if (problem_->is_updated()) {
79 ✗ resizeData();
80 }
81 ✗ xs_try_[0] =
82 ✗ problem_->get_x0(); // it is needed in case that init_xs[0] is infeasible
83 ✗ setCandidate(init_xs, init_us, is_feasible);
84
85 ✗ if (std::isnan(init_reg)) {
86 ✗ preg_ = reg_min_;
87 ✗ dreg_ = reg_min_;
88 } else {
89 ✗ preg_ = init_reg;
90 ✗ dreg_ = init_reg;
91 }
92 ✗ was_feasible_ = false;
93 ✗ if (zero_upsilon_) {
94 ✗ upsilon_ = 0.;
95 }
96
97 ✗ bool recalcDiff = true;
98 ✗ for (iter_ = 0; iter_ < maxiter; ++iter_) {
99 while (true) {
100 try {
101 ✗ computeDirection(recalcDiff);
102 ✗ } catch (std::exception& e) {
103 ✗ recalcDiff = false;
104 ✗ increaseRegularization();
105 ✗ if (preg_ == reg_max_) {
106 ✗ return false;
107 } else {
108 ✗ continue;
109 }
110 }
111 ✗ break;
112 }
113 ✗ updateExpectedImprovement();
114 ✗ expectedImprovement();
115
116 // Update the penalty parameter for computing the merit function and its
117 // directional derivative For more details see Section 3 of "An Interior
118 // Point Algorithm for Large Scale Nonlinear Programming"
119 ✗ if (hfeas_ != 0 && iter_ != 0) {
120 ✗ upsilon_ =
121 ✗ std::max(upsilon_, (d_[0] + .5 * d_[1]) / ((1 - rho_) * hfeas_));
122 }
123
124 // We need to recalculate the derivatives when the step length passes
125 ✗ recalcDiff = false;
126 ✗ for (std::vector<double>::const_iterator it = alphas_.begin();
127 ✗ it != alphas_.end(); ++it) {
128 ✗ steplength_ = *it;
129 try {
130 ✗ dV_ = tryStep(steplength_);
131 ✗ dfeas_ = hfeas_ - hfeas_try_;
132 ✗ dPhi_ = dV_ + upsilon_ * dfeas_;
133 ✗ } catch (std::exception& e) {
134 ✗ continue;
135 }
136 ✗ expectedImprovement();
137 ✗ dVexp_ = steplength_ * (d_[0] + 0.5 * steplength_ * d_[1]);
138 ✗ dPhiexp_ = dVexp_ + steplength_ * upsilon_ * dfeas_;
139 ✗ if (dPhiexp_ >= 0) { // descend direction
140 ✗ if (std::abs(d_[0]) < th_grad_ || dPhi_ > th_acceptstep_ * dPhiexp_) {
141 ✗ was_feasible_ = is_feasible_;
142 ✗ setCandidate(xs_try_, us_try_, (was_feasible_) || (steplength_ == 1));
143 ✗ cost_ = cost_try_;
144 ✗ hfeas_ = hfeas_try_;
145 ✗ merit_ = cost_ + upsilon_ * hfeas_;
146 ✗ recalcDiff = true;
147 ✗ break;
148 }
149 } else { // reducing the gaps by allowing a small increment in the cost
150 // value
151 ✗ if (dV_ > th_acceptnegstep_ * dVexp_) {
152 ✗ was_feasible_ = is_feasible_;
153 ✗ setCandidate(xs_try_, us_try_, (was_feasible_) || (steplength_ == 1));
154 ✗ cost_ = cost_try_;
155 ✗ hfeas_ = hfeas_try_;
156 ✗ merit_ = cost_ + upsilon_ * hfeas_;
157 ✗ recalcDiff = true;
158 ✗ break;
159 }
160 }
161 }
162
163 ✗ stoppingCriteria();
164 ✗ const std::size_t n_callbacks = callbacks_.size();
165 ✗ for (std::size_t c = 0; c < n_callbacks; ++c) {
166 ✗ CallbackAbstract& callback = *callbacks_[c];
167 ✗ callback(*this);
168 }
169
170 ✗ if (steplength_ > th_stepdec_ && dV_ >= 0.) {
171 ✗ decreaseRegularization();
172 }
173 ✗ if (steplength_ <= th_stepinc_ || std::abs(d_[1]) <= th_feas_) {
174 ✗ if (preg_ == reg_max_) {
175 ✗ STOP_PROFILER("SolverIntro::solve");
176 ✗ return false;
177 }
178 ✗ increaseRegularization();
179 }
180
181 ✗ if (is_feasible_ && stop_ < th_stop_) {
182 ✗ STOP_PROFILER("SolverIntro::solve");
183 ✗ return true;
184 }
185 }
186 ✗ STOP_PROFILER("SolverIntro::solve");
187 ✗ return false;
188 }
189
190 ✗ double SolverIntro::tryStep(const double steplength) {
191 ✗ forwardPass(steplength);
192 ✗ hfeas_try_ = computeEqualityFeasibility();
193 ✗ return cost_ - cost_try_;
194 }
195
196 ✗ double SolverIntro::stoppingCriteria() {
197 ✗ stop_ = std::max(hfeas_, std::abs(d_[0] + 0.5 * d_[1]));
198 ✗ return stop_;
199 }
200
201 ✗ void SolverIntro::resizeData() {
202 ✗ START_PROFILER("SolverIntro::resizeData");
203 ✗ SolverFDDP::resizeData();
204
205 ✗ const std::size_t T = problem_->get_T();
206 ✗ const std::size_t ndx = problem_->get_ndx();
207 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
208 ✗ problem_->get_runningModels();
209 ✗ for (std::size_t t = 0; t < T; ++t) {
210 ✗ const boost::shared_ptr<ActionModelAbstract>& model = models[t];
211 ✗ const std::size_t nu = model->get_nu();
212 ✗ const std::size_t nh = model->get_nh();
213 ✗ KQuu_tmp_[t].conservativeResize(ndx, nu);
214 ✗ YZ_[t].conservativeResize(nu, nu);
215 ✗ Hy_[t].conservativeResize(nh, nh);
216 ✗ Qz_[t].conservativeResize(nh);
217 ✗ Qzz_[t].conservativeResize(nh, nh);
218 ✗ Qxz_[t].conservativeResize(ndx, nh);
219 ✗ Quz_[t].conservativeResize(nu, nh);
220 ✗ kz_[t].conservativeResize(nu);
221 ✗ Kz_[t].conservativeResize(nu, ndx);
222 ✗ ks_[t].conservativeResize(nh);
223 ✗ Ks_[t].conservativeResize(nh, ndx);
224 ✗ QuuinvHuT_[t].conservativeResize(nu, nh);
225 }
226 ✗ STOP_PROFILER("SolverIntro::resizeData");
227 }
228
229 ✗ double SolverIntro::calcDiff() {
230 ✗ START_PROFILER("SolverIntro::calcDiff");
231 ✗ SolverFDDP::calcDiff();
232 ✗ const std::size_t T = problem_->get_T();
233 const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
234 ✗ problem_->get_runningModels();
235 const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
236 ✗ problem_->get_runningDatas();
237 ✗ switch (eq_solver_) {
238 ✗ case LuNull:
239 #ifdef CROCODDYL_WITH_MULTITHREADING
240 #pragma omp parallel for num_threads(problem_->get_nthreads())
241 #endif
242 ✗ for (std::size_t t = 0; t < T; ++t) {
243 const boost::shared_ptr<crocoddyl::ActionModelAbstract>& model =
244 ✗ models[t];
245 ✗ const boost::shared_ptr<crocoddyl::ActionDataAbstract>& data = datas[t];
246 ✗ if (model->get_nu() > 0 && model->get_nh() > 0) {
247 ✗ Hu_lu_[t].compute(data->Hu);
248 ✗ YZ_[t] << Hu_lu_[t].matrixLU().transpose(), Hu_lu_[t].kernel();
249 ✗ Hu_rank_[t] = Hu_lu_[t].rank();
250 const Eigen::Block<Eigen::MatrixXd, Eigen::Dynamic, Eigen::Dynamic,
251 Eigen::RowMajor>
252 ✗ Y = YZ_[t].leftCols(Hu_lu_[t].rank());
253 ✗ Hy_[t].noalias() = data->Hu * Y;
254 ✗ Hy_lu_[t].compute(Hy_[t]);
255 const Eigen::Inverse<Eigen::PartialPivLU<Eigen::MatrixXd> > Hy_inv =
256 ✗ Hy_lu_[t].inverse();
257 ✗ ks_[t].noalias() = Hy_inv * data->h;
258 ✗ Ks_[t].noalias() = Hy_inv * data->Hx;
259 ✗ kz_[t].noalias() = Y * ks_[t];
260 ✗ Kz_[t].noalias() = Y * Ks_[t];
261 }
262 }
263 ✗ break;
264 ✗ case QrNull:
265 #ifdef CROCODDYL_WITH_MULTITHREADING
266 #pragma omp parallel for num_threads(problem_->get_nthreads())
267 #endif
268 ✗ for (std::size_t t = 0; t < T; ++t) {
269 const boost::shared_ptr<crocoddyl::ActionModelAbstract>& model =
270 ✗ models[t];
271 ✗ const boost::shared_ptr<crocoddyl::ActionDataAbstract>& data = datas[t];
272 ✗ if (model->get_nu() > 0 && model->get_nh() > 0) {
273 ✗ Hu_qr_[t].compute(data->Hu.transpose());
274 ✗ YZ_[t] = Hu_qr_[t].householderQ();
275 ✗ Hu_rank_[t] = Hu_qr_[t].rank();
276 const Eigen::Block<Eigen::MatrixXd, Eigen::Dynamic, Eigen::Dynamic,
277 Eigen::RowMajor>
278 ✗ Y = YZ_[t].leftCols(Hu_qr_[t].rank());
279 ✗ Hy_[t].noalias() = data->Hu * Y;
280 ✗ Hy_lu_[t].compute(Hy_[t]);
281 const Eigen::Inverse<Eigen::PartialPivLU<Eigen::MatrixXd> > Hy_inv =
282 ✗ Hy_lu_[t].inverse();
283 ✗ ks_[t].noalias() = Hy_inv * data->h;
284 ✗ Ks_[t].noalias() = Hy_inv * data->Hx;
285 ✗ kz_[t].noalias() = Y * ks_[t];
286 ✗ Kz_[t].noalias() = Y * Ks_[t];
287 }
288 }
289 ✗ break;
290 ✗ case Schur:
291 ✗ break;
292 }
293
294 ✗ STOP_PROFILER("SolverIntro::calcDiff");
295 ✗ return cost_;
296 }
297
298 ✗ void SolverIntro::computeValueFunction(
299 const std::size_t t, const boost::shared_ptr<ActionModelAbstract>& model) {
300 ✗ const std::size_t nu = model->get_nu();
301 ✗ Vx_[t] = Qx_[t];
302 ✗ Vxx_[t] = Qxx_[t];
303 ✗ if (nu != 0) {
304 ✗ START_PROFILER("SolverIntro::Vx");
305 ✗ Quuk_[t].noalias() = Quu_[t] * k_[t];
306 ✗ Vx_[t].noalias() -= Qxu_[t] * k_[t];
307 ✗ Qu_[t] -= Quuk_[t];
308 ✗ Vx_[t].noalias() -= K_[t].transpose() * Qu_[t];
309 ✗ Qu_[t] += Quuk_[t];
310 ✗ STOP_PROFILER("SolverIntro::Vx");
311 ✗ START_PROFILER("SolverIntro::Vxx");
312 ✗ KQuu_tmp_[t].noalias() = K_[t].transpose() * Quu_[t];
313 ✗ KQuu_tmp_[t].noalias() -= 2 * Qxu_[t];
314 ✗ Vxx_[t].noalias() += KQuu_tmp_[t] * K_[t];
315 ✗ STOP_PROFILER("SolverIntro::Vxx");
316 }
317 ✗ Vxx_tmp_ = 0.5 * (Vxx_[t] + Vxx_[t].transpose());
318 ✗ Vxx_[t] = Vxx_tmp_;
319
320 ✗ if (!std::isnan(preg_)) {
321 ✗ Vxx_[t].diagonal().array() += preg_;
322 }
323
324 // Compute and store the Vx gradient at end of the interval (rollout state)
325 ✗ if (!is_feasible_) {
326 ✗ Vx_[t].noalias() += Vxx_[t] * fs_[t];
327 }
328 }
329
330 ✗ void SolverIntro::computeGains(const std::size_t t) {
331 ✗ START_PROFILER("SolverIntro::computeGains");
332 const boost::shared_ptr<crocoddyl::ActionModelAbstract>& model =
333 ✗ problem_->get_runningModels()[t];
334 const boost::shared_ptr<crocoddyl::ActionDataAbstract>& data =
335 ✗ problem_->get_runningDatas()[t];
336
337 ✗ const std::size_t nu = model->get_nu();
338 ✗ const std::size_t nh = model->get_nh();
339 ✗ switch (eq_solver_) {
340 ✗ case LuNull:
341 case QrNull:
342 ✗ if (nu > 0 && nh > 0) {
343 ✗ START_PROFILER("SolverIntro::Qzz_inv");
344 ✗ const std::size_t rank = Hu_rank_[t];
345 ✗ const std::size_t nullity = data->Hu.cols() - rank;
346 const Eigen::Block<Eigen::MatrixXd, Eigen::Dynamic, Eigen::Dynamic,
347 Eigen::RowMajor>
348 ✗ Z = YZ_[t].rightCols(nullity);
349 ✗ Quz_[t].noalias() = Quu_[t] * Z;
350 ✗ Qzz_[t].noalias() = Z.transpose() * Quz_[t];
351 ✗ Qzz_llt_[t].compute(Qzz_[t]);
352 ✗ STOP_PROFILER("SolverIntro::Qzz_inv");
353 ✗ const Eigen::ComputationInfo& info = Qzz_llt_[t].info();
354 ✗ if (info != Eigen::Success) {
355 ✗ throw_pretty("backward error");
356 }
357
358 ✗ k_[t] = kz_[t];
359 ✗ K_[t] = Kz_[t];
360 ✗ Eigen::Transpose<Eigen::MatrixXd> QzzinvQzu = Quz_[t].transpose();
361 ✗ Qzz_llt_[t].solveInPlace(QzzinvQzu);
362 ✗ Qz_[t].noalias() = Z.transpose() * Qu_[t];
363 ✗ Qzz_llt_[t].solveInPlace(Qz_[t]);
364 ✗ Qxz_[t].noalias() = Qxu_[t] * Z;
365 ✗ Eigen::Transpose<Eigen::MatrixXd> Qzx = Qxz_[t].transpose();
366 ✗ Qzz_llt_[t].solveInPlace(Qzx);
367 ✗ Qz_[t].noalias() -= QzzinvQzu * kz_[t];
368 ✗ Qzx.noalias() -= QzzinvQzu * Kz_[t];
369 ✗ k_[t].noalias() += Z * Qz_[t];
370 ✗ K_[t].noalias() += Z * Qzx;
371 ✗ } else {
372 ✗ SolverFDDP::computeGains(t);
373 }
374 ✗ break;
375 ✗ case Schur:
376 ✗ SolverFDDP::computeGains(t);
377 ✗ if (nu > 0 && nh > 0) {
378 ✗ START_PROFILER("SolverIntro::Qzz_inv");
379 ✗ QuuinvHuT_[t] = data->Hu.transpose();
380 ✗ Quu_llt_[t].solveInPlace(QuuinvHuT_[t]);
381 ✗ Qzz_[t].noalias() = data->Hu * QuuinvHuT_[t];
382 ✗ Qzz_llt_[t].compute(Qzz_[t]);
383 ✗ STOP_PROFILER("SolverIntro::Qzz_inv");
384 ✗ const Eigen::ComputationInfo& info = Qzz_llt_[t].info();
385 ✗ if (info != Eigen::Success) {
386 ✗ throw_pretty("backward error");
387 }
388 ✗ Eigen::Transpose<Eigen::MatrixXd> HuQuuinv = QuuinvHuT_[t].transpose();
389 ✗ Qzz_llt_[t].solveInPlace(HuQuuinv);
390 ✗ ks_[t] = data->h;
391 ✗ ks_[t].noalias() -= data->Hu * k_[t];
392 ✗ Ks_[t] = data->Hx;
393 ✗ Ks_[t].noalias() -= data->Hu * K_[t];
394 ✗ k_[t].noalias() += QuuinvHuT_[t] * ks_[t];
395 ✗ K_[t] += QuuinvHuT_[t] * Ks_[t];
396 }
397 ✗ break;
398 }
399 ✗ STOP_PROFILER("SolverIntro::computeGains");
400 }
401
402 ✗ EqualitySolverType SolverIntro::get_equality_solver() const {
403 ✗ return eq_solver_;
404 }
405
406 ✗ double SolverIntro::get_th_feas() const { return th_feas_; }
407
408 ✗ double SolverIntro::get_rho() const { return rho_; }
409
410 ✗ double SolverIntro::get_upsilon() const { return upsilon_; }
411
412 ✗ bool SolverIntro::get_zero_upsilon() const { return zero_upsilon_; }
413
414 ✗ const std::vector<std::size_t>& SolverIntro::get_Hu_rank() const {
415 ✗ return Hu_rank_;
416 }
417
418 ✗ const std::vector<Eigen::MatrixXd>& SolverIntro::get_YZ() const { return YZ_; }
419
420 ✗ const std::vector<Eigen::MatrixXd>& SolverIntro::get_Qzz() const {
421 ✗ return Qzz_;
422 }
423
424 ✗ const std::vector<Eigen::MatrixXd>& SolverIntro::get_Qxz() const {
425 ✗ return Qxz_;
426 }
427
428 ✗ const std::vector<Eigen::MatrixXd>& SolverIntro::get_Quz() const {
429 ✗ return Quz_;
430 }
431
432 ✗ const std::vector<Eigen::VectorXd>& SolverIntro::get_Qz() const { return Qz_; }
433
434 ✗ const std::vector<Eigen::MatrixXd>& SolverIntro::get_Hy() const { return Hy_; }
435
436 ✗ const std::vector<Eigen::VectorXd>& SolverIntro::get_kz() const { return kz_; }
437
438 ✗ const std::vector<Eigen::MatrixXd>& SolverIntro::get_Kz() const { return Kz_; }
439
440 ✗ const std::vector<Eigen::VectorXd>& SolverIntro::get_ks() const { return ks_; }
441
442 ✗ const std::vector<Eigen::MatrixXd>& SolverIntro::get_Ks() const { return Ks_; }
443
444 ✗ void SolverIntro::set_equality_solver(const EqualitySolverType type) {
445 ✗ eq_solver_ = type;
446 }
447
448 ✗ void SolverIntro::set_th_feas(const double th_feas) { th_feas_ = th_feas; }
449
450 ✗ void SolverIntro::set_rho(const double rho) {
451 ✗ if (0. >= rho || rho > 1.) {
452 ✗ throw_pretty("Invalid argument: " << "rho value should between 0 and 1.");
453 }
454 ✗ rho_ = rho;
455 }
456
457 ✗ void SolverIntro::set_zero_upsilon(const bool zero_upsilon) {
458 ✗ zero_upsilon_ = zero_upsilon;
459 }
460
461 } // namespace crocoddyl
462