GCC Code Coverage Report

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