GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: include/eiquadprog/eiquadprog.hpp Lines: 9 9 100.0 %
Date: 2021-03-10 23:02:29 Branches: 11 22 50.0 %

Line Branch Exec Source
1
//
2
// Copyright (c) 2019 CNRS
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 structures.
25
 uQuadProg++ is itself a port made by Angelo Furfaro of QuadProg++ originally developed by
26
 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 case
45
 the value of the x vector is not correct.
46
 References: D. Goldfarb, A. Idnani. A numerically stable dual method for solving
47
 strictly convex quadratic programs. Mathematical Programming 27 (1983) pp. 1-33.
48
 Notes:
49
 1. pay attention in setting up the vectors ce0 and ci0.
50
 If the constraints of your problem are specified in the form
51
 A^T x = b and C^T x >= d, then you should set ce0 = -b and ci0 = -d.
52
 2. The matrix G is modified within the function since it is used to compute
53
 the G = L^T L cholesky factorization for further computations inside the function.
54
 If you need the original matrix G you should make a copy of it and pass the copy
55
 to the function.
56
 The author will be grateful if the researchers using this software will
57
 acknowledge the contribution of this modified function and of Di Gaspero's
58
 original version in their research papers.
59
 LICENSE
60
 Copyright (2011) Benjamin Stephens
61
 Copyright (2010) Gael Guennebaud
62
 Copyright (2008) Angelo Furfaro
63
 Copyright (2006) Luca Di Gaspero
64
 This file is a porting of QuadProg++ routine, originally developed
65
 by Luca Di Gaspero, exploiting uBlas data structures for vectors and
66
 matrices instead of native C++ array.
67
 uquadprog is free software; you can redistribute it and/or modify
68
 it under the terms of the GNU General Public License as published by
69
 the Free Software Foundation; either version 2 of the License, or
70
 (at your option) any later version.
71
 uquadprog is distributed in the hope that it will be useful,
72
 but WITHOUT ANY WARRANTY; without even the implied warranty of
73
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
74
 GNU General Public License for more details.
75
 You should have received a copy of the GNU General Public License
76
 along with uquadprog; if not, write to the Free Software
77
 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
78
 */
79
80
#include <Eigen/Core>
81
#include <Eigen/Cholesky>
82
83
#include <iostream>
84
85
#include "eiquadprog/eiquadprog-utils.hxx"
86
87
// namespace internal {
88
namespace eiquadprog {
89
namespace solvers {
90
91
12
inline void compute_d(Eigen::VectorXd& d, const Eigen::MatrixXd& J, const Eigen::VectorXd& np) {
92

12
  d = J.adjoint() * np;
93
12
}
94
95
12
inline void update_z(Eigen::VectorXd& z, const Eigen::MatrixXd& J, const Eigen::VectorXd& d, size_t iq) {
96


12
  z = J.rightCols(z.size() - iq) * d.tail(d.size() - iq);
97
12
}
98
99
12
inline void update_r(const Eigen::MatrixXd& R, Eigen::VectorXd& r, const Eigen::VectorXd& d, size_t iq) {
100


12
  r.head(iq) = R.topLeftCorner(iq, iq).triangularView<Eigen::Upper>().solve(d.head(iq));
101
12
}
102
103
bool add_constraint(Eigen::MatrixXd& R, Eigen::MatrixXd& J, Eigen::VectorXd& d, size_t& iq, double& R_norm);
104
void delete_constraint(Eigen::MatrixXd& R, Eigen::MatrixXd& J, Eigen::VectorXi& A, Eigen::VectorXd& u, size_t p,
105
                       size_t& iq, size_t l);
106
107
/* solve_quadprog2 is used when the Cholesky decomposition of the G
108
   matrix is precomputed */
109
double solve_quadprog2(Eigen::LLT<Eigen::MatrixXd, Eigen::Lower>& chol, double c1, Eigen::VectorXd& g0,
110
                       const Eigen::MatrixXd& CE, const Eigen::VectorXd& ce0, const Eigen::MatrixXd& CI,
111
                       const Eigen::VectorXd& ci0, Eigen::VectorXd& x, Eigen::VectorXi& A, size_t& q);
112
113
/* solve_quadprog is used for on-demand QP solving */
114
double solve_quadprog(Eigen::MatrixXd& G, Eigen::VectorXd& g0, const Eigen::MatrixXd& CE, const Eigen::VectorXd& ce0,
115
                      const Eigen::MatrixXd& CI, const Eigen::VectorXd& ci0, Eigen::VectorXd& x,
116
                      Eigen::VectorXi& activeSet, size_t& activeSetSize);
117
// }
118
119
}  // namespace solvers
120
}  // namespace eiquadprog
121
122
#endif