10 #include "crocoddyl/core/solvers/box-qp.hpp" 11 #include "crocoddyl/core/utils/exception.hpp" 15 BoxQP::BoxQP(
const std::size_t nx, 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;
42 const std::size_t& n_alphas_ = 10;
43 alphas_.resize(n_alphas_);
44 for (std::size_t n = 0; n < n_alphas_; ++n) {
45 alphas_[n] = 1. / pow(2., static_cast<double>(n));
52 const Eigen::VectorXd& ub,
const Eigen::VectorXd& xinit) {
53 if (static_cast<std::size_t>(H.rows()) != nx_ || static_cast<std::size_t>(H.cols()) != nx_) {
54 throw_pretty(
"Invalid argument: " 55 <<
"H has wrong dimension (it should be " + std::to_string(nx_) +
"," + std::to_string(nx_) +
")");
57 if (static_cast<std::size_t>(q.size()) != nx_) {
58 throw_pretty(
"Invalid argument: " 59 <<
"q has wrong dimension (it should be " + std::to_string(nx_) +
")");
61 if (static_cast<std::size_t>(lb.size()) != nx_) {
62 throw_pretty(
"Invalid argument: " 63 <<
"lb has wrong dimension (it should be " + std::to_string(nx_) +
")");
65 if (static_cast<std::size_t>(ub.size()) != nx_) {
66 throw_pretty(
"Invalid argument: " 67 <<
"ub has wrong dimension (it should be " + std::to_string(nx_) +
")");
69 if (static_cast<std::size_t>(xinit.size()) != nx_) {
70 throw_pretty(
"Invalid argument: " 71 <<
"xinit has wrong dimension (it should be " + std::to_string(nx_) +
")");
75 for (std::size_t i = 0; i < nx_; ++i) {
76 x_(i) = std::max(std::min(xinit(i), ub(i)), lb(i));
80 for (std::size_t k = 0; k < maxiter_; ++k) {
85 g_.noalias() += H * x_;
86 for (std::size_t j = 0; j < nx_; ++j) {
87 const double& gj = g_(j);
88 const double& xj = x_(j);
89 const double& lbj = lb(j);
90 const double& ubj = ub(j);
91 if ((xj == lbj && gj > 0.) || (xj == ubj && gj < 0.)) {
101 if (g_.lpNorm<Eigen::Infinity>() <= th_grad_ || nf_ == 0) {
103 Hff_.resize(nf_, nf_);
104 for (std::size_t i = 0; i < nf_; ++i) {
105 const std::size_t& fi = solution_.
free_idx[i];
106 for (std::size_t j = 0; j < nf_; ++j) {
107 Hff_(i, j) = H(fi, solution_.
free_idx[j]);
111 Hff_.diagonal().array() += reg_;
113 Hff_inv_llt_.compute(Hff_);
114 const Eigen::ComputationInfo& info = Hff_inv_llt_.info();
115 if (info != Eigen::Success) {
116 throw_pretty(
"backward_error");
118 solution_.
Hff_inv.setIdentity(nf_, nf_);
119 Hff_inv_llt_.solveInPlace(solution_.
Hff_inv);
130 Hff_.resize(nf_, nf_);
131 Hfc_.resize(nf_, nc_);
132 for (std::size_t i = 0; i < nf_; ++i) {
133 const std::size_t& fi = solution_.
free_idx[i];
136 for (std::size_t j = 0; j < nf_; ++j) {
137 Hff_(i, j) = H(fi, solution_.
free_idx[j]);
139 for (std::size_t j = 0; j < nc_; ++j) {
142 Hfc_(i, j) = H(fi, cj);
146 Hff_.diagonal().array() += reg_;
148 Hff_inv_llt_.compute(Hff_);
149 const Eigen::ComputationInfo& info = Hff_inv_llt_.info();
150 if (info != Eigen::Success) {
151 throw_pretty(
"backward_error");
153 solution_.
Hff_inv.setIdentity(nf_, nf_);
154 Hff_inv_llt_.solveInPlace(solution_.
Hff_inv);
157 dxf_.noalias() -= Hfc_ * xc_;
159 Hff_inv_llt_.solveInPlace(dxf_);
162 for (std::size_t i = 0; i < nf_; ++i) {
163 dx_(solution_.
free_idx[i]) = dxf_(i);
167 fold_ = 0.5 * x_.dot(H * x_) + q.dot(x_);
168 for (std::vector<double>::const_iterator it = alphas_.begin(); it != alphas_.end(); ++it) {
169 double steplength = *it;
170 for (std::size_t i = 0; i < nx_; ++i) {
171 xnew_(i) = std::max(std::min(x_(i) + steplength * dx_(i), ub(i)), lb(i));
173 fnew_ = 0.5 * xnew_.dot(H * xnew_) + q.dot(xnew_);
174 if (fold_ - fnew_ > th_acceptstep_ * g_.dot(x_ - xnew_)) {
200 x_ = Eigen::VectorXd::Zero(nx);
201 xnew_ = Eigen::VectorXd::Zero(nx);
202 g_ = Eigen::VectorXd::Zero(nx);
203 dx_ = Eigen::VectorXd::Zero(nx);
209 if (0. >= th_acceptstep && th_acceptstep >= 0.5) {
210 throw_pretty(
"Invalid argument: " 211 <<
"th_acceptstep value should between 0 and 0.5");
213 th_acceptstep_ = th_acceptstep;
218 throw_pretty(
"Invalid argument: " 219 <<
"th_grad value has to be positive.");
226 throw_pretty(
"Invalid argument: " 227 <<
"reg value has to be positive.");
233 double prev_alpha = alphas[0];
234 if (prev_alpha != 1.) {
235 std::cerr <<
"Warning: alpha[0] should be 1" << std::endl;
237 for (std::size_t i = 1; i < alphas.size(); ++i) {
238 double alpha = alphas[i];
240 throw_pretty(
"Invalid argument: " 241 <<
"alpha values has to be positive.");
243 if (alpha >= prev_alpha) {
244 throw_pretty(
"Invalid argument: " 245 <<
"alpha values are monotonously decreasing.");
const double & get_th_grad() const
Return the gradient tolerance threshold.
const BoxQPSolution & get_solution() const
Return the stored solution.
void set_th_acceptstep(const double &th_acceptstep)
Modify the acceptance step threshold.
~BoxQP()
Destroy the Projected-Newton QP solver.
const std::vector< double > & get_alphas() const
Return the stack of step lengths using by the line-search procedure.
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.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW BoxQP(const std::size_t nx, 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.
Eigen::MatrixXd Hff_inv
Inverse of the free space Hessian.
const std::size_t & get_nx() const
Return the decision vector dimension.
void set_maxiter(const std::size_t &maxiter)
Modify the maximum allowed number of iterations.
const std::size_t & get_maxiter() const
Return the maximum allowed number of iterations.
void set_th_grad(const double &th_grad)
Modify the gradient tolerance threshold.
void set_nx(const std::size_t &nx)
Modify the decision vector dimension.
Eigen::VectorXd x
Decision vector.
const double & get_reg() const
Return the regularization value.
void set_alphas(const std::vector< double > &alphas)
Modify the stack of step lengths using by the line-search procedure.
std::vector< size_t > clamped_idx
Clamped space indexes.
const double & get_th_acceptstep() const
Return the acceptance step threshold.
void set_reg(const double ®)
Modify the regularization value.
std::vector< size_t > free_idx
Free space indexes.