10#include "crocoddyl/core/solvers/box-qp.hpp"
11#include "crocoddyl/core/utils/exception.hpp"
15BoxQP::BoxQP(
const std::size_t nx,
const std::size_t maxiter,
const double th_acceptstep,
const double th_grad,
19 th_acceptstep_(th_acceptstep),
29 if (0. >= th_acceptstep && th_acceptstep >= 0.5) {
30 std::cerr <<
"Warning: th_acceptstep value should between 0 and 0.5" << std::endl;
33 std::cerr <<
"Warning: th_grad value has to be positive." << std::endl;
36 std::cerr <<
"Warning: reg value has to be positive." << std::endl;
46 solution_.
x = Eigen::VectorXd::Zero(nx);
49 const std::size_t n_alphas_ = 10;
50 alphas_.resize(n_alphas_);
51 for (std::size_t n = 0; n < n_alphas_; ++n) {
52 alphas_[n] = 1. / pow(2.,
static_cast<double>(n));
59 const Eigen::VectorXd& ub,
const Eigen::VectorXd& xinit) {
60 if (
static_cast<std::size_t
>(H.rows()) != nx_ ||
static_cast<std::size_t
>(H.cols()) != nx_) {
61 throw_pretty(
"Invalid argument: "
62 <<
"H has wrong dimension (it should be " + std::to_string(nx_) +
"," + std::to_string(nx_) +
")");
64 if (
static_cast<std::size_t
>(q.size()) != nx_) {
65 throw_pretty(
"Invalid argument: "
66 <<
"q has wrong dimension (it should be " + std::to_string(nx_) +
")");
68 if (
static_cast<std::size_t
>(lb.size()) != nx_) {
69 throw_pretty(
"Invalid argument: "
70 <<
"lb has wrong dimension (it should be " + std::to_string(nx_) +
")");
72 if (
static_cast<std::size_t
>(ub.size()) != nx_) {
73 throw_pretty(
"Invalid argument: "
74 <<
"ub has wrong dimension (it should be " + std::to_string(nx_) +
")");
76 if (
static_cast<std::size_t
>(xinit.size()) != nx_) {
77 throw_pretty(
"Invalid argument: "
78 <<
"xinit has wrong dimension (it should be " + std::to_string(nx_) +
")");
82 for (std::size_t i = 0; i < nx_; ++i) {
83 x_(i) = std::max(std::min(xinit(i), ub(i)), lb(i));
87 for (std::size_t k = 0; k < maxiter_; ++k) {
92 g_.noalias() += H * x_;
93 for (std::size_t j = 0; j < nx_; ++j) {
94 const double gj = g_(j);
95 const double xj = x_(j);
96 const double lbj = lb(j);
97 const double ubj = ub(j);
98 if ((xj == lbj && gj > 0.) || (xj == ubj && gj < 0.)) {
108 if (g_.lpNorm<Eigen::Infinity>() <= th_grad_ || nf_ == 0) {
110 Hff_.resize(nf_, nf_);
111 for (std::size_t i = 0; i < nf_; ++i) {
112 const std::size_t fi = solution_.
free_idx[i];
113 for (std::size_t j = 0; j < nf_; ++j) {
114 Hff_(i, j) = H(fi, solution_.
free_idx[j]);
118 Hff_.diagonal().array() += reg_;
120 Hff_inv_llt_.compute(Hff_);
121 const Eigen::ComputationInfo& info = Hff_inv_llt_.info();
122 if (info != Eigen::Success) {
123 throw_pretty(
"backward_error");
125 solution_.
Hff_inv.setIdentity(nf_, nf_);
126 Hff_inv_llt_.solveInPlace(solution_.
Hff_inv);
137 Hff_.resize(nf_, nf_);
138 Hfc_.resize(nf_, nc_);
139 for (std::size_t i = 0; i < nf_; ++i) {
140 const std::size_t fi = solution_.
free_idx[i];
143 for (std::size_t j = 0; j < nf_; ++j) {
144 Hff_(i, j) = H(fi, solution_.
free_idx[j]);
146 for (std::size_t j = 0; j < nc_; ++j) {
149 Hfc_(i, j) = H(fi, cj);
153 Hff_.diagonal().array() += reg_;
155 Hff_inv_llt_.compute(Hff_);
156 const Eigen::ComputationInfo& info = Hff_inv_llt_.info();
157 if (info != Eigen::Success) {
158 throw_pretty(
"backward_error");
160 solution_.
Hff_inv.setIdentity(nf_, nf_);
161 Hff_inv_llt_.solveInPlace(solution_.
Hff_inv);
164 dxf_.noalias() -= Hfc_ * xc_;
166 Hff_inv_llt_.solveInPlace(dxf_);
169 for (std::size_t i = 0; i < nf_; ++i) {
170 dx_(solution_.
free_idx[i]) = dxf_(i);
174 fold_ = 0.5 * x_.dot(H * x_) + q.dot(x_);
175 for (std::vector<double>::const_iterator it = alphas_.begin(); it != alphas_.end(); ++it) {
176 double steplength = *it;
177 for (std::size_t i = 0; i < nx_; ++i) {
178 xnew_(i) = std::max(std::min(x_(i) + steplength * dx_(i), ub(i)), lb(i));
180 fnew_ = 0.5 * xnew_.dot(H * xnew_) + q.dot(xnew_);
181 if (fold_ - fnew_ > th_acceptstep_ * g_.dot(x_ - xnew_)) {
207 x_ = Eigen::VectorXd::Zero(nx);
208 xnew_ = Eigen::VectorXd::Zero(nx);
209 g_ = Eigen::VectorXd::Zero(nx);
210 dx_ = Eigen::VectorXd::Zero(nx);
216 if (0. >= th_acceptstep && th_acceptstep >= 0.5) {
217 throw_pretty(
"Invalid argument: "
218 <<
"th_acceptstep value should between 0 and 0.5");
220 th_acceptstep_ = th_acceptstep;
225 throw_pretty(
"Invalid argument: "
226 <<
"th_grad value has to be positive.");
233 throw_pretty(
"Invalid argument: "
234 <<
"reg value has to be positive.");
240 double prev_alpha = alphas[0];
241 if (prev_alpha != 1.) {
242 std::cerr <<
"Warning: alpha[0] should be 1" << std::endl;
244 for (std::size_t i = 1; i < alphas.size(); ++i) {
245 double alpha = alphas[i];
247 throw_pretty(
"Invalid argument: "
248 <<
"alpha values has to be positive.");
250 if (alpha >= prev_alpha) {
251 throw_pretty(
"Invalid argument: "
252 <<
"alpha values are monotonously decreasing.");
void set_reg(const double reg)
Modify the regularization value.
void set_th_grad(const double th_grad)
Modify the gradient tolerance threshold.
const BoxQPSolution & get_solution() const
Return the stored solution.
double get_th_grad() const
Return the gradient tolerance threshold.
const BoxQPSolution & solve(const Eigen::MatrixXd &H, const Eigen::VectorXd &q, const Eigen::VectorXd &lb, const Eigen::VectorXd &ub, const Eigen::VectorXd &xinit)
Compute the solution of bound-constrained QP based on Newton projection.
std::size_t get_maxiter() const
Return the maximum allowed number of iterations.
void set_maxiter(const std::size_t maxiter)
Modify the maximum allowed number of iterations.
~BoxQP()
Destroy the Projected-Newton QP solver.
void set_nx(const std::size_t nx)
Modify the decision vector dimension.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW BoxQP(const std::size_t nx, const std::size_t maxiter=100, const double th_acceptstep=0.1, const double th_grad=1e-9, const double reg=1e-9)
Initialize the Projected-Newton QP for bound constraints.
const std::vector< double > & get_alphas() const
Return the stack of step lengths using by the line-search procedure.
void set_alphas(const std::vector< double > &alphas)
Modify the stack of step lengths using by the line-search procedure.
void set_th_acceptstep(const double th_acceptstep)
Modify the acceptance step threshold.
double get_th_acceptstep() const
Return the acceptance step threshold.
double get_reg() const
Return the regularization value.
std::size_t get_nx() const
Return the decision vector dimension.
std::vector< size_t > free_idx
Free space indexes.
Eigen::MatrixXd Hff_inv
Inverse of the free space Hessian.
Eigen::VectorXd x
Decision vector.
std::vector< size_t > clamped_idx
Clamped space indexes.