crocoddyl  1.3.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
box-qp.cpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2020, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #include <iostream>
10 #include "crocoddyl/core/solvers/box-qp.hpp"
11 #include "crocoddyl/core/utils/exception.hpp"
12 
13 namespace crocoddyl {
14 
15 BoxQP::BoxQP(const std::size_t nx, std::size_t maxiter, const double th_acceptstep, const double th_grad,
16  const double reg)
17  : nx_(nx),
18  maxiter_(maxiter),
19  th_acceptstep_(th_acceptstep),
20  th_grad_(th_grad),
21  reg_(reg),
22  fold_(0.),
23  fnew_(0.),
24  x_(nx),
25  xnew_(nx),
26  g_(nx),
27  dx_(nx) {
28  // Check if values have a proper range
29  if (0. >= th_acceptstep && th_acceptstep >= 0.5) {
30  std::cerr << "Warning: th_acceptstep value should between 0 and 0.5" << std::endl;
31  }
32  if (0. > th_grad) {
33  std::cerr << "Warning: th_grad value has to be positive." << std::endl;
34  }
35  if (0. > reg) {
36  std::cerr << "Warning: reg value has to be positive." << std::endl;
37  }
38 
39  // Reserve the space and compute alphas
40  solution_.clamped_idx.reserve(nx_);
41  solution_.free_idx.reserve(nx_);
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));
46  }
47 }
48 
50 
51 const BoxQPSolution& BoxQP::solve(const Eigen::MatrixXd& H, const Eigen::VectorXd& q, const Eigen::VectorXd& lb,
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_) + ")");
56  }
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_) + ")");
60  }
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_) + ")");
64  }
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_) + ")");
68  }
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_) + ")");
72  }
73 
74  // We need to enforce feasible warm-starting of the algorithm
75  for (std::size_t i = 0; i < nx_; ++i) {
76  x_(i) = std::max(std::min(xinit(i), ub(i)), lb(i));
77  }
78 
79  // Start the numerical iterations
80  for (std::size_t k = 0; k < maxiter_; ++k) {
81  solution_.clamped_idx.clear();
82  solution_.free_idx.clear();
83  // Compute the gradient
84  g_ = q;
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.)) {
92  solution_.clamped_idx.push_back(j);
93  } else {
94  solution_.free_idx.push_back(j);
95  }
96  }
97 
98  // Check convergence
99  nf_ = solution_.free_idx.size();
100  nc_ = solution_.clamped_idx.size();
101  if (g_.lpNorm<Eigen::Infinity>() <= th_grad_ || nf_ == 0) {
102  if (k == 0) { // compute the inverse of the free Hessian
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]);
108  }
109  }
110  if (reg_ != 0.) {
111  Hff_.diagonal().array() += reg_;
112  }
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");
117  }
118  solution_.Hff_inv.setIdentity(nf_, nf_);
119  Hff_inv_llt_.solveInPlace(solution_.Hff_inv);
120  }
121  solution_.x = x_;
122  return solution_;
123  }
124 
125  // Compute the search direction as Newton step along the free space
126  qf_.resize(nf_);
127  xf_.resize(nf_);
128  xc_.resize(nc_);
129  dxf_.resize(nf_);
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];
134  qf_(i) = q(fi);
135  xf_(i) = x_(fi);
136  for (std::size_t j = 0; j < nf_; ++j) {
137  Hff_(i, j) = H(fi, solution_.free_idx[j]);
138  }
139  for (std::size_t j = 0; j < nc_; ++j) {
140  const std::size_t cj = solution_.clamped_idx[j];
141  xc_(j) = x_(cj);
142  Hfc_(i, j) = H(fi, cj);
143  }
144  }
145  if (reg_ != 0.) {
146  Hff_.diagonal().array() += reg_;
147  }
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");
152  }
153  solution_.Hff_inv.setIdentity(nf_, nf_);
154  Hff_inv_llt_.solveInPlace(solution_.Hff_inv);
155  dxf_ = -qf_;
156  if (nc_ != 0) {
157  dxf_.noalias() -= Hfc_ * xc_;
158  }
159  Hff_inv_llt_.solveInPlace(dxf_);
160  dxf_ -= xf_;
161  dx_.setZero();
162  for (std::size_t i = 0; i < nf_; ++i) {
163  dx_(solution_.free_idx[i]) = dxf_(i);
164  }
165 
166  // Try different step lengths
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));
172  }
173  fnew_ = 0.5 * xnew_.dot(H * xnew_) + q.dot(xnew_);
174  if (fold_ - fnew_ > th_acceptstep_ * g_.dot(x_ - xnew_)) {
175  x_ = xnew_;
176  break;
177  }
178  }
179  }
180  solution_.x = x_;
181  return solution_;
182 }
183 
184 const BoxQPSolution& BoxQP::get_solution() const { return solution_; }
185 
186 const std::size_t& BoxQP::get_nx() const { return nx_; }
187 
188 const std::size_t& BoxQP::get_maxiter() const { return maxiter_; }
189 
190 const double& BoxQP::get_th_acceptstep() const { return th_acceptstep_; }
191 
192 const double& BoxQP::get_th_grad() const { return th_grad_; }
193 
194 const double& BoxQP::get_reg() const { return reg_; }
195 
196 const std::vector<double>& BoxQP::get_alphas() const { return alphas_; }
197 
198 void BoxQP::set_nx(const std::size_t& nx) {
199  nx_ = nx;
200  x_ = Eigen::VectorXd::Zero(nx);
201  xnew_ = Eigen::VectorXd::Zero(nx);
202  g_ = Eigen::VectorXd::Zero(nx);
203  dx_ = Eigen::VectorXd::Zero(nx);
204 }
205 
206 void BoxQP::set_maxiter(const std::size_t& maxiter) { maxiter_ = maxiter; }
207 
208 void BoxQP::set_th_acceptstep(const double& th_acceptstep) {
209  if (0. >= th_acceptstep && th_acceptstep >= 0.5) {
210  throw_pretty("Invalid argument: "
211  << "th_acceptstep value should between 0 and 0.5");
212  }
213  th_acceptstep_ = th_acceptstep;
214 }
215 
216 void BoxQP::set_th_grad(const double& th_grad) {
217  if (0. > th_grad) {
218  throw_pretty("Invalid argument: "
219  << "th_grad value has to be positive.");
220  }
221  th_grad_ = th_grad;
222 }
223 
224 void BoxQP::set_reg(const double& reg) {
225  if (0. > reg) {
226  throw_pretty("Invalid argument: "
227  << "reg value has to be positive.");
228  }
229  reg_ = reg;
230 }
231 
232 void BoxQP::set_alphas(const std::vector<double>& alphas) {
233  double prev_alpha = alphas[0];
234  if (prev_alpha != 1.) {
235  std::cerr << "Warning: alpha[0] should be 1" << std::endl;
236  }
237  for (std::size_t i = 1; i < alphas.size(); ++i) {
238  double alpha = alphas[i];
239  if (0. >= alpha) {
240  throw_pretty("Invalid argument: "
241  << "alpha values has to be positive.");
242  }
243  if (alpha >= prev_alpha) {
244  throw_pretty("Invalid argument: "
245  << "alpha values are monotonously decreasing.");
246  }
247  prev_alpha = alpha;
248  }
249  alphas_ = alphas;
250 }
251 
252 } // namespace crocoddyl
const double & get_th_grad() const
Return the gradient tolerance threshold.
Definition: box-qp.cpp:192
const BoxQPSolution & get_solution() const
Return the stored solution.
Definition: box-qp.cpp:184
void set_th_acceptstep(const double &th_acceptstep)
Modify the acceptance step threshold.
Definition: box-qp.cpp:208
~BoxQP()
Destroy the Projected-Newton QP solver.
Definition: box-qp.cpp:49
Box QP solution.
Definition: box-qp.hpp:29
const std::vector< double > & get_alphas() const
Return the stack of step lengths using by the line-search procedure.
Definition: box-qp.cpp:196
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.
Definition: box-qp.cpp:51
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.
Definition: box-qp.cpp:15
Eigen::MatrixXd Hff_inv
Inverse of the free space Hessian.
Definition: box-qp.hpp:49
const std::size_t & get_nx() const
Return the decision vector dimension.
Definition: box-qp.cpp:186
void set_maxiter(const std::size_t &maxiter)
Modify the maximum allowed number of iterations.
Definition: box-qp.cpp:206
const std::size_t & get_maxiter() const
Return the maximum allowed number of iterations.
Definition: box-qp.cpp:188
void set_th_grad(const double &th_grad)
Modify the gradient tolerance threshold.
Definition: box-qp.cpp:216
void set_nx(const std::size_t &nx)
Modify the decision vector dimension.
Definition: box-qp.cpp:198
Eigen::VectorXd x
Decision vector.
Definition: box-qp.hpp:50
const double & get_reg() const
Return the regularization value.
Definition: box-qp.cpp:194
void set_alphas(const std::vector< double > &alphas)
Modify the stack of step lengths using by the line-search procedure.
Definition: box-qp.cpp:232
std::vector< size_t > clamped_idx
Clamped space indexes.
Definition: box-qp.hpp:52
const double & get_th_acceptstep() const
Return the acceptance step threshold.
Definition: box-qp.cpp:190
void set_reg(const double &reg)
Modify the regularization value.
Definition: box-qp.cpp:224
std::vector< size_t > free_idx
Free space indexes.
Definition: box-qp.hpp:51