Loading...
Searching...
No Matches
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 {
84namespace eiquadprog {
85namespace solvers {
86
87inline void compute_d(Eigen::VectorXd &d, const Eigen::MatrixXd &J,
88 const Eigen::VectorXd &np) {
89 d.noalias() = J.adjoint() * np;
90}
91
92inline 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
97inline 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
104bool add_constraint(Eigen::MatrixXd &R, Eigen::MatrixXd &J, Eigen::VectorXd &d,
105 size_t &iq, double &R_norm);
106void 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
110double 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
116double 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
122EIQUADPROG_DEPRECATED
123inline 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 */
134double 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
140double 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
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
void update_z(Eigen::VectorXd &z, const Eigen::MatrixXd &J, const Eigen::VectorXd &d, size_t iq)
Definition eiquadprog.hpp:92
bool add_constraint(Eigen::MatrixXd &R, Eigen::MatrixXd &J, Eigen::VectorXd &d, size_t &iq, double &R_norm)
void compute_d(Eigen::VectorXd &d, const Eigen::MatrixXd &J, const Eigen::VectorXd &np)
Definition eiquadprog.hpp:87
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)
void delete_constraint(Eigen::MatrixXd &R, Eigen::MatrixXd &J, Eigen::VectorXi &A, Eigen::VectorXd &u, size_t p, size_t &iq, size_t l)
void update_r(const Eigen::MatrixXd &R, Eigen::VectorXd &r, const Eigen::VectorXd &d, size_t iq)
Definition eiquadprog.hpp:97
Definition eiquadprog-fast.hpp:63