box_qp_old.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_OLD_H_
31 #define EXOTICA_CORE_BOX_QP_OLD_H_
32 
33 #include <Eigen/Dense>
34 #include <vector>
35 
37 
38 namespace exotica
39 {
40 inline BoxQPSolution ExoticaBoxQP(const Eigen::MatrixXd& H, const Eigen::VectorXd& q,
41  const Eigen::VectorXd& b_low, const Eigen::VectorXd& b_high,
42  const Eigen::VectorXd& x_init, const double gamma,
43  const int max_iterations, const double epsilon, const double lambda,
44  bool use_polynomial_linesearch = false,
45  bool use_cholesky_factorization = false)
46 {
47  int it = 0;
48  Eigen::VectorXd delta_xf, x = x_init;
49  std::vector<size_t> clamped_idx, free_idx;
50  Eigen::VectorXd grad = q + H * x_init;
51  Eigen::MatrixXd Hff, Hfc, Hff_inv;
52 
53  std::vector<double> alphas_;
54  const std::size_t& n_alphas_ = 10;
55  alphas_.resize(n_alphas_);
56  const Eigen::VectorXd alphas_linear = Eigen::VectorXd::LinSpaced(10, 1.0, 0.1);
57  for (std::size_t n = 0; n < n_alphas_; ++n)
58  {
59  if (use_polynomial_linesearch)
60  {
61  alphas_[n] = 1. / pow(2., static_cast<double>(n));
62  }
63  else
64  {
65  alphas_[n] = alphas_linear(n);
66  }
67  }
68 
69  // Ensure a feasible warm-start
70  for (int i = 0; i < x.size(); ++i)
71  {
72  x(i) = std::max(std::min(x_init(i), b_high(i)), b_low(i));
73  }
74 
75  if (use_cholesky_factorization)
76  {
77  Hff_inv = (Eigen::MatrixXd::Identity(H.rows(), H.cols()) * lambda + H).llt().solve(Eigen::MatrixXd::Identity(H.rows(), H.cols()));
78  }
79  else
80  {
81  Hff_inv = (Eigen::MatrixXd::Identity(H.rows(), H.cols()) * lambda + H).inverse();
82  }
83 
84  if (grad.norm() <= epsilon)
85  {
86  for (int i = 0; i < x.size(); ++i) free_idx.push_back(i);
87  return {Hff_inv, x_init, free_idx, clamped_idx};
88  }
89 
90  while (grad.norm() > epsilon && it < max_iterations)
91  {
92  ++it;
93  grad = q + H * x;
94  clamped_idx.clear();
95  free_idx.clear();
96 
97  for (int i = 0; i < grad.size(); ++i)
98  {
99  if ((x(i) == b_low(i) && grad(i) > 0) || (x(i) == b_high(i) && grad(i) < 0))
100  clamped_idx.push_back(i);
101  else
102  free_idx.push_back(i);
103  }
104 
105  if (free_idx.size() == 0)
106  break;
107 
108  Hff.resize(free_idx.size(), free_idx.size());
109  Hfc.resize(free_idx.size(), clamped_idx.size());
110 
111  if (clamped_idx.size() == 0)
112  Hff = H;
113  else
114  {
115  for (size_t i = 0; i < free_idx.size(); ++i)
116  for (size_t j = 0; j < free_idx.size(); ++j)
117  Hff(i, j) = H(free_idx[i], free_idx[j]);
118 
119  for (size_t i = 0; i < free_idx.size(); ++i)
120  for (size_t j = 0; j < clamped_idx.size(); ++j)
121  Hfc(i, j) = H(free_idx[i], clamped_idx[j]);
122  }
123 
124  // NOTE: Array indexing not supported in current eigen version
125  Eigen::VectorXd q_free(free_idx.size()), x_free(free_idx.size()), x_clamped(clamped_idx.size());
126  for (size_t i = 0; i < free_idx.size(); ++i)
127  {
128  q_free(i) = q(free_idx[i]);
129  x_free(i) = x(free_idx[i]);
130  }
131 
132  for (size_t j = 0; j < clamped_idx.size(); ++j)
133  x_clamped(j) = x(clamped_idx[j]);
134 
135  // Compute the inverse
136  Hff.diagonal().array() += lambda;
137  if (use_cholesky_factorization)
138  {
139  Hff_inv = Hff.llt().solve(Eigen::MatrixXd::Identity(Hff.rows(), Hff.cols()));
140  }
141  else
142  {
143  Hff_inv = Hff.inverse();
144  }
145 
146  if (clamped_idx.size() == 0)
147  delta_xf = -Hff_inv * (q_free)-x_free;
148  else
149  delta_xf = -Hff_inv * (q_free + Hfc * x_clamped) - x_free;
150 
151  double f_old = (0.5 * x.transpose() * H * x + q.transpose() * x)(0);
152 
153  bool armijo_reached = false;
154  Eigen::VectorXd x_new;
155  for (std::size_t ai = 0; ai < alphas_.size(); ++ai)
156  {
157  const double& alpha = alphas_[ai];
158 
159  x_new = x;
160  for (size_t i = 0; i < free_idx.size(); ++i)
161  x_new(free_idx[i]) = std::max(std::min(
162  x(free_idx[i]) + alpha * delta_xf(i), b_high(i)),
163  b_low(i));
164 
165  double f_new = (0.5 * x_new.transpose() * H * x_new + q.transpose() * x_new)(0);
166  Eigen::VectorXd x_diff = x - x_new;
167 
168  // armijo criterion>
169  double armijo_coef = (f_old - f_new) / (grad.transpose() * x_diff + 1e-5);
170  if (armijo_coef > gamma)
171  {
172  armijo_reached = true;
173  x = x_new;
174  break;
175  }
176  }
177 
178  // break if no step made
179  if (!armijo_reached) break;
180  }
181 
182  return {Hff_inv, x, free_idx, clamped_idx};
183 }
184 
185 inline BoxQPSolution ExoticaBoxQP(const Eigen::MatrixXd& H, const Eigen::VectorXd& q,
186  const Eigen::VectorXd& b_low, const Eigen::VectorXd& b_high,
187  const Eigen::VectorXd& x_init)
188 {
189  constexpr double epsilon = 1e-5;
190  constexpr double gamma = 0.1;
191  constexpr int max_iterations = 100;
192  constexpr double lambda = 1e-5;
193  return ExoticaBoxQP(H, q, b_low, b_high, x_init, gamma, max_iterations, epsilon, lambda, false, false);
194 }
195 } // namespace exotica
196 
197 #endif // EXOTICA_CORE_BOX_QP_OLD_H_
box_qp.h
exotica::ExoticaBoxQP
BoxQPSolution ExoticaBoxQP(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 gamma, const int max_iterations, const double epsilon, const double lambda, bool use_polynomial_linesearch=false, bool use_cholesky_factorization=false)
Definition: box_qp_old.h:40
epsilon
double epsilon
generate_initializers.n
n
Definition: generate_initializers.py:637
exotica
Definition: collision_scene.h:46
x
double x
exotica::BoxQPSolution
Definition: box_qp.h:39
inverse
TFSIMD_FORCE_INLINE Quaternion inverse(const Quaternion &q)


exotica_core
Author(s): Yiming Yang, Michael Camilleri
autogenerated on Fri Aug 2 2024 08:43:02