GCC Code Coverage Report

Directory: ./
File: src/core/solvers/box-qp.cpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 112 162 69.1%
Branches: 128 400 32.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2022, University of Edinburgh, Heriot-Watt University
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
7 ///////////////////////////////////////////////////////////////////////////////
8
9 #include "crocoddyl/core/solvers/box-qp.hpp"
10
11 #include <iostream>
12
13 namespace crocoddyl {
14
15 16 BoxQP::BoxQP(const std::size_t nx, const std::size_t maxiter,
16 16 const double th_acceptstep, const double th_grad, const double reg)
17 16 : nx_(nx),
18 16 maxiter_(maxiter),
19 16 th_acceptstep_(th_acceptstep),
20 16 th_grad_(th_grad),
21 16 reg_(reg),
22 16 fold_(0.),
23 16 fnew_(0.),
24
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 x_(nx),
25
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 xnew_(nx),
26
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 g_(nx),
27
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 dx_(nx),
28
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 xo_(nx),
29
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 dxo_(nx),
30
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 qo_(nx),
31
2/4
✓ Branch 3 taken 16 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 16 times.
✗ Branch 7 not taken.
 32 Ho_(nx, nx) {
32 // Check if values have a proper range
33
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 16 if (0. >= th_acceptstep && th_acceptstep >= 0.5) {
34 ✗ std::cerr << "Warning: th_acceptstep value should between 0 and 0.5"
35 ✗ << std::endl;
36 }
37
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 16 times.
 16 if (0. > th_grad) {
38 ✗ std::cerr << "Warning: th_grad value has to be positive." << std::endl;
39 }
40
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 16 times.
 16 if (0. > reg) {
41 ✗ std::cerr << "Warning: reg value has to be positive." << std::endl;
42 }
43
44 // Initialized the values of vectors
45
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 x_.setZero();
46
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 xnew_.setZero();
47
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 g_.setZero();
48
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 dx_.setZero();
49
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 xo_.setZero();
50
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 dxo_.setZero();
51
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 qo_.setZero();
52
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 Ho_.setZero();
53
54 // Reserve the space and compute alphas
55
2/4
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16 times.
✗ Branch 5 not taken.
 16 solution_.x = Eigen::VectorXd::Zero(nx);
56
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 solution_.clamped_idx.reserve(nx_);
57
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 solution_.free_idx.reserve(nx_);
58 16 const std::size_t n_alphas_ = 10;
59
1/2
✓ Branch 1 taken 16 times.
✗ Branch 2 not taken.
 16 alphas_.resize(n_alphas_);
60
2/2
✓ Branch 0 taken 160 times.
✓ Branch 1 taken 16 times.
 176 for (std::size_t n = 0; n < n_alphas_; ++n) {
61 160 alphas_[n] = 1. / pow(2., static_cast<double>(n));
62 }
63 16 }
64
65 20 BoxQP::~BoxQP() {}
66
67 6 const BoxQPSolution& BoxQP::solve(const Eigen::MatrixXd& H,
68 const Eigen::VectorXd& q,
69 const Eigen::VectorXd& lb,
70 const Eigen::VectorXd& ub,
71 const Eigen::VectorXd& xinit) {
72
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 6 times.
 12 if (static_cast<std::size_t>(H.rows()) != nx_ ||
73
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 6 times.
 6 static_cast<std::size_t>(H.cols()) != nx_) {
74 ✗ throw_pretty("Invalid argument: "
75 << "H has wrong dimension (it should be " +
76 std::to_string(nx_) + "," + std::to_string(nx_) + ")");
77 }
78
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 6 times.
 6 if (static_cast<std::size_t>(q.size()) != nx_) {
79 ✗ throw_pretty(
80 "Invalid argument: " << "q has wrong dimension (it should be " +
81 std::to_string(nx_) + ")");
82 }
83
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 6 times.
 6 if (static_cast<std::size_t>(lb.size()) != nx_) {
84 ✗ throw_pretty(
85 "Invalid argument: " << "lb has wrong dimension (it should be " +
86 std::to_string(nx_) + ")");
87 }
88
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 6 times.
 6 if (static_cast<std::size_t>(ub.size()) != nx_) {
89 ✗ throw_pretty(
90 "Invalid argument: " << "ub has wrong dimension (it should be " +
91 std::to_string(nx_) + ")");
92 }
93
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 6 times.
 6 if (static_cast<std::size_t>(xinit.size()) != nx_) {
94 ✗ throw_pretty(
95 "Invalid argument: " << "xinit has wrong dimension (it should be " +
96 std::to_string(nx_) + ")");
97 }
98
99 // We need to enforce feasible warm-starting of the algorithm
100
2/2
✓ Branch 0 taken 12 times.
✓ Branch 1 taken 6 times.
 18 for (std::size_t i = 0; i < nx_; ++i) {
101 12 x_(i) = std::max(std::min(xinit(i), ub(i)), lb(i));
102 }
103
104 // Start the numerical iterations
105
1/2
✓ Branch 0 taken 12 times.
✗ Branch 1 not taken.
 12 for (std::size_t k = 0; k < maxiter_; ++k) {
106 12 solution_.clamped_idx.clear();
107 12 solution_.free_idx.clear();
108 // Compute the Cauchy point and active set
109
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 g_ = q;
110
3/6
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
 12 g_.noalias() += H * x_;
111
2/2
✓ Branch 0 taken 24 times.
✓ Branch 1 taken 12 times.
 36 for (std::size_t j = 0; j < nx_; ++j) {
112
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 24 const double gj = g_(j);
113
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 24 const double xj = x_(j);
114
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 24 const double lbj = lb(j);
115
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 24 const double ubj = ub(j);
116
5/8
✓ Branch 0 taken 4 times.
✓ Branch 1 taken 20 times.
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 22 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 24 if ((xj == lbj && gj > 0.) || (xj == ubj && gj < 0.)) {
117
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 solution_.clamped_idx.push_back(j);
118 } else {
119
1/2
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
 22 solution_.free_idx.push_back(j);
120 }
121 }
122
123 // Compute the search direction as Newton step along the free space
124 12 nf_ = solution_.free_idx.size();
125 12 nc_ = solution_.clamped_idx.size();
126
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 Eigen::VectorBlock<Eigen::VectorXd> xf = xo_.head(nf_);
127
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 Eigen::VectorBlock<Eigen::VectorXd> xc = xo_.tail(nc_);
128
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 Eigen::VectorBlock<Eigen::VectorXd> dxf = dxo_.head(nf_);
129
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 Eigen::VectorBlock<Eigen::VectorXd> qf = qo_.head(nf_);
130
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 Eigen::Block<Eigen::MatrixXd> Hff = Ho_.topLeftCorner(nf_, nf_);
131
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 Eigen::Block<Eigen::MatrixXd> Hfc = Ho_.topRightCorner(nf_, nc_);
132
2/2
✓ Branch 0 taken 22 times.
✓ Branch 1 taken 12 times.
 34 for (std::size_t i = 0; i < nf_; ++i) {
133 22 const std::size_t fi = solution_.free_idx[i];
134
2/4
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
 22 qf(i) = q(fi);
135
2/4
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 22 times.
✗ Branch 5 not taken.
 22 xf(i) = x_(fi);
136
2/2
✓ Branch 0 taken 42 times.
✓ Branch 1 taken 22 times.
 64 for (std::size_t j = 0; j < nf_; ++j) {
137
2/4
✓ Branch 2 taken 42 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 42 times.
✗ Branch 6 not taken.
 42 Hff(i, j) = H(fi, solution_.free_idx[j]);
138 }
139
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 22 times.
 24 for (std::size_t j = 0; j < nc_; ++j) {
140 2 const std::size_t cj = solution_.clamped_idx[j];
141
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 xc(j) = x_(cj);
142
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 Hfc(i, j) = H(fi, cj);
143 }
144 }
145
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 6 times.
 12 if (reg_ != 0.) {
146
3/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
 6 Hff.diagonal().array() += reg_;
147 }
148
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 Hff_inv_llt_.compute(Hff);
149 12 const Eigen::ComputationInfo& info = Hff_inv_llt_.info();
150
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 12 times.
 12 if (info != Eigen::Success) {
151 ✗ throw_pretty("backward_error");
152 }
153
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 solution_.Hff_inv.setIdentity(nf_, nf_);
154
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 Hff_inv_llt_.solveInPlace(solution_.Hff_inv);
155
3/6
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
 12 qf.noalias() += Hff * xf;
156
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 10 times.
 12 if (nc_ != 0) {
157
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 2 qf.noalias() += Hfc * xc;
158 }
159
2/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 12 dxf = -qf;
160
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 Hff_inv_llt_.solveInPlace(dxf);
161
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 dx_.setZero();
162
2/2
✓ Branch 0 taken 22 times.
✓ Branch 1 taken 12 times.
 34 for (std::size_t i = 0; i < nf_; ++i) {
163
2/4
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 22 times.
✗ Branch 6 not taken.
 22 dx_(solution_.free_idx[i]) = dxf(i);
164
2/4
✓ Branch 1 taken 22 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 22 times.
✗ Branch 6 not taken.
 22 g_(solution_.free_idx[i]) = -qf(i);
165 }
166
167 // Try different step lengths
168
3/6
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12 times.
✗ Branch 8 not taken.
 12 fold_ = 0.5 * x_.dot(H * x_) + q.dot(x_);
169 12 for (std::vector<double>::const_iterator it = alphas_.begin();
170
2/2
✓ Branch 3 taken 30 times.
✓ Branch 4 taken 2 times.
 32 it != alphas_.end(); ++it) {
171 30 double steplength = *it;
172
2/2
✓ Branch 0 taken 60 times.
✓ Branch 1 taken 30 times.
 90 for (std::size_t i = 0; i < nx_; ++i) {
173 120 xnew_(i) =
174
5/10
✓ Branch 1 taken 60 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 60 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 60 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 60 times.
✗ Branch 11 not taken.
✓ Branch 15 taken 60 times.
✗ Branch 16 not taken.
 60 std::max(std::min(x_(i) + steplength * dx_(i), ub(i)), lb(i));
175 }
176
3/6
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 30 times.
✗ Branch 8 not taken.
 30 fnew_ = 0.5 * xnew_.dot(H * xnew_) + q.dot(xnew_);
177
4/6
✓ Branch 1 taken 30 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 10 times.
✓ Branch 7 taken 20 times.
 30 if (fold_ - fnew_ > th_acceptstep_ * g_.dot(x_ - xnew_)) {
178
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 x_ = xnew_;
179 10 break;
180 }
181 }
182
183 // Check convergence
184
3/4
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 6 times.
✓ Branch 4 taken 6 times.
 12 if (qf.lpNorm<Eigen::Infinity>() <= th_grad_) {
185
1/2
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
 6 solution_.x = x_;
186 6 return solution_;
187 }
188 }
189 ✗ solution_.x = x_;
190 ✗ return solution_;
191 }
192
193 ✗ const BoxQPSolution& BoxQP::get_solution() const { return solution_; }
194
195 1 std::size_t BoxQP::get_nx() const { return nx_; }
196
197 ✗ std::size_t BoxQP::get_maxiter() const { return maxiter_; }
198
199 ✗ double BoxQP::get_th_acceptstep() const { return th_acceptstep_; }
200
201 ✗ double BoxQP::get_th_grad() const { return th_grad_; }
202
203 ✗ double BoxQP::get_reg() const { return reg_; }
204
205 ✗ const std::vector<double>& BoxQP::get_alphas() const { return alphas_; }
206
207 ✗ void BoxQP::set_nx(const std::size_t nx) {
208 ✗ nx_ = nx;
209 ✗ x_.conservativeResize(nx);
210 ✗ xnew_.conservativeResize(nx);
211 ✗ g_.conservativeResize(nx);
212 ✗ dx_.conservativeResize(nx);
213 ✗ xo_.conservativeResize(nx);
214 ✗ dxo_.conservativeResize(nx);
215 ✗ qo_.conservativeResize(nx);
216 ✗ Ho_.conservativeResize(nx, nx);
217 }
218
219 ✗ void BoxQP::set_maxiter(const std::size_t maxiter) { maxiter_ = maxiter; }
220
221 ✗ void BoxQP::set_th_acceptstep(const double th_acceptstep) {
222 ✗ if (0. >= th_acceptstep && th_acceptstep >= 0.5) {
223 ✗ throw_pretty(
224 "Invalid argument: " << "th_acceptstep value should between 0 and 0.5");
225 }
226 ✗ th_acceptstep_ = th_acceptstep;
227 }
228
229 ✗ void BoxQP::set_th_grad(const double th_grad) {
230 ✗ if (0. > th_grad) {
231 ✗ throw_pretty("Invalid argument: " << "th_grad value has to be positive.");
232 }
233 ✗ th_grad_ = th_grad;
234 }
235
236 6 void BoxQP::set_reg(const double reg) {
237
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 6 times.
 6 if (0. > reg) {
238 ✗ throw_pretty("Invalid argument: " << "reg value has to be positive.");
239 }
240 6 reg_ = reg;
241 6 }
242
243 ✗ void BoxQP::set_alphas(const std::vector<double>& alphas) {
244 ✗ double prev_alpha = alphas[0];
245 ✗ if (prev_alpha != 1.) {
246 ✗ std::cerr << "Warning: alpha[0] should be 1" << std::endl;
247 }
248 ✗ for (std::size_t i = 1; i < alphas.size(); ++i) {
249 ✗ double alpha = alphas[i];
250 ✗ if (0. >= alpha) {
251 ✗ throw_pretty("Invalid argument: " << "alpha values has to be positive.");
252 }
253 ✗ if (alpha >= prev_alpha) {
254 ✗ throw_pretty(
255 "Invalid argument: " << "alpha values are monotonously decreasing.");
256 }
257 ✗ prev_alpha = alpha;
258 }
259 ✗ alphas_ = alphas;
260 }
261
262 } // namespace crocoddyl
263