GCC Code Coverage Report

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