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" |
81 |
|
|
#include "eiquadprog/eiquadprog-utils.hxx" |
82 |
|
|
|
83 |
|
|
// namespace internal { |
84 |
|
|
namespace eiquadprog { |
85 |
|
|
namespace solvers { |
86 |
|
|
|
87 |
|
12 |
inline void compute_d(Eigen::VectorXd &d, const Eigen::MatrixXd &J, |
88 |
|
|
const Eigen::VectorXd &np) { |
89 |
✓✗✓✗ ✓✗ |
12 |
d.noalias() = J.adjoint() * np; |
90 |
|
12 |
} |
91 |
|
|
|
92 |
|
12 |
inline void update_z(Eigen::VectorXd &z, const Eigen::MatrixXd &J, |
93 |
|
|
const Eigen::VectorXd &d, size_t iq) { |
94 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
12 |
z.noalias() = J.rightCols(z.size() - iq) * d.tail(d.size() - iq); |
95 |
|
12 |
} |
96 |
|
|
|
97 |
|
12 |
inline void update_r(const Eigen::MatrixXd &R, Eigen::VectorXd &r, |
98 |
|
|
const Eigen::VectorXd &d, size_t iq) { |
99 |
✓✗✓✗
|
12 |
r.head(iq) = d.head(iq); |
100 |
✓✗✓✗
|
24 |
R.topLeftCorner(iq, iq).triangularView<Eigen::Upper>().solveInPlace( |
101 |
✓✗ |
12 |
r.head(iq)); |
102 |
|
12 |
} |
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 |
|
|
inline double solve_quadprog2(Eigen::LLT<Eigen::MatrixXd, Eigen::Lower> &chol, |
124 |
|
|
double c1, Eigen::VectorXd &g0, |
125 |
|
|
const Eigen::MatrixXd &CE, |
126 |
|
|
const Eigen::VectorXd &ce0, |
127 |
|
|
const Eigen::MatrixXd &CI, |
128 |
|
|
const Eigen::VectorXd &ci0, Eigen::VectorXd &x, |
129 |
|
|
Eigen::VectorXi &A, size_t &q) { |
130 |
|
|
return solve_quadprog(chol, c1, g0, CE, ce0, CI, ci0, x, A, q); |
131 |
|
|
} |
132 |
|
|
|
133 |
|
|
/* solve_quadprog is used for on-demand QP solving */ |
134 |
|
|
double solve_quadprog(Eigen::MatrixXd &G, Eigen::VectorXd &g0, |
135 |
|
|
const Eigen::MatrixXd &CE, const Eigen::VectorXd &ce0, |
136 |
|
|
const Eigen::MatrixXd &CI, const Eigen::VectorXd &ci0, |
137 |
|
|
Eigen::VectorXd &x, Eigen::VectorXi &activeSet, |
138 |
|
|
size_t &activeSetSize); |
139 |
|
|
|
140 |
|
|
double solve_quadprog(Eigen::MatrixXd &G, Eigen::VectorXd &g0, |
141 |
|
|
const Eigen::MatrixXd &CE, const Eigen::VectorXd &ce0, |
142 |
|
|
const Eigen::MatrixXd &CI, const Eigen::VectorXd &ci0, |
143 |
|
|
Eigen::VectorXd &x, Eigen::VectorXd &y, |
144 |
|
|
Eigen::VectorXi &activeSet, size_t &activeSetSize); |
145 |
|
|
// } |
146 |
|
|
|
147 |
|
|
} // namespace solvers |
148 |
|
|
} // namespace eiquadprog |
149 |
|
|
|
150 |
|
|
#endif |