Exotica
box_qp.h
Go to the documentation of this file.
1 //
2 // Copyright (c) 2019-2020, University of Edinburgh, University of Oxford
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 //
8 // * Redistributions of source code must retain the above copyright notice,
9 // this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // * Neither the name of nor the names of its contributors may be used to
14 // endorse or promote products derived from this software without specific
15 // prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 // POSSIBILITY OF SUCH DAMAGE.
28 //
29 
30 #ifndef EXOTICA_CORE_BOX_QP_H_
31 #define EXOTICA_CORE_BOX_QP_H_
32 
34 #include <Eigen/Dense>
35 #include <vector>
36 
37 namespace exotica
38 {
39 typedef struct BoxQPSolution
40 {
41  Eigen::MatrixXd Hff_inv;
42  Eigen::VectorXd x;
43  std::vector<size_t> free_idx;
44  std::vector<size_t> clamped_idx;
46 
47 inline BoxQPSolution BoxQP(const Eigen::MatrixXd& H, const Eigen::VectorXd& q, const Eigen::VectorXd& b_low, const Eigen::VectorXd& b_high, const Eigen::VectorXd& x_init, const double th_acceptstep, const int max_iterations, const double th_gradient_tolerance, const double lambda, bool use_polynomial_linesearch = true, bool use_cholesky_factorization = true)
48 {
49  if (lambda < 0.) ThrowPretty("lambda needs to be positive.");
50 
51  // gamma = acceptance threshold
52  // epsilon = gradient tolerance
53  // lambda = regularization for Cholesky factorization
54  const std::size_t nx = x_init.size();
55 
56  Eigen::VectorXd delta_xf(nx), x = x_init;
57  std::vector<size_t> clamped_idx, free_idx;
58  Eigen::VectorXd grad(nx);
59  Eigen::MatrixXd Hff(nx, nx), Hfc(nx, nx);
60 
61  std::vector<double> alphas_;
62  const std::size_t& n_alphas_ = 10;
63  alphas_.resize(n_alphas_);
64  const Eigen::VectorXd alphas_linear = Eigen::VectorXd::LinSpaced(n_alphas_, 1.0, 0.1);
65  for (std::size_t n = 0; n < n_alphas_; ++n)
66  {
67  if (use_polynomial_linesearch)
68  {
69  alphas_[n] = 1. / pow(2., static_cast<double>(n));
70  }
71  else
72  {
73  alphas_[n] = alphas_linear(n);
74  }
75  }
76  double fold_, fnew_;
77 
78  // Ensure a feasible warm-start
79  for (int i = 0; i < x.size(); ++i)
80  {
81  x(i) = std::max(std::min(x_init(i), b_high(i)), b_low(i));
82  }
83 
84  BoxQPSolution solution;
85  solution.clamped_idx.reserve(nx);
86  solution.free_idx.reserve(nx);
87  std::size_t num_free, num_clamped;
88  Eigen::LLT<Eigen::MatrixXd> Hff_inv_llt_;
89  Eigen::VectorXd qf_, xf_, xc_, dxf_, dx_(nx);
90 
91  for (int k = 0; k < max_iterations; ++k)
92  {
93  solution.clamped_idx.clear();
94  solution.free_idx.clear();
95 
96  // Compute the gradient
97  grad = q;
98  grad.noalias() += H * x;
99 
100  // Check if any element is at the limits
101  for (std::size_t i = 0; i < nx; ++i)
102  {
103  if ((x(i) == b_low(i) && grad(i) > 0.) || (x(i) == b_high(i) && grad(i) < 0.))
104  solution.clamped_idx.push_back(i);
105  else
106  solution.free_idx.push_back(i);
107  }
108  num_free = solution.free_idx.size();
109  num_clamped = solution.clamped_idx.size();
110 
111  // Check convergence
112  // a) Either norm of gradient is below threshold
113  // OR
114  // b) None of the dimensions is free (all are at boundary)
115  if (grad.lpNorm<Eigen::Infinity>() <= th_gradient_tolerance || num_free == 0)
116  {
117  // During first iteration return the inverse of the free Hessian
118  if (k == 0 && num_free != 0)
119  {
120  Hff.resize(num_free, num_free);
121  for (std::size_t i = 0; i < num_free; ++i)
122  {
123  const std::size_t& fi = solution.free_idx[i];
124  for (std::size_t j = 0; j < num_free; ++j)
125  {
126  Hff(i, j) = H(fi, solution.free_idx[j]);
127  }
128  }
129  // Add regularization
130  if (lambda != 0.)
131  {
132  Hff.diagonal().array() += lambda;
133  }
134 
135  // Compute the inverse
136  if (use_cholesky_factorization)
137  {
138  Hff_inv_llt_.compute(Hff);
139  const Eigen::ComputationInfo& info = Hff_inv_llt_.info();
140  if (info != Eigen::Success)
141  {
142  ThrowPretty("Error during Cholesky decomposition of Hff");
143  }
144  solution.Hff_inv.setIdentity(num_free, num_free);
145  Hff_inv_llt_.solveInPlace(solution.Hff_inv);
146  }
147  else
148  {
149  solution.Hff_inv = Hff.inverse();
150  }
151  }
152 
153  if (num_free == 0)
154  {
155  solution.Hff_inv.resize(num_free, num_free);
156  }
157 
158  // Set solution
159  solution.x = x;
160  return solution;
161  }
162 
163  // Compute the search direction as Newton step along the free space
164  qf_.resize(num_free);
165  xf_.resize(num_free);
166  xc_.resize(num_clamped);
167  dxf_.resize(num_free);
168  Hff.resize(num_free, num_free);
169  Hfc.resize(num_free, num_clamped);
170  for (std::size_t i = 0; i < num_free; ++i)
171  {
172  const std::size_t& fi = solution.free_idx[i];
173  qf_(i) = q(fi);
174  xf_(i) = x(fi);
175  for (std::size_t j = 0; j < num_free; ++j)
176  {
177  Hff(i, j) = H(fi, solution.free_idx[j]);
178  }
179  for (std::size_t j = 0; j < num_clamped; ++j)
180  {
181  const std::size_t cj = solution.clamped_idx[j];
182  xc_(j) = x(cj);
183  Hfc(i, j) = H(fi, cj);
184  }
185  }
186  if (lambda != 0.)
187  {
188  Hff.diagonal().array() += lambda;
189  }
190 
191  // Compute the inverse
192  if (use_cholesky_factorization)
193  {
194  Hff_inv_llt_.compute(Hff);
195  const Eigen::ComputationInfo& info = Hff_inv_llt_.info();
196  if (info != Eigen::Success)
197  {
198  ThrowPretty("Error during Cholesky decomposition of Hff (iter=" << k << "):\n"
199  << Hff << "\n"
200  << "H:\n"
201  << H << "\nnum_free: " << num_free << " num_clamped: " << num_clamped << " lambda: " << lambda);
202  }
203  solution.Hff_inv.setIdentity(num_free, num_free);
204  Hff_inv_llt_.solveInPlace(solution.Hff_inv);
205  }
206  else
207  {
208  solution.Hff_inv = Hff.inverse();
209  }
210 
211  dxf_ = -qf_;
212  if (num_clamped != 0)
213  {
214  dxf_.noalias() -= Hfc * xc_;
215  }
216  if (use_cholesky_factorization)
217  {
218  Hff_inv_llt_.solveInPlace(dxf_);
219  }
220  else
221  {
222  dxf_ = solution.Hff_inv * dxf_;
223  }
224  dxf_ -= xf_;
225  dx_.setZero();
226  for (std::size_t i = 0; i < num_free; ++i)
227  {
228  dx_(solution.free_idx[i]) = dxf_(i);
229  }
230 
231  // Try different step lengths
232  Eigen::VectorXd xnew_(nx);
233  fold_ = 0.5 * x.dot(H * x) + q.dot(x);
234  for (std::vector<double>::const_iterator it = alphas_.begin(); it != alphas_.end(); ++it)
235  {
236  double steplength = *it;
237  for (std::size_t i = 0; i < nx; ++i)
238  {
239  xnew_(i) = std::max(std::min(x(i) + steplength * dx_(i), b_high(i)), b_low(i));
240  }
241  fnew_ = 0.5 * xnew_.dot(H * xnew_) + q.dot(xnew_);
242  if (fold_ - fnew_ > th_acceptstep * grad.dot(x - xnew_))
243  {
244  x = xnew_;
245  break;
246  }
247 
248  // If line-search fails, return.
249  if (it == alphas_.end() - 1)
250  {
251  solution.x = x;
252  return solution;
253  }
254  }
255  }
256 
257  solution.x = x;
258  return solution;
259 }
260 
261 inline BoxQPSolution BoxQP(const Eigen::MatrixXd& H, const Eigen::VectorXd& q,
262  const Eigen::VectorXd& b_low, const Eigen::VectorXd& b_high,
263  const Eigen::VectorXd& x_init)
264 {
265  constexpr double epsilon = 1e-5;
266  constexpr double gamma = 0.1;
267  constexpr int max_iterations = 100;
268  constexpr double lambda = 1e-5;
269  return BoxQP(H, q, b_low, b_high, x_init, gamma, max_iterations, epsilon, lambda, true, true);
270 }
271 } // namespace exotica
272 
273 #endif // EXOTICA_CORE_BOX_QP_H_
exotica::BoxQPSolution::clamped_idx
std::vector< size_t > clamped_idx
Definition: box_qp.h:44
exotica
Definition: cartpole_dynamics_solver.h:38
exotica::BoxQPSolution::Hff_inv
Eigen::MatrixXd Hff_inv
Definition: box_qp.h:41
exotica::BoxQPSolution
struct exotica::BoxQPSolution BoxQPSolution
ThrowPretty
#define ThrowPretty(m)
Definition: exception.h:36
exotica::BoxQPSolution::x
Eigen::VectorXd x
Definition: box_qp.h:42
exotica::BoxQPSolution
Definition: box_qp.h:39
exception.h
exotica::BoxQPSolution::free_idx
std::vector< size_t > free_idx
Definition: box_qp.h:43
exotica::BoxQP
BoxQPSolution BoxQP(const Eigen::MatrixXd &H, const Eigen::VectorXd &q, const Eigen::VectorXd &b_low, const Eigen::VectorXd &b_high, const Eigen::VectorXd &x_init, const double th_acceptstep, const int max_iterations, const double th_gradient_tolerance, const double lambda, bool use_polynomial_linesearch=true, bool use_cholesky_factorization=true)
Definition: box_qp.h:47