eiquadprog: include/eiquadprog/eiquadprog-fast.hpp Source File

eiquadprog 1.3.2 C++ reimplementation of eiquadprog

Loading...
Searching...
No Matches
eiquadprog-fast.hpp
Go to the documentation of this file.
1//
2// Copyright (c) 2017 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 EIQUADPROGFAST_HPP_
20#define EIQUADPROGFAST_HPP_
21
22#include <Eigen/Dense>
23
24#define OPTIMIZE_STEP_1_2 // compute s(x) = ci^T * x + ci0
25#define OPTIMIZE_COMPUTE_D
26#define OPTIMIZE_UPDATE_Z
27#define OPTIMIZE_HESSIAN_INVERSE
28#define OPTIMIZE_UNCONSTR_MINIM
29
30// #define USE_WARM_START
31// #define PROFILE_EIQUADPROG
32
33// #define DEBUG_STREAM(msg) std::cout<<msg;
34#define DEBUG_STREAM(msg)
35
36#ifdef PROFILE_EIQUADPROG
37#define START_PROFILER_EIQUADPROG_FAST(x) START_PROFILER(x)
38#define STOP_PROFILER_EIQUADPROG_FAST(x) STOP_PROFILER(x)
39#else
40#define START_PROFILER_EIQUADPROG_FAST(x)
41#define STOP_PROFILER_EIQUADPROG_FAST(x)
42#endif
43
44#define EIQUADPROG_FAST_CHOLESKY_DECOMPOSITION "EIQUADPROG_FAST Cholesky dec"
45#define EIQUADPROG_FAST_CHOLESKY_INVERSE "EIQUADPROG_FAST Cholesky inv"
46#define EIQUADPROG_FAST_ADD_EQ_CONSTR "EIQUADPROG_FAST ADD_EQ_CONSTR"
47#define EIQUADPROG_FAST_ADD_EQ_CONSTR_1 "EIQUADPROG_FAST ADD_EQ_CONSTR_1"
48#define EIQUADPROG_FAST_ADD_EQ_CONSTR_2 "EIQUADPROG_FAST ADD_EQ_CONSTR_2"
49#define EIQUADPROG_FAST_STEP_1 "EIQUADPROG_FAST STEP_1"
50#define EIQUADPROG_FAST_STEP_1_1 "EIQUADPROG_FAST STEP_1_1"
51#define EIQUADPROG_FAST_STEP_1_2 "EIQUADPROG_FAST STEP_1_2"
52#define EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM \
53 "EIQUADPROG_FAST STEP_1_UNCONSTR_MINIM"
54#define EIQUADPROG_FAST_STEP_2 "EIQUADPROG_FAST STEP_2"
55#define EIQUADPROG_FAST_STEP_2A "EIQUADPROG_FAST STEP_2A"
56#define EIQUADPROG_FAST_STEP_2B "EIQUADPROG_FAST STEP_2B"
57#define EIQUADPROG_FAST_STEP_2C "EIQUADPROG_FAST STEP_2C"
58
59#define DEFAULT_MAX_ITER 1000
60
61#include "eiquadprog/config.hpp"
62#include "eiquadprog/eiquadprog-utils.hxx"
63
64 namespace eiquadprog {
65
66 namespace solvers {
67
71 enum EiquadprogFast_status {
72 EIQUADPROG_FAST_OPTIMAL = 0,
73 EIQUADPROG_FAST_INFEASIBLE = 1,
74 EIQUADPROG_FAST_UNBOUNDED = 2,
75 EIQUADPROG_FAST_MAX_ITER_REACHED = 3,
76 EIQUADPROG_FAST_REDUNDANT_EQUALITIES = 4
77};
78
79 class EIQUADPROG_DLLAPI EiquadprogFast {
80 typedef Eigen::MatrixXd MatrixXd;
81 typedef Eigen::VectorXd VectorXd;
82 typedef Eigen::VectorXi VectorXi;
83
84 public:
85 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
86
87 EiquadprogFast();
88 virtual ~EiquadprogFast();
89
90 void reset(size_t dim_qp, size_t num_eq, size_t num_ineq);
91
92 int getMaxIter() const { return m_maxIter; }
93
94 bool setMaxIter(int maxIter) {
95 if (maxIter < 0) return false;
96 m_maxIter = maxIter;
97 return true;
98 }
99
104 size_t getActiveSetSize() const { return q; }
105
109 int getIteratios() const { return iter; }
110
114 double getObjValue() const { return f_value; }
115
119 const VectorXd& getLagrangeMultipliers() const { return u; }
120
129 const VectorXi& getActiveSet() const { return A; }
130
137 EiquadprogFast_status solve_quadprog(const MatrixXd& Hess, const VectorXd& g0,
138 const MatrixXd& CE, const VectorXd& ce0,
139 const MatrixXd& CI, const VectorXd& ci0,
140 VectorXd& x);
141
142 MatrixXd m_J; // J * J' = Hessian <nVars,nVars>::d
143 bool is_inverse_provided_;
144
145 private:
146 size_t m_nVars;
147 size_t m_nEqCon;
148 size_t m_nIneqCon;
149
150 int m_maxIter;
151 double f_value;
152
153 Eigen::LLT<MatrixXd, Eigen::Lower> chol_; // <nVars,nVars>::d
154
157 MatrixXd R; // <nVars,nVars>::d
158
160 VectorXd s; // <nIneqCon>::d
161
163 VectorXd r; // <nIneqCon+nEqCon>::d
164
166 VectorXd u; // <nIneqCon+nEqCon>::d
167
169 VectorXd z; // <nVars>::d
170
172 VectorXd d; //<nVars>::d
173
175 VectorXd np; //<nVars>::d
176
180 VectorXi A; // <nIneqCon+nEqCon>
181
185 VectorXi iai; // <nIneqCon>::i
186
192 VectorXi iaexcl; //<nIneqCon>::i
193
194 VectorXd x_old; // old value of x <nVars>::d
195 VectorXd u_old; // old value of u <nIneqCon+nEqCon>::d
196 VectorXi A_old; // old value of A <nIneqCon+nEqCon>::i
197
198#ifdef OPTIMIZE_ADD_CONSTRAINT
199 VectorXd T1;
200#endif
201
204 size_t q;
205
207 int iter;
208
209 inline void compute_d(VectorXd& d, const MatrixXd& J, const VectorXd& np) {
210#ifdef OPTIMIZE_COMPUTE_D
211 d.noalias() = J.adjoint() * np;
212#else
213 d = J.adjoint() * np;
214#endif
215 }
216
217 inline void update_z(VectorXd& z, const MatrixXd& J, const VectorXd& d,
218 size_t iq) {
219#ifdef OPTIMIZE_UPDATE_Z
220 z.noalias() = J.rightCols(z.size() - iq) * d.tail(z.size() - iq);
221#else
222 z = J.rightCols(J.cols() - iq) * d.tail(J.cols() - iq);
223#endif
224 }
225
226 inline void update_r(const MatrixXd& R, VectorXd& r, const VectorXd& d,
227 size_t iq) {
228 r.head(iq) = d.head(iq);
229 R.topLeftCorner(iq, iq).triangularView<Eigen::Upper>().solveInPlace(
230 r.head(iq));
231 }
232
233 inline bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, size_t& iq,
234 double& R_norm);
235
236 inline void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A,
237 VectorXd& u, size_t nEqCon, size_t& iq,
238 size_t l);
239};
240
241} /* namespace solvers */
242} /* namespace eiquadprog */
243
244#endif /* EIQUADPROGFAST_HPP_ */
eiquadprog::solvers::EiquadprogFast
Definition eiquadprog-fast.hpp:79
eiquadprog::solvers::EiquadprogFast::getObjValue
double getObjValue() const
Definition eiquadprog-fast.hpp:114
eiquadprog::solvers::EiquadprogFast::getIteratios
int getIteratios() const
Definition eiquadprog-fast.hpp:109
eiquadprog::solvers::EiquadprogFast::getActiveSetSize
size_t getActiveSetSize() const
Definition eiquadprog-fast.hpp:104
eiquadprog::solvers::EiquadprogFast::getMaxIter
int getMaxIter() const
Definition eiquadprog-fast.hpp:92
eiquadprog::solvers::EiquadprogFast::m_J
MatrixXd m_J
Definition eiquadprog-fast.hpp:142
eiquadprog::solvers::EiquadprogFast::setMaxIter
bool setMaxIter(int maxIter)
Definition eiquadprog-fast.hpp:94
eiquadprog::solvers::EiquadprogFast::getActiveSet
const VectorXi & getActiveSet() const
Definition eiquadprog-fast.hpp:129
eiquadprog::solvers::EiquadprogFast::is_inverse_provided_
bool is_inverse_provided_
Definition eiquadprog-fast.hpp:143
eiquadprog::solvers::EiquadprogFast::getLagrangeMultipliers
const VectorXd & getLagrangeMultipliers() const
Definition eiquadprog-fast.hpp:119
config.hpp
EIQUADPROG_DLLAPI
#define EIQUADPROG_DLLAPI
Definition config.hpp:88
eiquadprog-utils.hxx
eiquadprog::solvers::update_z
void update_z(Eigen::VectorXd &z, const Eigen::MatrixXd &J, const Eigen::VectorXd &d, size_t iq)
Definition eiquadprog.hpp:93
eiquadprog::solvers::solve_quadprog
EIQUADPROG_DLLAPI 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::solvers::EiquadprogFast_status
EiquadprogFast_status
Definition eiquadprog-fast.hpp:71
eiquadprog::solvers::EIQUADPROG_FAST_MAX_ITER_REACHED
@ EIQUADPROG_FAST_MAX_ITER_REACHED
Definition eiquadprog-fast.hpp:75
eiquadprog::solvers::EIQUADPROG_FAST_REDUNDANT_EQUALITIES
@ EIQUADPROG_FAST_REDUNDANT_EQUALITIES
Definition eiquadprog-fast.hpp:76
eiquadprog::solvers::EIQUADPROG_FAST_OPTIMAL
@ EIQUADPROG_FAST_OPTIMAL
Definition eiquadprog-fast.hpp:72
eiquadprog::solvers::EIQUADPROG_FAST_INFEASIBLE
@ EIQUADPROG_FAST_INFEASIBLE
Definition eiquadprog-fast.hpp:73
eiquadprog::solvers::EIQUADPROG_FAST_UNBOUNDED
@ EIQUADPROG_FAST_UNBOUNDED
Definition eiquadprog-fast.hpp:74
eiquadprog::solvers::compute_d
void compute_d(Eigen::VectorXd &d, const Eigen::MatrixXd &J, const Eigen::VectorXd &np)
Definition eiquadprog.hpp:88
eiquadprog
Definition eiquadprog-fast.hpp:64