GCC Code Coverage Report

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