| Directory: | ./ |
|---|---|
| File: | include/eiquadprog/eiquadprog-fast.hpp |
| Date: | 2024-12-04 10:05:36 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 12 | 12 | 100.0% |
| Branches: | 12 | 24 | 50.0% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 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/eiquadprog-utils.hxx" | ||
| 62 | |||
| 63 | namespace eiquadprog { | ||
| 64 | |||
| 65 | namespace solvers { | ||
| 66 | |||
| 67 | /** | ||
| 68 | * Possible states of the solver. | ||
| 69 | */ | ||
| 70 | enum EiquadprogFast_status { | ||
| 71 | EIQUADPROG_FAST_OPTIMAL = 0, | ||
| 72 | EIQUADPROG_FAST_INFEASIBLE = 1, | ||
| 73 | EIQUADPROG_FAST_UNBOUNDED = 2, | ||
| 74 | EIQUADPROG_FAST_MAX_ITER_REACHED = 3, | ||
| 75 | EIQUADPROG_FAST_REDUNDANT_EQUALITIES = 4 | ||
| 76 | }; | ||
| 77 | |||
| 78 | class EiquadprogFast { | ||
| 79 | typedef Eigen::MatrixXd MatrixXd; | ||
| 80 | typedef Eigen::VectorXd VectorXd; | ||
| 81 | typedef Eigen::VectorXi VectorXi; | ||
| 82 | |||
| 83 | public: | ||
| 84 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 85 | |||
| 86 | EiquadprogFast(); | ||
| 87 | virtual ~EiquadprogFast(); | ||
| 88 | |||
| 89 | void reset(size_t dim_qp, size_t num_eq, size_t num_ineq); | ||
| 90 | |||
| 91 | int getMaxIter() const { return m_maxIter; } | ||
| 92 | |||
| 93 | bool setMaxIter(int maxIter) { | ||
| 94 | if (maxIter < 0) return false; | ||
| 95 | m_maxIter = maxIter; | ||
| 96 | return true; | ||
| 97 | } | ||
| 98 | |||
| 99 | /** | ||
| 100 | * @return The size of the active set, namely the number of | ||
| 101 | * active constraints (including the equalities). | ||
| 102 | */ | ||
| 103 | size_t getActiveSetSize() const { return q; } | ||
| 104 | |||
| 105 | /** | ||
| 106 | * @return The number of active-set iteratios. | ||
| 107 | */ | ||
| 108 | int getIteratios() const { return iter; } | ||
| 109 | |||
| 110 | /** | ||
| 111 | * @return The value of the objective function. | ||
| 112 | */ | ||
| 113 | 7 | double getObjValue() const { return f_value; } | |
| 114 | |||
| 115 | /** | ||
| 116 | * @return The Lagrange multipliers | ||
| 117 | */ | ||
| 118 | const VectorXd& getLagrangeMultipliers() const { return u; } | ||
| 119 | |||
| 120 | /** | ||
| 121 | * Return the active set, namely the indeces of active constraints. | ||
| 122 | * The first nEqCon indexes are for the equalities and are negative. | ||
| 123 | * The last nIneqCon indexes are for the inequalities and start from 0. | ||
| 124 | * Only the first q elements of the return vector are valid, where q | ||
| 125 | * is the size of the active set. | ||
| 126 | * @return The set of indexes of the active constraints. | ||
| 127 | */ | ||
| 128 | const VectorXi& getActiveSet() const { return A; } | ||
| 129 | |||
| 130 | /** | ||
| 131 | * solves the problem | ||
| 132 | * min. x' Hess x + 2 g0' x | ||
| 133 | * s.t. CE x + ce0 = 0 | ||
| 134 | * CI x + ci0 >= 0 | ||
| 135 | */ | ||
| 136 | EiquadprogFast_status solve_quadprog(const MatrixXd& Hess, const VectorXd& g0, | ||
| 137 | const MatrixXd& CE, const VectorXd& ce0, | ||
| 138 | const MatrixXd& CI, const VectorXd& ci0, | ||
| 139 | VectorXd& x); | ||
| 140 | |||
| 141 | MatrixXd m_J; // J * J' = Hessian <nVars,nVars>::d | ||
| 142 | bool is_inverse_provided_; | ||
| 143 | |||
| 144 | private: | ||
| 145 | size_t m_nVars; | ||
| 146 | size_t m_nEqCon; | ||
| 147 | size_t m_nIneqCon; | ||
| 148 | |||
| 149 | int m_maxIter; /// max number of active-set iterations | ||
| 150 | double f_value; /// current value of cost function | ||
| 151 | |||
| 152 | Eigen::LLT<MatrixXd, Eigen::Lower> chol_; // <nVars,nVars>::d | ||
| 153 | |||
| 154 | /// from QR of L' N, where L is Cholewsky factor of Hessian, and N is the | ||
| 155 | /// matrix of active constraints | ||
| 156 | MatrixXd R; // <nVars,nVars>::d | ||
| 157 | |||
| 158 | /// CI*x+ci0 | ||
| 159 | VectorXd s; // <nIneqCon>::d | ||
| 160 | |||
| 161 | /// infeasibility multipliers, i.e. negative step direction in dual space | ||
| 162 | VectorXd r; // <nIneqCon+nEqCon>::d | ||
| 163 | |||
| 164 | /// Lagrange multipliers | ||
| 165 | VectorXd u; // <nIneqCon+nEqCon>::d | ||
| 166 | |||
| 167 | /// step direction in primal space | ||
| 168 | VectorXd z; // <nVars>::d | ||
| 169 | |||
| 170 | /// J' np | ||
| 171 | VectorXd d; //<nVars>::d | ||
| 172 | |||
| 173 | /// current constraint normal | ||
| 174 | VectorXd np; //<nVars>::d | ||
| 175 | |||
| 176 | /// active set (indeces of active constraints) | ||
| 177 | /// the first nEqCon indeces are for the equalities and are negative | ||
| 178 | /// the last nIneqCon indeces are for the inequalities are start from 0 | ||
| 179 | VectorXi A; // <nIneqCon+nEqCon> | ||
| 180 | |||
| 181 | /// initialized as K \ A | ||
| 182 | /// iai(i)=-1 iff inequality constraint i is in the active set | ||
| 183 | /// iai(i)=i otherwise | ||
| 184 | VectorXi iai; // <nIneqCon>::i | ||
| 185 | |||
| 186 | /// initialized as [1, ..., 1, .] | ||
| 187 | /// if iaexcl(i)!=1 inequality constraint i cannot be added to the active set | ||
| 188 | /// if adding ineq constraint i fails => iaexcl(i)=0 | ||
| 189 | /// iaexcl(i)=0 iff ineq constraint i is linearly dependent to other active | ||
| 190 | /// constraints iaexcl(i)=1 otherwise | ||
| 191 | VectorXi iaexcl; //<nIneqCon>::i | ||
| 192 | |||
| 193 | VectorXd x_old; // old value of x <nVars>::d | ||
| 194 | VectorXd u_old; // old value of u <nIneqCon+nEqCon>::d | ||
| 195 | VectorXi A_old; // old value of A <nIneqCon+nEqCon>::i | ||
| 196 | |||
| 197 | #ifdef OPTIMIZE_ADD_CONSTRAINT | ||
| 198 | VectorXd T1; /// tmp variable used in add_constraint | ||
| 199 | #endif | ||
| 200 | |||
| 201 | /// size of the active set A (containing the indices of the active | ||
| 202 | /// constraints) | ||
| 203 | size_t q; | ||
| 204 | |||
| 205 | /// number of active-set iterations | ||
| 206 | int iter; | ||
| 207 | |||
| 208 | 12 | inline void compute_d(VectorXd& d, const MatrixXd& J, const VectorXd& np) { | |
| 209 | #ifdef OPTIMIZE_COMPUTE_D | ||
| 210 |
3/6✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 12 times.
✗ Branch 9 not taken.
|
12 | d.noalias() = J.adjoint() * np; |
| 211 | #else | ||
| 212 | d = J.adjoint() * np; | ||
| 213 | #endif | ||
| 214 | 12 | } | |
| 215 | |||
| 216 | 11 | inline void update_z(VectorXd& z, const MatrixXd& J, const VectorXd& d, | |
| 217 | size_t iq) { | ||
| 218 | #ifdef OPTIMIZE_UPDATE_Z | ||
| 219 |
4/8✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 11 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 11 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 11 times.
✗ Branch 14 not taken.
|
11 | z.noalias() = J.rightCols(z.size() - iq) * d.tail(z.size() - iq); |
| 220 | #else | ||
| 221 | z = J.rightCols(J.cols() - iq) * d.tail(J.cols() - iq); | ||
| 222 | #endif | ||
| 223 | 11 | } | |
| 224 | |||
| 225 | 12 | inline void update_r(const MatrixXd& R, VectorXd& r, const VectorXd& d, | |
| 226 | size_t iq) { | ||
| 227 |
2/4✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
|
12 | r.head(iq) = d.head(iq); |
| 228 |
2/4✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12 times.
✗ Branch 6 not taken.
|
24 | R.topLeftCorner(iq, iq).triangularView<Eigen::Upper>().solveInPlace( |
| 229 |
1/2✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
|
12 | r.head(iq)); |
| 230 | 12 | } | |
| 231 | |||
| 232 | inline bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, size_t& iq, | ||
| 233 | double& R_norm); | ||
| 234 | |||
| 235 | inline void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, | ||
| 236 | VectorXd& u, size_t nEqCon, size_t& iq, | ||
| 237 | size_t l); | ||
| 238 | }; | ||
| 239 | |||
| 240 | } /* namespace solvers */ | ||
| 241 | } /* namespace eiquadprog */ | ||
| 242 | |||
| 243 | #endif /* EIQUADPROGFAST_HPP_ */ | ||
| 244 |