Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/core/solvers/box-qp.cpp	Lines:	98	148	66.2 %
Date:	2024-02-13 11:12:33	Branches:	128	400	32.0 %


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2022, University of Edinburgh, Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#include "crocoddyl/core/solvers/box-qp.hpp"

#include <iostream>

#include "crocoddyl/core/utils/exception.hpp"

namespace crocoddyl {

BoxQP::BoxQP(const std::size_t nx, const std::size_t maxiter,
             const double th_acceptstep, const double th_grad, const double reg)
    : nx_(nx),
      maxiter_(maxiter),
      th_acceptstep_(th_acceptstep),
      th_grad_(th_grad),
      reg_(reg),
      fold_(0.),
      fnew_(0.),
      x_(nx),
      xnew_(nx),
      g_(nx),
      dx_(nx),
      xo_(nx),
      dxo_(nx),
      qo_(nx),
      Ho_(nx, nx) {
  // Check if values have a proper range
  if (0. >= th_acceptstep && th_acceptstep >= 0.5) {
    std::cerr << "Warning: th_acceptstep value should between 0 and 0.5"
              << std::endl;
  }
  if (0. > th_grad) {
    std::cerr << "Warning: th_grad value has to be positive." << std::endl;
  }
  if (0. > reg) {
    std::cerr << "Warning: reg value has to be positive." << std::endl;
  }

  // Initialized the values of vectors
  x_.setZero();
  xnew_.setZero();
  g_.setZero();
  dx_.setZero();
  xo_.setZero();
  dxo_.setZero();
  qo_.setZero();
  Ho_.setZero();

  // Reserve the space and compute alphas
  solution_.x = Eigen::VectorXd::Zero(nx);
  solution_.clamped_idx.reserve(nx_);
  solution_.free_idx.reserve(nx_);
  const std::size_t n_alphas_ = 10;
  alphas_.resize(n_alphas_);
  for (std::size_t n = 0; n < n_alphas_; ++n) {
    alphas_[n] = 1. / pow(2., static_cast<double>(n));
  }
}

BoxQP::~BoxQP() {}

const BoxQPSolution& BoxQP::solve(const Eigen::MatrixXd& H,
                                  const Eigen::VectorXd& q,
                                  const Eigen::VectorXd& lb,
                                  const Eigen::VectorXd& ub,
                                  const Eigen::VectorXd& xinit) {
  if (static_cast<std::size_t>(H.rows()) != nx_ ||
      static_cast<std::size_t>(H.cols()) != nx_) {
    throw_pretty("Invalid argument: "
                 << "H has wrong dimension (it should be " +
                        std::to_string(nx_) + "," + std::to_string(nx_) + ")");
  }
  if (static_cast<std::size_t>(q.size()) != nx_) {
    throw_pretty("Invalid argument: "
                 << "q has wrong dimension (it should be " +
                        std::to_string(nx_) + ")");
  }
  if (static_cast<std::size_t>(lb.size()) != nx_) {
    throw_pretty("Invalid argument: "
                 << "lb has wrong dimension (it should be " +
                        std::to_string(nx_) + ")");
  }
  if (static_cast<std::size_t>(ub.size()) != nx_) {
    throw_pretty("Invalid argument: "
                 << "ub has wrong dimension (it should be " +
                        std::to_string(nx_) + ")");
  }
  if (static_cast<std::size_t>(xinit.size()) != nx_) {
    throw_pretty("Invalid argument: "
                 << "xinit has wrong dimension (it should be " +
                        std::to_string(nx_) + ")");
  }

  // We need to enforce feasible warm-starting of the algorithm
  for (std::size_t i = 0; i < nx_; ++i) {
    x_(i) = std::max(std::min(xinit(i), ub(i)), lb(i));
  }

  // Start the numerical iterations
  for (std::size_t k = 0; k < maxiter_; ++k) {
    solution_.clamped_idx.clear();
    solution_.free_idx.clear();
    // Compute the Cauchy point and active set
    g_ = q;
    g_.noalias() += H * x_;
    for (std::size_t j = 0; j < nx_; ++j) {
      const double gj = g_(j);
      const double xj = x_(j);
      const double lbj = lb(j);
      const double ubj = ub(j);
      if ((xj == lbj && gj > 0.) || (xj == ubj && gj < 0.)) {
        solution_.clamped_idx.push_back(j);
      } else {
        solution_.free_idx.push_back(j);
      }
    }

    // Compute the search direction as Newton step along the free space
    nf_ = solution_.free_idx.size();
    nc_ = solution_.clamped_idx.size();
    Eigen::VectorBlock<Eigen::VectorXd> xf = xo_.head(nf_);
    Eigen::VectorBlock<Eigen::VectorXd> xc = xo_.tail(nc_);
    Eigen::VectorBlock<Eigen::VectorXd> dxf = dxo_.head(nf_);
    Eigen::VectorBlock<Eigen::VectorXd> qf = qo_.head(nf_);
    Eigen::Block<Eigen::MatrixXd> Hff = Ho_.topLeftCorner(nf_, nf_);
    Eigen::Block<Eigen::MatrixXd> Hfc = Ho_.topRightCorner(nf_, nc_);
    for (std::size_t i = 0; i < nf_; ++i) {
      const std::size_t fi = solution_.free_idx[i];
      qf(i) = q(fi);
      xf(i) = x_(fi);
      for (std::size_t j = 0; j < nf_; ++j) {
        Hff(i, j) = H(fi, solution_.free_idx[j]);
      }
      for (std::size_t j = 0; j < nc_; ++j) {
        const std::size_t cj = solution_.clamped_idx[j];
        xc(j) = x_(cj);
        Hfc(i, j) = H(fi, cj);
      }
    }
    if (reg_ != 0.) {
      Hff.diagonal().array() += reg_;
    }
    Hff_inv_llt_.compute(Hff);
    const Eigen::ComputationInfo& info = Hff_inv_llt_.info();
    if (info != Eigen::Success) {
      throw_pretty("backward_error");
    }
    solution_.Hff_inv.setIdentity(nf_, nf_);
    Hff_inv_llt_.solveInPlace(solution_.Hff_inv);
    qf.noalias() += Hff * xf;
    if (nc_ != 0) {
      qf.noalias() += Hfc * xc;
    }
    dxf = -qf;
    Hff_inv_llt_.solveInPlace(dxf);
    dx_.setZero();
    for (std::size_t i = 0; i < nf_; ++i) {
      dx_(solution_.free_idx[i]) = dxf(i);
      g_(solution_.free_idx[i]) = -qf(i);
    }

    // Try different step lengths
    fold_ = 0.5 * x_.dot(H * x_) + q.dot(x_);
    for (std::vector<double>::const_iterator it = alphas_.begin();
         it != alphas_.end(); ++it) {
      double steplength = *it;
      for (std::size_t i = 0; i < nx_; ++i) {
        xnew_(i) =
            std::max(std::min(x_(i) + steplength * dx_(i), ub(i)), lb(i));
      }
      fnew_ = 0.5 * xnew_.dot(H * xnew_) + q.dot(xnew_);
      if (fold_ - fnew_ > th_acceptstep_ * g_.dot(x_ - xnew_)) {
        x_ = xnew_;
        break;
      }
    }

    // Check convergence
    if (qf.lpNorm<Eigen::Infinity>() <= th_grad_) {
      solution_.x = x_;
      return solution_;
    }
  }
  solution_.x = x_;
  return solution_;
}

const BoxQPSolution& BoxQP::get_solution() const { return solution_; }

std::size_t BoxQP::get_nx() const { return nx_; }

std::size_t BoxQP::get_maxiter() const { return maxiter_; }

double BoxQP::get_th_acceptstep() const { return th_acceptstep_; }

double BoxQP::get_th_grad() const { return th_grad_; }

double BoxQP::get_reg() const { return reg_; }

const std::vector<double>& BoxQP::get_alphas() const { return alphas_; }

void BoxQP::set_nx(const std::size_t nx) {
  nx_ = nx;
  x_.conservativeResize(nx);
  xnew_.conservativeResize(nx);
  g_.conservativeResize(nx);
  dx_.conservativeResize(nx);
  xo_.conservativeResize(nx);
  dxo_.conservativeResize(nx);
  qo_.conservativeResize(nx);
  Ho_.conservativeResize(nx, nx);
}

void BoxQP::set_maxiter(const std::size_t maxiter) { maxiter_ = maxiter; }

void BoxQP::set_th_acceptstep(const double th_acceptstep) {
  if (0. >= th_acceptstep && th_acceptstep >= 0.5) {
    throw_pretty("Invalid argument: "
                 << "th_acceptstep value should between 0 and 0.5");
  }
  th_acceptstep_ = th_acceptstep;
}

void BoxQP::set_th_grad(const double th_grad) {
  if (0. > th_grad) {
    throw_pretty("Invalid argument: "
                 << "th_grad value has to be positive.");
  }
  th_grad_ = th_grad;
}

void BoxQP::set_reg(const double reg) {
  if (0. > reg) {
    throw_pretty("Invalid argument: "
                 << "reg value has to be positive.");
  }
  reg_ = reg;
}

void BoxQP::set_alphas(const std::vector<double>& alphas) {
  double prev_alpha = alphas[0];
  if (prev_alpha != 1.) {
    std::cerr << "Warning: alpha[0] should be 1" << std::endl;
  }
  for (std::size_t i = 1; i < alphas.size(); ++i) {
    double alpha = alphas[i];
    if (0. >= alpha) {
      throw_pretty("Invalid argument: "
                   << "alpha values has to be positive.");
    }
    if (alpha >= prev_alpha) {
      throw_pretty("Invalid argument: "
                   << "alpha values are monotonously decreasing.");
    }
    prev_alpha = alpha;
  }
  alphas_ = alphas;
}

}  // namespace crocoddyl


Generated by: GCOVR (Version 4.2)

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		12	BoxQP::BoxQP(const std::size_t nx, const std::size_t maxiter,
18		12	const double th_acceptstep, const double th_grad, const double reg)
19			: nx_(nx),
20			maxiter_(maxiter),
21			th_acceptstep_(th_acceptstep),
22			th_grad_(th_grad),
23			reg_(reg),
24			fold_(0.),
25			fnew_(0.),
26			x_(nx),
27			xnew_(nx),
28			g_(nx),
29			dx_(nx),
30			xo_(nx),
31			dxo_(nx),
32			qo_(nx),
33	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	12	Ho_(nx, nx) {
34			// Check if values have a proper range
35	✗✓✗✗	12	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	✗✓	12	if (0. > th_grad) {
40			std::cerr << "Warning: th_grad value has to be positive." << std::endl;
41			}
42	✗✓	12	if (0. > reg) {
43			std::cerr << "Warning: reg value has to be positive." << std::endl;
44			}
45
46			// Initialized the values of vectors
47	✓✗	12	x_.setZero();
48	✓✗	12	xnew_.setZero();
49	✓✗	12	g_.setZero();
50	✓✗	12	dx_.setZero();
51	✓✗	12	xo_.setZero();
52	✓✗	12	dxo_.setZero();
53	✓✗	12	qo_.setZero();
54	✓✗	12	Ho_.setZero();
55
56			// Reserve the space and compute alphas
57	✓✗✓✗	12	solution_.x = Eigen::VectorXd::Zero(nx);
58	✓✗	12	solution_.clamped_idx.reserve(nx_);
59	✓✗	12	solution_.free_idx.reserve(nx_);
60		12	const std::size_t n_alphas_ = 10;
61	✓✗	12	alphas_.resize(n_alphas_);
62	✓✓	132	for (std::size_t n = 0; n < n_alphas_; ++n) {
63		120	alphas_[n] = 1. / pow(2., static_cast<double>(n));
64			}
65		12	}
66
67		16	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	✓✗✗✓	12	if (static_cast<std::size_t>(H.rows()) != nx_ \|\|
75	✗✓	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	✗✓	6	if (static_cast<std::size_t>(q.size()) != nx_) {
81			throw_pretty("Invalid argument: "
82			<< "q has wrong dimension (it should be " +
83			std::to_string(nx_) + ")");
84			}
85	✗✓	6	if (static_cast<std::size_t>(lb.size()) != nx_) {
86			throw_pretty("Invalid argument: "
87			<< "lb has wrong dimension (it should be " +
88			std::to_string(nx_) + ")");
89			}
90	✗✓	6	if (static_cast<std::size_t>(ub.size()) != nx_) {
91			throw_pretty("Invalid argument: "
92			<< "ub has wrong dimension (it should be " +
93			std::to_string(nx_) + ")");
94			}
95	✗✓	6	if (static_cast<std::size_t>(xinit.size()) != nx_) {
96			throw_pretty("Invalid argument: "
97			<< "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	✓✓	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	✓✗	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	✓✗	12	g_ = q;
112	✓✗✓✗ ✓✗	12	g_.noalias() += H * x_;
113	✓✓	36	for (std::size_t j = 0; j < nx_; ++j) {
114	✓✗	24	const double gj = g_(j);
115	✓✗	24	const double xj = x_(j);
116	✓✗	24	const double lbj = lb(j);
117	✓✗	24	const double ubj = ub(j);
118	✓✓✓✓ ✗✓✗✗	24	if ((xj == lbj && gj > 0.) \|\| (xj == ubj && gj < 0.)) {
119	✓✗	2	solution_.clamped_idx.push_back(j);
120			} else {
121	✓✗	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	✓✗	12	Eigen::VectorBlock<Eigen::VectorXd> xf = xo_.head(nf_);
129	✓✗	12	Eigen::VectorBlock<Eigen::VectorXd> xc = xo_.tail(nc_);
130	✓✗	12	Eigen::VectorBlock<Eigen::VectorXd> dxf = dxo_.head(nf_);
131	✓✗	12	Eigen::VectorBlock<Eigen::VectorXd> qf = qo_.head(nf_);
132	✓✗	12	Eigen::Block<Eigen::MatrixXd> Hff = Ho_.topLeftCorner(nf_, nf_);
133	✓✗	12	Eigen::Block<Eigen::MatrixXd> Hfc = Ho_.topRightCorner(nf_, nc_);
134	✓✓	34	for (std::size_t i = 0; i < nf_; ++i) {
135		22	const std::size_t fi = solution_.free_idx[i];
136	✓✗✓✗	22	qf(i) = q(fi);
137	✓✗✓✗	22	xf(i) = x_(fi);
138	✓✓	64	for (std::size_t j = 0; j < nf_; ++j) {
139	✓✗✓✗	42	Hff(i, j) = H(fi, solution_.free_idx[j]);
140			}
141	✓✓	24	for (std::size_t j = 0; j < nc_; ++j) {
142		2	const std::size_t cj = solution_.clamped_idx[j];
143	✓✗✓✗	2	xc(j) = x_(cj);
144	✓✗✓✗	2	Hfc(i, j) = H(fi, cj);
145			}
146			}
147	✓✓	12	if (reg_ != 0.) {
148	✓✗✓✗ ✓✗	6	Hff.diagonal().array() += reg_;
149			}
150	✓✗	12	Hff_inv_llt_.compute(Hff);
151		12	const Eigen::ComputationInfo& info = Hff_inv_llt_.info();
152	✗✓	12	if (info != Eigen::Success) {
153			throw_pretty("backward_error");
154			}
155	✓✗	12	solution_.Hff_inv.setIdentity(nf_, nf_);
156	✓✗	12	Hff_inv_llt_.solveInPlace(solution_.Hff_inv);
157	✓✗✓✗ ✓✗	12	qf.noalias() += Hff * xf;
158	✓✓	12	if (nc_ != 0) {
159	✓✗✓✗ ✓✗	2	qf.noalias() += Hfc * xc;
160			}
161	✓✗✓✗	12	dxf = -qf;
162	✓✗	12	Hff_inv_llt_.solveInPlace(dxf);
163	✓✗	12	dx_.setZero();
164	✓✓	34	for (std::size_t i = 0; i < nf_; ++i) {
165	✓✗✓✗	22	dx_(solution_.free_idx[i]) = dxf(i);
166	✓✗✓✗	22	g_(solution_.free_idx[i]) = -qf(i);
167			}
168
169			// Try different step lengths
170	✓✗✓✗ ✓✗	12	fold_ = 0.5 * x_.dot(H * x_) + q.dot(x_);
171		32	for (std::vector<double>::const_iterator it = alphas_.begin();
172	✓✓	52	it != alphas_.end(); ++it) {
173		30	double steplength = *it;
174	✓✓	90	for (std::size_t i = 0; i < nx_; ++i) {
175	✓✗	60	xnew_(i) =
176	✓✗✓✗ ✓✗✓✗	120	std::max(std::min(x_(i) + steplength * dx_(i), ub(i)), lb(i));
177			}
178	✓✗✓✗ ✓✗	30	fnew_ = 0.5 * xnew_.dot(H * xnew_) + q.dot(xnew_);
179	✓✗✓✗ ✓✓	30	if (fold_ - fnew_ > th_acceptstep_ * g_.dot(x_ - xnew_)) {
180	✓✗	10	x_ = xnew_;
181		10	break;
182			}
183			}
184
185			// Check convergence
186	✓✗✓✓	12	if (qf.lpNorm<Eigen::Infinity>() <= th_grad_) {
187	✓✗	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("Invalid argument: "
226			<< "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: "
234			<< "th_grad value has to be positive.");
235			}
236			th_grad_ = th_grad;
237			}
238
239		6	void BoxQP::set_reg(const double reg) {
240	✗✓	6	if (0. > reg) {
241			throw_pretty("Invalid argument: "
242			<< "reg value has to be positive.");
243			}
244		6	reg_ = reg;
245		6	}
246
247			void BoxQP::set_alphas(const std::vector<double>& alphas) {
248			double prev_alpha = alphas[0];
249			if (prev_alpha != 1.) {
250			std::cerr << "Warning: alpha[0] should be 1" << std::endl;
251			}
252			for (std::size_t i = 1; i < alphas.size(); ++i) {
253			double alpha = alphas[i];
254			if (0. >= alpha) {
255			throw_pretty("Invalid argument: "
256			<< "alpha values has to be positive.");
257			}
258			if (alpha >= prev_alpha) {
259			throw_pretty("Invalid argument: "
260			<< "alpha values are monotonously decreasing.");
261			}
262			prev_alpha = alpha;
263			}
264			alphas_ = alphas;
265			}
266
267			} // namespace crocoddyl