eiquadprog.hpp 6.14 KB
Newer Older
1
//
2
// Copyright (c) 2019,2022 CNRS INRIA
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
//
// This file is part of eiquadprog.
//
// eiquadprog is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
//(at your option) any later version.

// eiquadprog is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Lesser General Public License for more details.

// You should have received a copy of the GNU Lesser General Public License
// along with eiquadprog.  If not, see <https://www.gnu.org/licenses/>.

19
20
21
22
#ifndef _EIGEN_QUADSOLVE_HPP_
#define _EIGEN_QUADSOLVE_HPP_

/*
23
 FILE eiquadprog.hpp
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
 NOTE: this is a modified of uQuadProg++ package, working with Eigen data structures.
 uQuadProg++ is itself a port made by Angelo Furfaro of QuadProg++ originally developed by
 Luca Di Gaspero, working with ublas data structures.
 The quadprog_solve() function implements the algorithm of Goldfarb and Idnani
 for the solution of a (convex) Quadratic Programming problem
 by means of a dual method.
 The problem is in the form:
 min 0.5 * x G x + g0 x
 s.t.
 CE^T x + ce0 = 0
 CI^T x + ci0 >= 0
 The matrix and vectors dimensions are as follows:
 G: n * n
 g0: n
 CE: n * p
 ce0: p
 CI: n * m
 ci0: m
 x: n
 The function will return the cost of the solution written in the x vector or
 std::numeric_limits::infinity() if the problem is infeasible. In the latter case
 the value of the x vector is not correct.
 References: D. Goldfarb, A. Idnani. A numerically stable dual method for solving
 strictly convex quadratic programs. Mathematical Programming 27 (1983) pp. 1-33.
 Notes:
 1. pay attention in setting up the vectors ce0 and ci0.
 If the constraints of your problem are specified in the form
 A^T x = b and C^T x >= d, then you should set ce0 = -b and ci0 = -d.
 2. The matrix G is modified within the function since it is used to compute
 the G = L^T L cholesky factorization for further computations inside the function.
 If you need the original matrix G you should make a copy of it and pass the copy
 to the function.
 The author will be grateful if the researchers using this software will
 acknowledge the contribution of this modified function and of Di Gaspero's
 original version in their research papers.
 LICENSE
 Copyright (2011) Benjamin Stephens
 Copyright (2010) Gael Guennebaud
 Copyright (2008) Angelo Furfaro
 Copyright (2006) Luca Di Gaspero
 This file is a porting of QuadProg++ routine, originally developed
 by Luca Di Gaspero, exploiting uBlas data structures for vectors and
 matrices instead of native C++ array.
 uquadprog is free software; you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation; either version 2 of the License, or
 (at your option) any later version.
 uquadprog is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.
 You should have received a copy of the GNU General Public License
 along with uquadprog; if not, write to the Free Software
 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */

#include <Eigen/Core>
#include <Eigen/Cholesky>

#include <iostream>

85
#include "eiquadprog/eiquadprog-utils.hxx"
86
#include "eiquadprog/deprecated.hpp"
87

Guilhem Saurel's avatar
Guilhem Saurel committed
88
// namespace internal {
89
90
namespace eiquadprog {
namespace solvers {
Guilhem Saurel's avatar
Guilhem Saurel committed
91

92
inline void compute_d(Eigen::VectorXd& d, const Eigen::MatrixXd& J, const Eigen::VectorXd& np) {
93
  d.noalias() = J.adjoint() * np;
94
}
95

96
inline void update_z(Eigen::VectorXd& z, const Eigen::MatrixXd& J, const Eigen::VectorXd& d, size_t iq) {
97
  z.noalias() = J.rightCols(z.size() - iq) * d.tail(d.size() - iq);
Guilhem Saurel's avatar
Guilhem Saurel committed
98
}
99

100
inline void update_r(const Eigen::MatrixXd& R, Eigen::VectorXd& r, const Eigen::VectorXd& d, size_t iq) {
101
  r.head(iq) = d.head(iq); 
102
  R.topLeftCorner(iq, iq).triangularView<Eigen::Upper>().solveInPlace(r.head(iq));
Guilhem Saurel's avatar
Guilhem Saurel committed
103
}
104

105
106
107
bool add_constraint(Eigen::MatrixXd& R, Eigen::MatrixXd& J, Eigen::VectorXd& d, size_t& iq, double& R_norm);
void delete_constraint(Eigen::MatrixXd& R, Eigen::MatrixXd& J, Eigen::VectorXi& A, Eigen::VectorXd& u, size_t p,
                       size_t& iq, size_t l);
108

109
110
111
112
113
114
115
116
117
118
119
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);

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::VectorXd& y, Eigen::VectorXi& A, size_t& q);


EIQUADPROG_DEPRECATED 
120
121
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,
122
123
124
125
                       const Eigen::VectorXd& ci0, Eigen::VectorXd& x, Eigen::VectorXi& A, size_t& q)
{
  return solve_quadprog(chol, c1, g0, CE, ce0, CI, ci0, x, A, q);
}
126

Guilhem Saurel's avatar
Guilhem Saurel committed
127
/* solve_quadprog is used for on-demand QP solving */
128
129
130
double solve_quadprog(Eigen::MatrixXd& G, Eigen::VectorXd& g0, const Eigen::MatrixXd& CE, const Eigen::VectorXd& ce0,
                      const Eigen::MatrixXd& CI, const Eigen::VectorXd& ci0, Eigen::VectorXd& x,
                      Eigen::VectorXi& activeSet, size_t& activeSetSize);
131
132
133
134
135

double solve_quadprog(Eigen::MatrixXd& G, Eigen::VectorXd& g0, const Eigen::MatrixXd& CE, const Eigen::VectorXd& ce0,
                      const Eigen::MatrixXd& CI, const Eigen::VectorXd& ci0, 
                      Eigen::VectorXd& x, Eigen::VectorXd& y,
                      Eigen::VectorXi& activeSet, size_t& activeSetSize);
136
// }
137

138
139
}  // namespace solvers
}  // namespace eiquadprog
Guilhem Saurel's avatar
Guilhem Saurel committed
140

141
#endif