| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // | ||
| 2 | // Copyright (c) 2017 CNRS | ||
| 3 | // | ||
| 4 | // This file is part of tsid | ||
| 5 | // tsid is free software: you can redistribute it | ||
| 6 | // and/or modify it under the terms of the GNU Lesser General Public | ||
| 7 | // License as published by the Free Software Foundation, either version | ||
| 8 | // 3 of the License, or (at your option) any later version. | ||
| 9 | // tsid is distributed in the hope that it will be | ||
| 10 | // useful, but WITHOUT ANY WARRANTY; without even the implied warranty | ||
| 11 | // of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU | ||
| 12 | // General Lesser Public License for more details. You should have | ||
| 13 | // received a copy of the GNU Lesser General Public License along with | ||
| 14 | // tsid If not, see | ||
| 15 | // <http://www.gnu.org/licenses/>. | ||
| 16 | // | ||
| 17 | |||
| 18 | #ifndef EIQUADPROGFAST_HH_ | ||
| 19 | #define EIQUADPROGFAST_HH_ | ||
| 20 | |||
| 21 | #include <Eigen/Dense> | ||
| 22 | #include <Eigen/Sparse> | ||
| 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 | // #define TRACE_SOLVER 1 | ||
| 30 | |||
| 31 | // #define USE_WARM_START | ||
| 32 | // #define PROFILE_EIQUADPROG | ||
| 33 | |||
| 34 | // #define DEBUG_STREAM(msg) std::cout<<msg; | ||
| 35 | #define DEBUG_STREAM(msg) | ||
| 36 | |||
| 37 | #ifdef PROFILE_EIQUADPROG | ||
| 38 | #define START_PROFILER_EIQUADPROG_FAST START_PROFILER | ||
| 39 | #define STOP_PROFILER_EIQUADPROG_FAST STOP_PROFILER | ||
| 40 | #else | ||
| 41 | #define START_PROFILER_EIQUADPROG_FAST | ||
| 42 | #define STOP_PROFILER_EIQUADPROG_FAST | ||
| 43 | #endif | ||
| 44 | |||
| 45 | #define EIQUADPROG_FAST_CHOWLESKY_DECOMPOSITION "EIQUADPROG_FAST Chowlesky dec" | ||
| 46 | #define EIQUADPROG_FAST_CHOWLESKY_INVERSE "EIQUADPROG_FAST Chowlesky inv" | ||
| 47 | #define EIQUADPROG_FAST_ADD_EQ_CONSTR "EIQUADPROG_FAST ADD_EQ_CONSTR" | ||
| 48 | #define EIQUADPROG_FAST_ADD_EQ_CONSTR_1 "EIQUADPROG_FAST ADD_EQ_CONSTR_1" | ||
| 49 | #define EIQUADPROG_FAST_ADD_EQ_CONSTR_2 "EIQUADPROG_FAST ADD_EQ_CONSTR_2" | ||
| 50 | #define EIQUADPROG_FAST_STEP_1 "EIQUADPROG_FAST STEP_1" | ||
| 51 | #define EIQUADPROG_FAST_STEP_1_1 "EIQUADPROG_FAST STEP_1_1" | ||
| 52 | #define EIQUADPROG_FAST_STEP_1_2 "EIQUADPROG_FAST STEP_1_2" | ||
| 53 | #define EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM \ | ||
| 54 | "EIQUADPROG_FAST STEP_1_UNCONSTR_MINIM" | ||
| 55 | #define EIQUADPROG_FAST_STEP_2 "EIQUADPROG_FAST STEP_2" | ||
| 56 | #define EIQUADPROG_FAST_STEP_2A "EIQUADPROG_FAST STEP_2A" | ||
| 57 | #define EIQUADPROG_FAST_STEP_2B "EIQUADPROG_FAST STEP_2B" | ||
| 58 | #define EIQUADPROG_FAST_STEP_2C "EIQUADPROG_FAST STEP_2C" | ||
| 59 | |||
| 60 | #define DEFAULT_MAX_ITER 1000 | ||
| 61 | |||
| 62 | namespace tsid { | ||
| 63 | |||
| 64 | namespace solvers { | ||
| 65 | |||
| 66 | /** | ||
| 67 | * Possible states of the solver. | ||
| 68 | */ | ||
| 69 | enum EiquadprogFast_status { | ||
| 70 | EIQUADPROG_FAST_OPTIMAL = 0, | ||
| 71 | EIQUADPROG_FAST_INFEASIBLE = 1, | ||
| 72 | EIQUADPROG_FAST_UNBOUNDED = 2, | ||
| 73 | EIQUADPROG_FAST_MAX_ITER_REACHED = 3, | ||
| 74 | EIQUADPROG_FAST_REDUNDANT_EQUALITIES = 4 | ||
| 75 | }; | ||
| 76 | |||
| 77 | class EiquadprogFast { | ||
| 78 | public: | ||
| 79 | typedef Eigen::MatrixXd MatrixXd; | ||
| 80 | typedef Eigen::VectorXd VectorXd; | ||
| 81 | typedef Eigen::VectorXi VectorXi; | ||
| 82 | |||
| 83 | typedef Eigen::SparseMatrix<double> SpMat; | ||
| 84 | typedef Eigen::SparseVector<double> SpVec; | ||
| 85 | typedef Eigen::SparseVector<int> SpVeci; | ||
| 86 | |||
| 87 | public: | ||
| 88 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 89 | |||
| 90 | EiquadprogFast(); | ||
| 91 | virtual ~EiquadprogFast(); | ||
| 92 | |||
| 93 | void reset(int dim_qp, int num_eq, int num_ineq); | ||
| 94 | |||
| 95 | int getMaxIter() const { return m_maxIter; } | ||
| 96 | |||
| 97 | bool setMaxIter(int maxIter) { | ||
| 98 | if (maxIter < 0) return false; | ||
| 99 | m_maxIter = maxIter; | ||
| 100 | return true; | ||
| 101 | } | ||
| 102 | |||
| 103 | /** | ||
| 104 | * @return The size of the active set, namely the number of | ||
| 105 | * active constraints (including the equalities). | ||
| 106 | */ | ||
| 107 | int getActiveSetSize() const { return q; } | ||
| 108 | |||
| 109 | /** | ||
| 110 | * @return The number of active-set iteratios. | ||
| 111 | */ | ||
| 112 | int getIteratios() const { return iter; } | ||
| 113 | |||
| 114 | /** | ||
| 115 | * @return The value of the objective function. | ||
| 116 | */ | ||
| 117 | 557 | double getObjValue() const { return f_value; } | |
| 118 | |||
| 119 | /** | ||
| 120 | * @return The Lagrange multipliers | ||
| 121 | */ | ||
| 122 | const VectorXd& getLagrangeMultipliers() const { return u; } | ||
| 123 | /** | ||
| 124 | * Return the active set, namely the indeces of active constraints. | ||
| 125 | * The first nEqCon indexes are for the equalities and are negative. | ||
| 126 | * The last nIneqCon indexes are for the inequalities and start from 0. | ||
| 127 | * Only the first q elements of the return vector are valid, where q | ||
| 128 | * is the size of the active set. | ||
| 129 | * @return The set of indexes of the active constraints. | ||
| 130 | */ | ||
| 131 | const VectorXi& getActiveSet() const { return A; } | ||
| 132 | |||
| 133 | /** | ||
| 134 | * solves the problem | ||
| 135 | * min. x' Hess x + 2 g0' x | ||
| 136 | * s.t. CE x + ce0 = 0 | ||
| 137 | * CI x + ci0 >= 0 | ||
| 138 | */ | ||
| 139 | EiquadprogFast_status solve_quadprog(const MatrixXd& Hess, const VectorXd& g0, | ||
| 140 | const MatrixXd& CE, const VectorXd& ce0, | ||
| 141 | const MatrixXd& CI, const VectorXd& ci0, | ||
| 142 | VectorXd& x); | ||
| 143 | /** | ||
| 144 | * solves the sparse problem | ||
| 145 | * min. x' Hess x + 2 g0' x | ||
| 146 | * s.t. CE x + ce0 = 0 | ||
| 147 | * CI x + ci0 >= 0 | ||
| 148 | */ | ||
| 149 | EiquadprogFast_status solve_quadprog_sparse(const SpMat& Hess, | ||
| 150 | const VectorXd& g0, | ||
| 151 | const MatrixXd& CE, | ||
| 152 | const VectorXd& ce0, | ||
| 153 | const MatrixXd& CI, | ||
| 154 | const VectorXd& ci0, VectorXd& x); | ||
| 155 | |||
| 156 | MatrixXd m_J; // J * J' = Hessian <nVars,nVars>::d | ||
| 157 | bool is_inverse_provided_; | ||
| 158 | |||
| 159 | private: | ||
| 160 | int m_nVars; | ||
| 161 | int m_nEqCon; | ||
| 162 | int m_nIneqCon; | ||
| 163 | |||
| 164 | int m_maxIter; /// max number of active-set iterations | ||
| 165 | double f_value; /// current value of cost function | ||
| 166 | |||
| 167 | Eigen::LLT<MatrixXd, Eigen::Lower> chol_; // <nVars,nVars>::d | ||
| 168 | // Eigen::LLT<MatrixXd,Eigen::Lower> chol_; // <nVars,nVars>::d | ||
| 169 | |||
| 170 | /// from QR of L' N, where L is Cholewsky factor of Hessian, and N is the | ||
| 171 | /// matrix of active constraints | ||
| 172 | MatrixXd R; // <nVars,nVars>::d | ||
| 173 | |||
| 174 | /// CI*x+ci0 | ||
| 175 | VectorXd s; // <nIneqCon>::d | ||
| 176 | |||
| 177 | /// infeasibility multipliers, i.e. negative step direction in dual space | ||
| 178 | VectorXd r; // <nIneqCon+nEqCon>::d | ||
| 179 | |||
| 180 | /// Lagrange multipliers | ||
| 181 | VectorXd u; // <nIneqCon+nEqCon>::d | ||
| 182 | |||
| 183 | /// step direction in primal space | ||
| 184 | VectorXd z; // <nVars>::d | ||
| 185 | |||
| 186 | /// J' np | ||
| 187 | VectorXd d; //<nVars>::d | ||
| 188 | |||
| 189 | /// current constraint normal | ||
| 190 | VectorXd np; //<nVars>::d | ||
| 191 | |||
| 192 | /// active set (indeces of active constraints) | ||
| 193 | /// the first nEqCon indeces are for the equalities and are negative | ||
| 194 | /// the last nIneqCon indeces are for the inequalities are start from 0 | ||
| 195 | VectorXi A; // <nIneqCon+nEqCon> | ||
| 196 | |||
| 197 | /// initialized as K \ A | ||
| 198 | /// iai(i)=-1 iff inequality constraint i is in the active set | ||
| 199 | /// iai(i)=i otherwise | ||
| 200 | VectorXi iai; // <nIneqCon>::i | ||
| 201 | |||
| 202 | /// initialized as [1, ..., 1, .] | ||
| 203 | /// if iaexcl(i)!=1 inequality constraint i cannot be added to the active set | ||
| 204 | /// if adding ineq constraint i fails => iaexcl(i)=0 | ||
| 205 | /// iaexcl(i)=0 iff ineq constraint i is linearly dependent to other active | ||
| 206 | /// constraints iaexcl(i)=1 otherwise | ||
| 207 | VectorXi iaexcl; //<nIneqCon>::i | ||
| 208 | |||
| 209 | VectorXd x_old; // old value of x <nVars>::d | ||
| 210 | VectorXd u_old; // old value of u <nIneqCon+nEqCon>::d | ||
| 211 | VectorXi A_old; // old value of A <nIneqCon+nEqCon>::i | ||
| 212 | |||
| 213 | #ifdef OPTIMIZE_ADD_CONSTRAINT | ||
| 214 | VectorXd T1; /// tmp variable used in add_constraint | ||
| 215 | #endif | ||
| 216 | |||
| 217 | /// size of the active set A (containing the indices of the active | ||
| 218 | /// constraints) | ||
| 219 | int q; | ||
| 220 | |||
| 221 | /// number of active-set iterations | ||
| 222 | int iter; | ||
| 223 | |||
| 224 | 11182 | inline void compute_d(VectorXd& d, const MatrixXd& J, const VectorXd& np) { | |
| 225 | #ifdef OPTIMIZE_COMPUTE_D | ||
| 226 |
4/8✓ Branch 1 taken 11182 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11182 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 11182 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 11182 times.
✗ Branch 11 not taken.
|
11182 | d.noalias() = J.adjoint() * np; |
| 227 | #else | ||
| 228 | d = J.adjoint() * np; | ||
| 229 | #endif | ||
| 230 | 11182 | } | |
| 231 | |||
| 232 | 11181 | inline void update_z(VectorXd& z, const MatrixXd& J, const VectorXd& d, | |
| 233 | int iq) { | ||
| 234 | #ifdef OPTIMIZE_UPDATE_Z | ||
| 235 |
5/10✓ Branch 2 taken 11181 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 11181 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 11181 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 11181 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 11181 times.
✗ Branch 16 not taken.
|
11181 | z.noalias() = J.rightCols(z.size() - iq) * d.tail(z.size() - iq); |
| 236 | #else | ||
| 237 | z = J.rightCols(J.cols() - iq) * d.tail(J.cols() - iq); | ||
| 238 | #endif | ||
| 239 | 11181 | } | |
| 240 | |||
| 241 | 11182 | inline void update_r(const MatrixXd& R, VectorXd& r, const VectorXd& d, | |
| 242 | int iq) { | ||
| 243 |
3/6✓ Branch 1 taken 11182 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11182 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 11182 times.
✗ Branch 8 not taken.
|
11182 | r.head(iq) = d.head(iq); |
| 244 |
3/6✓ Branch 1 taken 11182 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11182 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 11182 times.
✗ Branch 8 not taken.
|
22364 | R.topLeftCorner(iq, iq).triangularView<Eigen::Upper>().solveInPlace( |
| 245 |
1/2✓ Branch 1 taken 11182 times.
✗ Branch 2 not taken.
|
11182 | r.head(iq)); |
| 246 | 11182 | } | |
| 247 | |||
| 248 | inline bool add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, int& iq, | ||
| 249 | double& R_norm); | ||
| 250 | |||
| 251 | inline void delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, | ||
| 252 | VectorXd& u, int nEqCon, int& iq, int l); | ||
| 253 | }; | ||
| 254 | |||
| 255 | } /* namespace solvers */ | ||
| 256 | } /* namespace tsid */ | ||
| 257 | |||
| 258 | #endif /* EIQUADPROGFAST_HH_ */ | ||
| 259 |