eiquadprog.hpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2019,2022 CNRS INRIA
3 //
4 // This file is part of eiquadprog.
5 //
6 // eiquadprog is free software: you can redistribute it and/or modify
7 // it under the terms of the GNU Lesser General Public License as published by
8 // the Free Software Foundation, either version 3 of the License, or
9 //(at your option) any later version.
10 
11 // eiquadprog is distributed in the hope that it will be useful,
12 // but WITHOUT ANY WARRANTY; without even the implied warranty of
13 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 // GNU Lesser General Public License for more details.
15 
16 // You should have received a copy of the GNU Lesser General Public License
17 // along with eiquadprog. If not, see <https://www.gnu.org/licenses/>.
18 
19 #ifndef _EIGEN_QUADSOLVE_HPP_
20 #define _EIGEN_QUADSOLVE_HPP_
21 
22 /*
23  FILE eiquadprog.hpp
24  NOTE: this is a modified of uQuadProg++ package, working with Eigen data
25  structures. uQuadProg++ is itself a port made by Angelo Furfaro of QuadProg++
26  originally developed by Luca Di Gaspero, working with ublas data structures.
27  The quadprog_solve() function implements the algorithm of Goldfarb and Idnani
28  for the solution of a (convex) Quadratic Programming problem
29  by means of a dual method.
30  The problem is in the form:
31  min 0.5 * x G x + g0 x
32  s.t.
33  CE^T x + ce0 = 0
34  CI^T x + ci0 >= 0
35  The matrix and vectors dimensions are as follows:
36  G: n * n
37  g0: n
38  CE: n * p
39  ce0: p
40  CI: n * m
41  ci0: m
42  x: n
43  The function will return the cost of the solution written in the x vector or
44  std::numeric_limits::infinity() if the problem is infeasible. In the latter
45  case the value of the x vector is not correct. References: D. Goldfarb, A.
46  Idnani. A numerically stable dual method for solving strictly convex quadratic
47  programs. Mathematical Programming 27 (1983) pp. 1-33. Notes:
48  1. pay attention in setting up the vectors ce0 and ci0.
49  If the constraints of your problem are specified in the form
50  A^T x = b and C^T x >= d, then you should set ce0 = -b and ci0 = -d.
51  2. The matrix G is modified within the function since it is used to compute
52  the G = L^T L cholesky factorization for further computations inside the
53  function. If you need the original matrix G you should make a copy of it and
54  pass the copy to the function. The author will be grateful if the researchers
55  using this software will acknowledge the contribution of this modified function
56  and of Di Gaspero's original version in their research papers. LICENSE
57  Copyright (2011) Benjamin Stephens
58  Copyright (2010) Gael Guennebaud
59  Copyright (2008) Angelo Furfaro
60  Copyright (2006) Luca Di Gaspero
61  This file is a porting of QuadProg++ routine, originally developed
62  by Luca Di Gaspero, exploiting uBlas data structures for vectors and
63  matrices instead of native C++ array.
64  uquadprog is free software; you can redistribute it and/or modify
65  it under the terms of the GNU General Public License as published by
66  the Free Software Foundation; either version 2 of the License, or
67  (at your option) any later version.
68  uquadprog is distributed in the hope that it will be useful,
69  but WITHOUT ANY WARRANTY; without even the implied warranty of
70  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
71  GNU General Public License for more details.
72  You should have received a copy of the GNU General Public License
73  along with uquadprog; if not, write to the Free Software
74  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
75  */
76 
77 #include <Eigen/Cholesky>
78 #include <Eigen/Core>
79 
80 #include "eiquadprog/deprecated.hpp"
82 
83 // namespace internal {
84 namespace eiquadprog {
85 namespace solvers {
86 
87 inline void compute_d(Eigen::VectorXd &d, const Eigen::MatrixXd &J,
88  const Eigen::VectorXd &np) {
89  d.noalias() = J.adjoint() * np;
90 }
91 
92 inline void update_z(Eigen::VectorXd &z, const Eigen::MatrixXd &J,
93  const Eigen::VectorXd &d, size_t iq) {
94  z.noalias() = J.rightCols(z.size() - iq) * d.tail(d.size() - iq);
95 }
96 
97 inline void update_r(const Eigen::MatrixXd &R, Eigen::VectorXd &r,
98  const Eigen::VectorXd &d, size_t iq) {
99  r.head(iq) = d.head(iq);
100  R.topLeftCorner(iq, iq).triangularView<Eigen::Upper>().solveInPlace(
101  r.head(iq));
102 }
103 
104 bool add_constraint(Eigen::MatrixXd &R, Eigen::MatrixXd &J, Eigen::VectorXd &d,
105  size_t &iq, double &R_norm);
106 void delete_constraint(Eigen::MatrixXd &R, Eigen::MatrixXd &J,
107  Eigen::VectorXi &A, Eigen::VectorXd &u, size_t p,
108  size_t &iq, size_t l);
109 
110 double solve_quadprog(Eigen::LLT<Eigen::MatrixXd, Eigen::Lower> &chol,
111  double c1, Eigen::VectorXd &g0, const Eigen::MatrixXd &CE,
112  const Eigen::VectorXd &ce0, const Eigen::MatrixXd &CI,
113  const Eigen::VectorXd &ci0, Eigen::VectorXd &x,
114  Eigen::VectorXi &A, size_t &q);
115 
116 double solve_quadprog(Eigen::LLT<Eigen::MatrixXd, Eigen::Lower> &chol,
117  double c1, Eigen::VectorXd &g0, const Eigen::MatrixXd &CE,
118  const Eigen::VectorXd &ce0, const Eigen::MatrixXd &CI,
119  const Eigen::VectorXd &ci0, Eigen::VectorXd &x,
120  Eigen::VectorXd &y, Eigen::VectorXi &A, size_t &q);
121 
122 EIQUADPROG_DEPRECATED
123 double solve_quadprog2(Eigen::LLT<Eigen::MatrixXd, Eigen::Lower> &chol,
124  double c1, Eigen::VectorXd &g0,
125  const Eigen::MatrixXd &CE, const Eigen::VectorXd &ce0,
126  const Eigen::MatrixXd &CI, const Eigen::VectorXd &ci0,
127  Eigen::VectorXd &x, Eigen::VectorXi &A, size_t &q) {
128  return solve_quadprog(chol, c1, g0, CE, ce0, CI, ci0, x, A, q);
129 }
130 
131 /* solve_quadprog is used for on-demand QP solving */
132 double solve_quadprog(Eigen::MatrixXd &G, Eigen::VectorXd &g0,
133  const Eigen::MatrixXd &CE, const Eigen::VectorXd &ce0,
134  const Eigen::MatrixXd &CI, const Eigen::VectorXd &ci0,
135  Eigen::VectorXd &x, Eigen::VectorXi &activeSet,
136  size_t &activeSetSize);
137 
138 double solve_quadprog(Eigen::MatrixXd &G, Eigen::VectorXd &g0,
139  const Eigen::MatrixXd &CE, const Eigen::VectorXd &ce0,
140  const Eigen::MatrixXd &CI, const Eigen::VectorXd &ci0,
141  Eigen::VectorXd &x, Eigen::VectorXd &y,
142  Eigen::VectorXi &activeSet, size_t &activeSetSize);
143 // }
144 
145 } // namespace solvers
146 } // namespace eiquadprog
147 
148 #endif
eiquadprog::solvers::compute_d
void compute_d(Eigen::VectorXd &d, const Eigen::MatrixXd &J, const Eigen::VectorXd &np)
Definition: eiquadprog.hpp:87
eiquadprog::solvers::delete_constraint
void delete_constraint(Eigen::MatrixXd &R, Eigen::MatrixXd &J, Eigen::VectorXi &A, Eigen::VectorXd &u, size_t p, size_t &iq, size_t l)
eiquadprog::solvers::update_z
void update_z(Eigen::VectorXd &z, const Eigen::MatrixXd &J, const Eigen::VectorXd &d, size_t iq)
Definition: eiquadprog.hpp:92
eiquadprog
Definition: eiquadprog-fast.hpp:63
eiquadprog::solvers::solve_quadprog2
EIQUADPROG_DEPRECATED double solve_quadprog2(Eigen::LLT< Eigen::MatrixXd, Eigen::Lower > &chol, double c1, Eigen::VectorXd &g0, const Eigen::MatrixXd &CE, const Eigen::VectorXd &ce0, const Eigen::MatrixXd &CI, const Eigen::VectorXd &ci0, Eigen::VectorXd &x, Eigen::VectorXi &A, size_t &q)
Definition: eiquadprog.hpp:123
eiquadprog::solvers::update_r
void update_r(const Eigen::MatrixXd &R, Eigen::VectorXd &r, const Eigen::VectorXd &d, size_t iq)
Definition: eiquadprog.hpp:97
eiquadprog::solvers::add_constraint
bool add_constraint(Eigen::MatrixXd &R, Eigen::MatrixXd &J, Eigen::VectorXd &d, size_t &iq, double &R_norm)
eiquadprog::solvers::solve_quadprog
double solve_quadprog(Eigen::LLT< Eigen::MatrixXd, Eigen::Lower > &chol, double c1, Eigen::VectorXd &g0, const Eigen::MatrixXd &CE, const Eigen::VectorXd &ce0, const Eigen::MatrixXd &CI, const Eigen::VectorXd &ci0, Eigen::VectorXd &x, Eigen::VectorXi &A, size_t &q)
eiquadprog-utils.hxx