GCC Code Coverage Report

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