crocoddyl  1.7.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, const 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  // Initialized the values of vectors
40  x_.setZero();
41  xnew_.setZero();
42  g_.setZero();
43  dx_.setZero();
44 
45  // Reserve the space and compute alphas
46  solution_.x = Eigen::VectorXd::Zero(nx);
47  solution_.clamped_idx.reserve(nx_);
48  solution_.free_idx.reserve(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));
53  }
54 }
55 
57 
58 const BoxQPSolution& BoxQP::solve(const Eigen::MatrixXd& H, const Eigen::VectorXd& q, const Eigen::VectorXd& lb,
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_) + ")");
63  }
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_) + ")");
67  }
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_) + ")");
71  }
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_) + ")");
75  }
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_) + ")");
79  }
80 
81  // We need to enforce feasible warm-starting of the algorithm
82  for (std::size_t i = 0; i < nx_; ++i) {
83  x_(i) = std::max(std::min(xinit(i), ub(i)), lb(i));
84  }
85 
86  // Start the numerical iterations
87  for (std::size_t k = 0; k < maxiter_; ++k) {
88  solution_.clamped_idx.clear();
89  solution_.free_idx.clear();
90  // Compute the gradient
91  g_ = q;
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.)) {
99  solution_.clamped_idx.push_back(j);
100  } else {
101  solution_.free_idx.push_back(j);
102  }
103  }
104 
105  // Check convergence
106  nf_ = solution_.free_idx.size();
107  nc_ = solution_.clamped_idx.size();
108  if (g_.lpNorm<Eigen::Infinity>() <= th_grad_ || nf_ == 0) {
109  if (k == 0) { // compute the inverse of the free Hessian
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]);
115  }
116  }
117  if (reg_ != 0.) {
118  Hff_.diagonal().array() += reg_;
119  }
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");
124  }
125  solution_.Hff_inv.setIdentity(nf_, nf_);
126  Hff_inv_llt_.solveInPlace(solution_.Hff_inv);
127  }
128  solution_.x = x_;
129  return solution_;
130  }
131 
132  // Compute the search direction as Newton step along the free space
133  qf_.resize(nf_);
134  xf_.resize(nf_);
135  xc_.resize(nc_);
136  dxf_.resize(nf_);
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];
141  qf_(i) = q(fi);
142  xf_(i) = x_(fi);
143  for (std::size_t j = 0; j < nf_; ++j) {
144  Hff_(i, j) = H(fi, solution_.free_idx[j]);
145  }
146  for (std::size_t j = 0; j < nc_; ++j) {
147  const std::size_t cj = solution_.clamped_idx[j];
148  xc_(j) = x_(cj);
149  Hfc_(i, j) = H(fi, cj);
150  }
151  }
152  if (reg_ != 0.) {
153  Hff_.diagonal().array() += reg_;
154  }
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");
159  }
160  solution_.Hff_inv.setIdentity(nf_, nf_);
161  Hff_inv_llt_.solveInPlace(solution_.Hff_inv);
162  dxf_ = -qf_;
163  if (nc_ != 0) {
164  dxf_.noalias() -= Hfc_ * xc_;
165  }
166  Hff_inv_llt_.solveInPlace(dxf_);
167  dxf_ -= xf_;
168  dx_.setZero();
169  for (std::size_t i = 0; i < nf_; ++i) {
170  dx_(solution_.free_idx[i]) = dxf_(i);
171  }
172 
173  // Try different step lengths
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));
179  }
180  fnew_ = 0.5 * xnew_.dot(H * xnew_) + q.dot(xnew_);
181  if (fold_ - fnew_ > th_acceptstep_ * g_.dot(x_ - xnew_)) {
182  x_ = xnew_;
183  break;
184  }
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_ = Eigen::VectorXd::Zero(nx);
208  xnew_ = Eigen::VectorXd::Zero(nx);
209  g_ = Eigen::VectorXd::Zero(nx);
210  dx_ = Eigen::VectorXd::Zero(nx);
211 }
212 
213 void BoxQP::set_maxiter(const std::size_t maxiter) { maxiter_ = maxiter; }
214 
215 void BoxQP::set_th_acceptstep(const double th_acceptstep) {
216  if (0. >= th_acceptstep && th_acceptstep >= 0.5) {
217  throw_pretty("Invalid argument: "
218  << "th_acceptstep value should between 0 and 0.5");
219  }
220  th_acceptstep_ = th_acceptstep;
221 }
222 
223 void BoxQP::set_th_grad(const double th_grad) {
224  if (0. > th_grad) {
225  throw_pretty("Invalid argument: "
226  << "th_grad value has to be positive.");
227  }
228  th_grad_ = th_grad;
229 }
230 
231 void BoxQP::set_reg(const double reg) {
232  if (0. > reg) {
233  throw_pretty("Invalid argument: "
234  << "reg value has to be positive.");
235  }
236  reg_ = reg;
237 }
238 
239 void BoxQP::set_alphas(const std::vector<double>& alphas) {
240  double prev_alpha = alphas[0];
241  if (prev_alpha != 1.) {
242  std::cerr << "Warning: alpha[0] should be 1" << std::endl;
243  }
244  for (std::size_t i = 1; i < alphas.size(); ++i) {
245  double alpha = alphas[i];
246  if (0. >= alpha) {
247  throw_pretty("Invalid argument: "
248  << "alpha values has to be positive.");
249  }
250  if (alpha >= prev_alpha) {
251  throw_pretty("Invalid argument: "
252  << "alpha values are monotonously decreasing.");
253  }
254  prev_alpha = alpha;
255  }
256  alphas_ = alphas;
257 }
258 
259 } // namespace crocoddyl
void set_th_acceptstep(const double th_acceptstep)
Modify the acceptance step threshold.
Definition: box-qp.cpp:215
const BoxQPSolution & get_solution() const
Return the stored solution.
Definition: box-qp.cpp:191
~BoxQP()
Destroy the Projected-Newton QP solver.
Definition: box-qp.cpp:56
Box QP solution.
Definition: box-qp.hpp:28
void set_nx(const std::size_t nx)
Modify the decision vector dimension.
Definition: box-qp.cpp:205
const std::vector< double > & get_alphas() const
Return the stack of step lengths using by the line-search procedure.
Definition: box-qp.cpp:203
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:58
Eigen::MatrixXd Hff_inv
Inverse of the free space Hessian.
Definition: box-qp.hpp:48
double get_th_acceptstep() const
Return the acceptance step threshold.
Definition: box-qp.cpp:197
Eigen::VectorXd x
Decision vector.
Definition: box-qp.hpp:49
double get_th_grad() const
Return the gradient tolerance threshold.
Definition: box-qp.cpp:199
void set_alphas(const std::vector< double > &alphas)
Modify the stack of step lengths using by the line-search procedure.
Definition: box-qp.cpp:239
std::size_t get_maxiter() const
Return the maximum allowed number of iterations.
Definition: box-qp.cpp:195
double get_reg() const
Return the regularization value.
Definition: box-qp.cpp:201
std::vector< size_t > clamped_idx
Clamped space indexes.
Definition: box-qp.hpp:51
void set_reg(const double reg)
Modify the regularization value.
Definition: box-qp.cpp:231
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.
Definition: box-qp.cpp:15
std::size_t get_nx() const
Return the decision vector dimension.
Definition: box-qp.cpp:193
void set_th_grad(const double th_grad)
Modify the gradient tolerance threshold.
Definition: box-qp.cpp:223
std::vector< size_t > free_idx
Free space indexes.
Definition: box-qp.hpp:50
void set_maxiter(const std::size_t maxiter)
Modify the maximum allowed number of iterations.
Definition: box-qp.cpp:213