| Directory: | ./ |
|---|---|
| File: | src/eiquadprog-fast.cpp |
| Date: | 2025-04-18 04:07:36 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 218 | 362 | 60.2% |
| Branches: | 195 | 660 | 29.5% |
| 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 | #include "hpp/bezier-com-traj/solver/eiquadprog-fast.hpp" | ||
| 19 | |||
| 20 | #include <iostream> | ||
| 21 | namespace tsid { | ||
| 22 | namespace solvers { | ||
| 23 | |||
| 24 | /// Compute sqrt(a^2 + b^2) | ||
| 25 | template <typename Scalar> | ||
| 26 | 1340130 | inline Scalar distance(Scalar a, Scalar b) { | |
| 27 | Scalar a1, b1, t; | ||
| 28 | 1340130 | a1 = std::abs(a); | |
| 29 | 1340130 | b1 = std::abs(b); | |
| 30 |
2/2✓ Branch 0 taken 11302 times.
✓ Branch 1 taken 1328828 times.
|
1340130 | if (a1 > b1) { |
| 31 | 11302 | t = (b1 / a1); | |
| 32 | 11302 | return a1 * std::sqrt(1.0 + t * t); | |
| 33 |
2/2✓ Branch 0 taken 661135 times.
✓ Branch 1 taken 667693 times.
|
1328828 | } else if (b1 > a1) { |
| 34 | 661135 | t = (a1 / b1); | |
| 35 | 661135 | return b1 * std::sqrt(1.0 + t * t); | |
| 36 | } | ||
| 37 | 667693 | return a1 * std::sqrt(2.0); | |
| 38 | } | ||
| 39 | |||
| 40 |
14/28✓ Branch 2 taken 2571 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2571 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2571 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2571 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 2571 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 2571 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 2571 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 2571 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 2571 times.
✗ Branch 27 not taken.
✓ Branch 29 taken 2571 times.
✗ Branch 30 not taken.
✓ Branch 32 taken 2571 times.
✗ Branch 33 not taken.
✓ Branch 35 taken 2571 times.
✗ Branch 36 not taken.
✓ Branch 38 taken 2571 times.
✗ Branch 39 not taken.
✓ Branch 41 taken 2571 times.
✗ Branch 42 not taken.
|
2571 | EiquadprogFast::EiquadprogFast() { |
| 41 | 2571 | m_maxIter = DEFAULT_MAX_ITER; | |
| 42 | 2571 | q = 0; // size of the active set A (containing the indices of the active | |
| 43 | // constraints) | ||
| 44 | 2571 | is_inverse_provided_ = false; | |
| 45 | 2571 | m_nVars = 0; | |
| 46 | 2571 | m_nEqCon = 0; | |
| 47 | 2571 | m_nIneqCon = 0; | |
| 48 | 2571 | } | |
| 49 | |||
| 50 | 5142 | EiquadprogFast::~EiquadprogFast() {} | |
| 51 | |||
| 52 | 2571 | void EiquadprogFast::reset(int nVars, int nEqCon, int nIneqCon) { | |
| 53 | 2571 | m_nVars = nVars; | |
| 54 | 2571 | m_nEqCon = nEqCon; | |
| 55 | 2571 | m_nIneqCon = nIneqCon; | |
| 56 | 2571 | m_J.setZero(nVars, nVars); | |
| 57 | 2571 | chol_.compute(m_J); | |
| 58 | 2571 | R.resize(nVars, nVars); | |
| 59 | 2571 | s.resize(nIneqCon); | |
| 60 | 2571 | r.resize(nIneqCon + nEqCon); | |
| 61 | 2571 | u.resize(nIneqCon + nEqCon); | |
| 62 | 2571 | z.resize(nVars); | |
| 63 | 2571 | d.resize(nVars); | |
| 64 | 2571 | np.resize(nVars); | |
| 65 | 2571 | A.resize(nIneqCon + nEqCon); | |
| 66 | 2571 | iai.resize(nIneqCon); | |
| 67 | 2571 | iaexcl.resize(nIneqCon); | |
| 68 | 2571 | x_old.resize(nVars); | |
| 69 | 2571 | u_old.resize(nIneqCon + nEqCon); | |
| 70 | 2571 | A_old.resize(nIneqCon + nEqCon); | |
| 71 | |||
| 72 | #ifdef OPTIMIZE_ADD_CONSTRAINT | ||
| 73 | T1.resize(nVars); | ||
| 74 | #endif | ||
| 75 | 2571 | } | |
| 76 | |||
| 77 | 6731 | bool EiquadprogFast::add_constraint(MatrixXd& R, MatrixXd& J, VectorXd& d, | |
| 78 | int& iq, double& R_norm) { | ||
| 79 | 6731 | long int nVars = J.rows(); | |
| 80 | #ifdef TRACE_SOLVER | ||
| 81 | std::cout << "Add constraint " << iq << '/'; | ||
| 82 | #endif | ||
| 83 | long int j, k; | ||
| 84 | double cc, ss, h, t1, t2, xny; | ||
| 85 | |||
| 86 | #ifdef OPTIMIZE_ADD_CONSTRAINT | ||
| 87 | Eigen::Vector2d cc_ss; | ||
| 88 | #endif | ||
| 89 | |||
| 90 | /* we have to find the Givens rotation which will reduce the element | ||
| 91 | d(j) to zero. | ||
| 92 | if it is already zero we don't have to do anything, except of | ||
| 93 | decreasing j */ | ||
| 94 |
2/2✓ Branch 1 taken 1337331 times.
✓ Branch 2 taken 6731 times.
|
1344062 | for (j = d.size() - 1; j >= iq + 1; j--) { |
| 95 | /* The Givens rotation is done with the matrix (cc cs, cs -cc). | ||
| 96 | If cc is one, then element (j) of d is zero compared with | ||
| 97 | element (j - 1). Hence we don't have to do anything. If cc is zero, then | ||
| 98 | we just have to switch column (j) and column (j - 1) of J. Since we only | ||
| 99 | switch columns in J, we have to be careful how we update d depending on | ||
| 100 | the sign of gs. Otherwise we have to apply the Givens rotation to these | ||
| 101 | columns. The i - 1 element of d has to be updated to h. */ | ||
| 102 | 1337331 | cc = d(j - 1); | |
| 103 | 1337331 | ss = d(j); | |
| 104 | 1337331 | h = distance(cc, ss); | |
| 105 |
2/2✓ Branch 0 taken 667375 times.
✓ Branch 1 taken 669956 times.
|
1337331 | if (h == 0.0) continue; |
| 106 | 669956 | d(j) = 0.0; | |
| 107 | 669956 | ss = ss / h; | |
| 108 | 669956 | cc = cc / h; | |
| 109 |
2/2✓ Branch 0 taken 41354 times.
✓ Branch 1 taken 628602 times.
|
669956 | if (cc < 0.0) { |
| 110 | 41354 | cc = -cc; | |
| 111 | 41354 | ss = -ss; | |
| 112 | 41354 | d(j - 1) = -h; | |
| 113 | } else | ||
| 114 | 628602 | d(j - 1) = h; | |
| 115 | 669956 | xny = ss / (1.0 + cc); | |
| 116 | |||
| 117 | // #define OPTIMIZE_ADD_CONSTRAINT | ||
| 118 | #ifdef OPTIMIZE_ADD_CONSTRAINT // the optimized code is actually slower than | ||
| 119 | // the original | ||
| 120 | T1 = J.col(j - 1); | ||
| 121 | cc_ss(0) = cc; | ||
| 122 | cc_ss(1) = ss; | ||
| 123 | J.col(j - 1).noalias() = J.middleCols<2>(j - 1) * cc_ss; | ||
| 124 | J.col(j) = xny * (T1 + J.col(j - 1)) - J.col(j); | ||
| 125 | #else | ||
| 126 | // J.col(j-1) = J[:,j-1:j] * [cc; ss] | ||
| 127 |
2/2✓ Branch 0 taken 381394956 times.
✓ Branch 1 taken 669956 times.
|
382064912 | for (k = 0; k < nVars; k++) { |
| 128 | 381394956 | t1 = J(k, j - 1); | |
| 129 | 381394956 | t2 = J(k, j); | |
| 130 | 381394956 | J(k, j - 1) = t1 * cc + t2 * ss; | |
| 131 | 381394956 | J(k, j) = xny * (t1 + J(k, j - 1)) - t2; | |
| 132 | } | ||
| 133 | #endif | ||
| 134 | } | ||
| 135 | /* update the number of constraints added*/ | ||
| 136 | 6731 | iq++; | |
| 137 | /* To update R we have to put the iq components of the d vector | ||
| 138 | into column iq - 1 of R | ||
| 139 | */ | ||
| 140 |
3/6✓ Branch 2 taken 6731 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6731 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6731 times.
✗ Branch 9 not taken.
|
6731 | R.col(iq - 1).head(iq) = d.head(iq); |
| 141 | #ifdef TRACE_SOLVER | ||
| 142 | std::cout << iq << std::endl; | ||
| 143 | #endif | ||
| 144 | |||
| 145 |
1/2✗ Branch 3 not taken.
✓ Branch 4 taken 6731 times.
|
6731 | if (std::abs(d(iq - 1)) <= std::numeric_limits<double>::epsilon() * R_norm) |
| 146 | // problem degenerate | ||
| 147 | ✗ | return false; | |
| 148 | 6731 | R_norm = std::max<double>(R_norm, std::abs(d(iq - 1))); | |
| 149 | 6731 | return true; | |
| 150 | } | ||
| 151 | |||
| 152 | 947 | void EiquadprogFast::delete_constraint(MatrixXd& R, MatrixXd& J, VectorXi& A, | |
| 153 | VectorXd& u, int nEqCon, int& iq, | ||
| 154 | int l) { | ||
| 155 | 947 | const long int nVars = R.rows(); | |
| 156 | #ifdef TRACE_SOLVER | ||
| 157 | std::cout << "Delete constraint " << l << ' ' << iq; | ||
| 158 | #endif | ||
| 159 | int i, j, k; | ||
| 160 | 947 | int qq = 0; | |
| 161 | double cc, ss, h, xny, t1, t2; | ||
| 162 | |||
| 163 | /* Find the index qq for active constraint l to be removed */ | ||
| 164 |
1/2✓ Branch 0 taken 4942 times.
✗ Branch 1 not taken.
|
4942 | for (i = nEqCon; i < iq; i++) |
| 165 |
2/2✓ Branch 1 taken 947 times.
✓ Branch 2 taken 3995 times.
|
4942 | if (A(i) == l) { |
| 166 | 947 | qq = i; | |
| 167 | 947 | break; | |
| 168 | } | ||
| 169 | |||
| 170 | /* remove the constraint from the active set and the duals */ | ||
| 171 |
2/2✓ Branch 0 taken 2799 times.
✓ Branch 1 taken 947 times.
|
3746 | for (i = qq; i < iq - 1; i++) { |
| 172 | 2799 | A(i) = A(i + 1); | |
| 173 | 2799 | u(i) = u(i + 1); | |
| 174 |
2/4✓ Branch 2 taken 2799 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2799 times.
✗ Branch 6 not taken.
|
2799 | R.col(i) = R.col(i + 1); |
| 175 | } | ||
| 176 | |||
| 177 | 947 | A(iq - 1) = A(iq); | |
| 178 | 947 | u(iq - 1) = u(iq); | |
| 179 | 947 | A(iq) = 0; | |
| 180 | 947 | u(iq) = 0.0; | |
| 181 |
2/2✓ Branch 1 taken 37537 times.
✓ Branch 2 taken 947 times.
|
38484 | for (j = 0; j < iq; j++) R(j, iq - 1) = 0.0; |
| 182 | /* constraint has been fully removed */ | ||
| 183 | 947 | iq--; | |
| 184 | #ifdef TRACE_SOLVER | ||
| 185 | std::cout << '/' << iq << std::endl; | ||
| 186 | #endif | ||
| 187 | |||
| 188 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 947 times.
|
947 | if (iq == 0) return; |
| 189 | |||
| 190 |
2/2✓ Branch 0 taken 2799 times.
✓ Branch 1 taken 947 times.
|
3746 | for (j = qq; j < iq; j++) { |
| 191 | 2799 | cc = R(j, j); | |
| 192 | 2799 | ss = R(j + 1, j); | |
| 193 | 2799 | h = distance(cc, ss); | |
| 194 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 2799 times.
|
2799 | if (h == 0.0) continue; |
| 195 | 2799 | cc = cc / h; | |
| 196 | 2799 | ss = ss / h; | |
| 197 | 2799 | R(j + 1, j) = 0.0; | |
| 198 |
2/2✓ Branch 0 taken 1375 times.
✓ Branch 1 taken 1424 times.
|
2799 | if (cc < 0.0) { |
| 199 | 1375 | R(j, j) = -h; | |
| 200 | 1375 | cc = -cc; | |
| 201 | 1375 | ss = -ss; | |
| 202 | } else | ||
| 203 | 1424 | R(j, j) = h; | |
| 204 | |||
| 205 | 2799 | xny = ss / (1.0 + cc); | |
| 206 |
2/2✓ Branch 0 taken 55174 times.
✓ Branch 1 taken 2799 times.
|
57973 | for (k = j + 1; k < iq; k++) { |
| 207 | 55174 | t1 = R(j, k); | |
| 208 | 55174 | t2 = R(j + 1, k); | |
| 209 | 55174 | R(j, k) = t1 * cc + t2 * ss; | |
| 210 | 55174 | R(j + 1, k) = xny * (t1 + R(j, k)) - t2; | |
| 211 | } | ||
| 212 |
2/2✓ Branch 0 taken 1372909 times.
✓ Branch 1 taken 2799 times.
|
1375708 | for (k = 0; k < nVars; k++) { |
| 213 | 1372909 | t1 = J(k, j); | |
| 214 | 1372909 | t2 = J(k, j + 1); | |
| 215 | 1372909 | J(k, j) = t1 * cc + t2 * ss; | |
| 216 | 1372909 | J(k, j + 1) = xny * (J(k, j) + t1) - t2; | |
| 217 | } | ||
| 218 | } | ||
| 219 | } | ||
| 220 | |||
| 221 | template <class Derived> | ||
| 222 | void print_vector(const char* name, Eigen::MatrixBase<Derived>& x, int n) { | ||
| 223 | if (x.size() < 10) std::cout << name << x.transpose() << std::endl; | ||
| 224 | } | ||
| 225 | template <class Derived> | ||
| 226 | void print_matrix(const char* name, Eigen::MatrixBase<Derived>& x, int n) { | ||
| 227 | // std::cout << name << std::endl << x << std::endl; | ||
| 228 | } | ||
| 229 | |||
| 230 | 2571 | EiquadprogFast_status EiquadprogFast::solve_quadprog( | |
| 231 | const MatrixXd& Hess, const VectorXd& g0, const MatrixXd& CE, | ||
| 232 | const VectorXd& ce0, const MatrixXd& CI, const VectorXd& ci0, VectorXd& x) { | ||
| 233 | 2571 | const int nVars = (int)g0.size(); | |
| 234 | 2571 | const int nEqCon = (int)ce0.size(); | |
| 235 | 2571 | const int nIneqCon = (int)ci0.size(); | |
| 236 | |||
| 237 |
1/6✗ Branch 0 not taken.
✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
2571 | if (nVars != m_nVars || nEqCon != m_nEqCon || nIneqCon != m_nIneqCon) |
| 238 |
1/2✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
|
2571 | reset(nVars, nEqCon, nIneqCon); |
| 239 | |||
| 240 |
2/4✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2571 times.
✗ Branch 5 not taken.
|
2571 | assert(Hess.rows() == m_nVars && Hess.cols() == m_nVars); |
| 241 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 2571 times.
|
2571 | assert(g0.size() == m_nVars); |
| 242 |
2/4✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2571 times.
✗ Branch 5 not taken.
|
2571 | assert(CE.rows() == m_nEqCon && CE.cols() == m_nVars); |
| 243 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 2571 times.
|
2571 | assert(ce0.size() == m_nEqCon); |
| 244 |
2/4✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2571 times.
✗ Branch 5 not taken.
|
2571 | assert(CI.rows() == m_nIneqCon && CI.cols() == m_nVars); |
| 245 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 2571 times.
|
2571 | assert(ci0.size() == m_nIneqCon); |
| 246 | |||
| 247 | int i, k, l; // indices | ||
| 248 | int ip; // index of the chosen violated constraint | ||
| 249 | int iq; // current number of active constraints | ||
| 250 | double psi; // current sum of constraint violations | ||
| 251 | double c1; // Hessian trace | ||
| 252 | double c2; // Hessian Chowlesky factor trace | ||
| 253 | double ss; // largest constraint violation (negative for violation) | ||
| 254 | double R_norm; // norm of matrix R | ||
| 255 | 2571 | const double inf = std::numeric_limits<double>::infinity(); | |
| 256 | double t, t1, t2; | ||
| 257 | /* t is the step length, which is the minimum of the partial step length t1 | ||
| 258 | * and the full step length t2 */ | ||
| 259 | |||
| 260 | 2571 | iter = 0; // active-set iteration number | |
| 261 | |||
| 262 | /* | ||
| 263 | * Preprocessing phase | ||
| 264 | */ | ||
| 265 | /* compute the trace of the original matrix Hess */ | ||
| 266 |
1/2✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
|
2571 | c1 = Hess.trace(); |
| 267 | |||
| 268 | /* decompose the matrix Hess in the form LL^T */ | ||
| 269 |
1/2✓ Branch 0 taken 2571 times.
✗ Branch 1 not taken.
|
2571 | if (!is_inverse_provided_) { |
| 270 | START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOWLESKY_DECOMPOSITION); | ||
| 271 |
1/2✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
|
2571 | chol_.compute(Hess); |
| 272 | STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOWLESKY_DECOMPOSITION); | ||
| 273 | } | ||
| 274 | |||
| 275 | /* initialize the matrix R */ | ||
| 276 |
1/2✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
|
2571 | d.setZero(nVars); |
| 277 |
1/2✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
|
2571 | R.setZero(nVars, nVars); |
| 278 | 2571 | R_norm = 1.0; | |
| 279 | |||
| 280 | /* compute the inverse of the factorized matrix Hess^-1, this is the initial | ||
| 281 | * value for H */ | ||
| 282 | // m_J = L^-T | ||
| 283 |
1/2✓ Branch 0 taken 2571 times.
✗ Branch 1 not taken.
|
2571 | if (!is_inverse_provided_) { |
| 284 | START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOWLESKY_INVERSE); | ||
| 285 |
1/2✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
|
2571 | m_J.setIdentity(nVars, nVars); |
| 286 | #ifdef OPTIMIZE_HESSIAN_INVERSE | ||
| 287 |
2/4✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2571 times.
✗ Branch 5 not taken.
|
2571 | chol_.matrixU().solveInPlace(m_J); |
| 288 | #else | ||
| 289 | m_J = chol_.matrixU().solve(m_J); | ||
| 290 | #endif | ||
| 291 | STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOWLESKY_INVERSE); | ||
| 292 | } | ||
| 293 | |||
| 294 |
1/2✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
|
2571 | c2 = m_J.trace(); |
| 295 | #ifdef _SOLVER | ||
| 296 | print_matrix("m_J", m_J, nVars); | ||
| 297 | #endif | ||
| 298 | |||
| 299 | /* c1 * c2 is an estimate for cond(Hess) */ | ||
| 300 | |||
| 301 | /* | ||
| 302 | * Find the unconstrained minimizer of the quadratic form 0.5 * x Hess x + g0 | ||
| 303 | * x this is a feasible point in the dual space x = Hess^-1 * g0 | ||
| 304 | */ | ||
| 305 | START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM); | ||
| 306 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 2571 times.
|
2571 | if (is_inverse_provided_) { |
| 307 | ✗ | x = m_J * (m_J.transpose() * g0); | |
| 308 | } else { | ||
| 309 | #ifdef OPTIMIZE_UNCONSTR_MINIM | ||
| 310 |
2/4✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2571 times.
✗ Branch 5 not taken.
|
2571 | x = -g0; |
| 311 |
1/2✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
|
2571 | chol_.solveInPlace(x); |
| 312 | } | ||
| 313 | #else | ||
| 314 | x = chol_.solve(g0); | ||
| 315 | } | ||
| 316 | x = -x; | ||
| 317 | #endif | ||
| 318 | /* and compute the current solution value */ | ||
| 319 |
1/2✓ Branch 1 taken 2571 times.
✗ Branch 2 not taken.
|
2571 | f_value = 0.5 * g0.dot(x); |
| 320 | #ifdef TRACE_SOLVER | ||
| 321 | std::cout << "Unconstrained solution: " << f_value << std::endl; | ||
| 322 | print_vector("x", x, nVars); | ||
| 323 | #endif | ||
| 324 | STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM); | ||
| 325 | |||
| 326 | /* Add equality constraints to the working set A */ | ||
| 327 | |||
| 328 | START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR); | ||
| 329 | 2571 | iq = 0; | |
| 330 |
2/2✓ Branch 0 taken 1926 times.
✓ Branch 1 taken 2571 times.
|
4497 | for (i = 0; i < nEqCon; i++) { |
| 331 | START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_1); | ||
| 332 |
2/4✓ Branch 1 taken 1926 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1926 times.
✗ Branch 5 not taken.
|
1926 | np = CE.row(i); |
| 333 |
1/2✓ Branch 1 taken 1926 times.
✗ Branch 2 not taken.
|
1926 | compute_d(d, m_J, np); |
| 334 |
1/2✓ Branch 1 taken 1926 times.
✗ Branch 2 not taken.
|
1926 | update_z(z, m_J, d, iq); |
| 335 |
1/2✓ Branch 1 taken 1926 times.
✗ Branch 2 not taken.
|
1926 | update_r(R, r, d, iq); |
| 336 | |||
| 337 | #ifdef TRACE_SOLVER | ||
| 338 | print_matrix("R", R, iq); | ||
| 339 | print_vector("z", z, nVars); | ||
| 340 | print_vector("r", r, iq); | ||
| 341 | print_vector("d", d, nVars); | ||
| 342 | #endif | ||
| 343 | |||
| 344 | /* compute full step length t2: i.e., the minimum step in primal space s.t. | ||
| 345 | the contraint becomes feasible */ | ||
| 346 | 1926 | t2 = 0.0; | |
| 347 |
2/4✓ Branch 1 taken 1926 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1926 times.
✗ Branch 5 not taken.
|
3852 | if (std::abs(z.dot(z)) > |
| 348 | 1926 | std::numeric_limits<double>::epsilon()) // i.e. z != 0 | |
| 349 |
3/6✓ Branch 1 taken 1926 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1926 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1926 times.
✗ Branch 8 not taken.
|
1926 | t2 = (-np.dot(x) - ce0(i)) / z.dot(np); |
| 350 | |||
| 351 |
2/4✓ Branch 1 taken 1926 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1926 times.
✗ Branch 5 not taken.
|
1926 | x += t2 * z; |
| 352 | |||
| 353 | /* set u = u+ */ | ||
| 354 |
1/2✓ Branch 1 taken 1926 times.
✗ Branch 2 not taken.
|
1926 | u(iq) = t2; |
| 355 |
4/8✓ Branch 1 taken 1926 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1926 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1926 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1926 times.
✗ Branch 11 not taken.
|
1926 | u.head(iq) -= t2 * r.head(iq); |
| 356 | |||
| 357 | /* compute the new solution value */ | ||
| 358 |
1/2✓ Branch 1 taken 1926 times.
✗ Branch 2 not taken.
|
1926 | f_value += 0.5 * (t2 * t2) * z.dot(np); |
| 359 |
1/2✓ Branch 1 taken 1926 times.
✗ Branch 2 not taken.
|
1926 | A(i) = -i - 1; |
| 360 | STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_1); | ||
| 361 | |||
| 362 | START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_2); | ||
| 363 |
2/4✓ Branch 1 taken 1926 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1926 times.
|
1926 | if (!add_constraint(R, m_J, d, iq, R_norm)) { |
| 364 | // Equality constraints are linearly dependent | ||
| 365 | ✗ | return EIQUADPROG_FAST_REDUNDANT_EQUALITIES; | |
| 366 | } | ||
| 367 | STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_2); | ||
| 368 | } | ||
| 369 | STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR); | ||
| 370 | |||
| 371 | /* set iai = K \ A */ | ||
| 372 |
3/4✓ Branch 1 taken 99898 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 99898 times.
✓ Branch 4 taken 2571 times.
|
102469 | for (i = 0; i < nIneqCon; i++) iai(i) = i; |
| 373 | |||
| 374 | #ifdef USE_WARM_START | ||
| 375 | // DEBUG_STREAM("Gonna warm start using previous active | ||
| 376 | // set:\n"<<A.transpose()<<"\n") | ||
| 377 | for (i = nEqCon; i < q; i++) { | ||
| 378 | iai(i - nEqCon) = -1; | ||
| 379 | ip = A(i); | ||
| 380 | np = CI.row(ip); | ||
| 381 | compute_d(d, m_J, np); | ||
| 382 | update_z(z, m_J, d, iq); | ||
| 383 | update_r(R, r, d, iq); | ||
| 384 | |||
| 385 | /* compute full step length t2: i.e., the minimum step in primal space s.t. | ||
| 386 | the contraint becomes feasible */ | ||
| 387 | t2 = 0.0; | ||
| 388 | if (std::abs(z.dot(z)) > | ||
| 389 | std::numeric_limits<double>::epsilon()) // i.e. z != 0 | ||
| 390 | t2 = (-np.dot(x) - ci0(ip)) / z.dot(np); | ||
| 391 | else | ||
| 392 | DEBUG_STREAM("[WARM START] z=0\n") | ||
| 393 | |||
| 394 | x += t2 * z; | ||
| 395 | |||
| 396 | /* set u = u+ */ | ||
| 397 | u(iq) = t2; | ||
| 398 | u.head(iq) -= t2 * r.head(iq); | ||
| 399 | |||
| 400 | /* compute the new solution value */ | ||
| 401 | f_value += 0.5 * (t2 * t2) * z.dot(np); | ||
| 402 | |||
| 403 | if (!add_constraint(R, m_J, d, iq, R_norm)) { | ||
| 404 | // constraints are linearly dependent | ||
| 405 | std::cout << "[WARM START] Constraints are linearly dependent\n"; | ||
| 406 | return RT_EIQUADPROG_REDUNDANT_EQUALITIES; | ||
| 407 | } | ||
| 408 | } | ||
| 409 | #else | ||
| 410 | |||
| 411 | #endif | ||
| 412 | |||
| 413 | 2571 | l1: | |
| 414 | 7376 | iter++; | |
| 415 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 7376 times.
|
7376 | if (iter >= m_maxIter) { |
| 416 | ✗ | q = iq; | |
| 417 | ✗ | return EIQUADPROG_FAST_MAX_ITER_REACHED; | |
| 418 | } | ||
| 419 | |||
| 420 | START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1); | ||
| 421 | |||
| 422 | #ifdef TRACE_SOLVER | ||
| 423 | print_vector("x", x, nVars); | ||
| 424 | #endif | ||
| 425 | /* step 1: choose a violated constraint */ | ||
| 426 |
2/2✓ Branch 0 taken 63763 times.
✓ Branch 1 taken 7376 times.
|
71139 | for (i = nEqCon; i < iq; i++) { |
| 427 |
1/2✓ Branch 1 taken 63763 times.
✗ Branch 2 not taken.
|
63763 | ip = A(i); |
| 428 |
1/2✓ Branch 1 taken 63763 times.
✗ Branch 2 not taken.
|
63763 | iai(ip) = -1; |
| 429 | } | ||
| 430 | |||
| 431 | /* compute s(x) = ci^T * x + ci0 for all elements of K \ A */ | ||
| 432 | START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_2); | ||
| 433 | 7376 | ss = 0.0; | |
| 434 | 7376 | ip = 0; /* ip will be the index of the chosen violated constraint */ | |
| 435 | |||
| 436 | #ifdef OPTIMIZE_STEP_1_2 | ||
| 437 |
1/2✓ Branch 1 taken 7376 times.
✗ Branch 2 not taken.
|
7376 | s = ci0; |
| 438 |
3/6✓ Branch 1 taken 7376 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7376 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7376 times.
✗ Branch 8 not taken.
|
7376 | s.noalias() += CI * x; |
| 439 |
1/2✓ Branch 1 taken 7376 times.
✗ Branch 2 not taken.
|
7376 | iaexcl.setOnes(); |
| 440 |
3/6✓ Branch 1 taken 7376 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7376 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7376 times.
✗ Branch 8 not taken.
|
7376 | psi = (s.cwiseMin(VectorXd::Zero(nIneqCon))).sum(); |
| 441 | #else | ||
| 442 | psi = 0.0; /* this value will contain the sum of all infeasibilities */ | ||
| 443 | for (i = 0; i < nIneqCon; i++) { | ||
| 444 | iaexcl(i) = 1; | ||
| 445 | s(i) = CI.row(i).dot(x) + ci0(i); | ||
| 446 | psi += std::min(0.0, s(i)); | ||
| 447 | } | ||
| 448 | #endif | ||
| 449 | STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_2); | ||
| 450 | #ifdef TRACE_SOLVER | ||
| 451 | print_vector("s", s, nIneqCon); | ||
| 452 | #endif | ||
| 453 | |||
| 454 | STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1); | ||
| 455 | |||
| 456 | 7376 | if (std::abs(psi) <= | |
| 457 |
2/2✓ Branch 1 taken 557 times.
✓ Branch 2 taken 6819 times.
|
7376 | nIneqCon * std::numeric_limits<double>::epsilon() * c1 * c2 * 100.0) { |
| 458 | /* numerically there are not infeasibilities anymore */ | ||
| 459 | 557 | q = iq; | |
| 460 | // DEBUG_STREAM("Optimal active | ||
| 461 | // set:\n"<<A.head(iq).transpose()<<"\n\n") | ||
| 462 | 557 | return EIQUADPROG_FAST_OPTIMAL; | |
| 463 | } | ||
| 464 | |||
| 465 | /* save old values for u, x and A */ | ||
| 466 |
3/6✓ Branch 1 taken 6819 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6819 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6819 times.
✗ Branch 8 not taken.
|
6819 | u_old.head(iq) = u.head(iq); |
| 467 |
3/6✓ Branch 1 taken 6819 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6819 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6819 times.
✗ Branch 8 not taken.
|
6819 | A_old.head(iq) = A.head(iq); |
| 468 |
1/2✓ Branch 1 taken 6819 times.
✗ Branch 2 not taken.
|
6819 | x_old = x; |
| 469 | |||
| 470 | 6819 | l2: /* Step 2: check for feasibility and determine a new S-pair */ | |
| 471 | START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2); | ||
| 472 | // find constraint with highest violation (what about normalizing | ||
| 473 | // constraints?) | ||
| 474 |
2/2✓ Branch 0 taken 1074627 times.
✓ Branch 1 taken 6819 times.
|
1081446 | for (i = 0; i < nIneqCon; i++) { |
| 475 |
10/14✓ Branch 1 taken 1074627 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 19471 times.
✓ Branch 4 taken 1055156 times.
✓ Branch 6 taken 19471 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 17228 times.
✓ Branch 9 taken 2243 times.
✓ Branch 11 taken 17228 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 17228 times.
✗ Branch 14 not taken.
✓ Branch 15 taken 17228 times.
✓ Branch 16 taken 1057399 times.
|
1074627 | if (s(i) < ss && iai(i) != -1 && iaexcl(i)) { |
| 476 |
1/2✓ Branch 1 taken 17228 times.
✗ Branch 2 not taken.
|
17228 | ss = s(i); |
| 477 | 17228 | ip = i; | |
| 478 | } | ||
| 479 | } | ||
| 480 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 6819 times.
|
6819 | if (ss >= 0.0) { |
| 481 | ✗ | q = iq; | |
| 482 | // DEBUG_STREAM("Optimal active set:\n"<<A.transpose()<<"\n\n") | ||
| 483 | ✗ | return EIQUADPROG_FAST_OPTIMAL; | |
| 484 | } | ||
| 485 | |||
| 486 | /* set np = n(ip) */ | ||
| 487 |
2/4✓ Branch 1 taken 6819 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6819 times.
✗ Branch 5 not taken.
|
6819 | np = CI.row(ip); |
| 488 | /* set u = (u 0)^T */ | ||
| 489 |
1/2✓ Branch 1 taken 6819 times.
✗ Branch 2 not taken.
|
6819 | u(iq) = 0.0; |
| 490 | /* add ip to the active set A */ | ||
| 491 |
1/2✓ Branch 1 taken 6819 times.
✗ Branch 2 not taken.
|
6819 | A(iq) = ip; |
| 492 | |||
| 493 | // DEBUG_STREAM("Add constraint "<<ip<<" to active set\n") | ||
| 494 | |||
| 495 | #ifdef TRACE_SOLVER | ||
| 496 | std::cout << "Trying with constraint " << ip << std::endl; | ||
| 497 | print_vector("np", np, nVars); | ||
| 498 | #endif | ||
| 499 | STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2); | ||
| 500 | |||
| 501 | 7766 | l2a: /* Step 2a: determine step direction */ | |
| 502 | START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2A); | ||
| 503 | /* compute z = H np: the step direction in the primal space (through m_J, see | ||
| 504 | * the paper) */ | ||
| 505 |
1/2✓ Branch 1 taken 7766 times.
✗ Branch 2 not taken.
|
7766 | compute_d(d, m_J, np); |
| 506 | // update_z(z, m_J, d, iq); | ||
| 507 |
2/2✓ Branch 0 taken 1 times.
✓ Branch 1 taken 7765 times.
|
7766 | if (iq >= nVars) { |
| 508 | // throw std::runtime_error("iq >= m_J.cols()"); | ||
| 509 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | z.setZero(); |
| 510 | } else { | ||
| 511 |
1/2✓ Branch 1 taken 7765 times.
✗ Branch 2 not taken.
|
7765 | update_z(z, m_J, d, iq); |
| 512 | } | ||
| 513 | /* compute N* np (if q > 0): the negative of the step direction in the dual | ||
| 514 | * space */ | ||
| 515 |
1/2✓ Branch 1 taken 7766 times.
✗ Branch 2 not taken.
|
7766 | update_r(R, r, d, iq); |
| 516 | #ifdef TRACE_SOLVER | ||
| 517 | std::cout << "Step direction z" << std::endl; | ||
| 518 | print_vector("z", z, nVars); | ||
| 519 | print_vector("r", r, iq + 1); | ||
| 520 | print_vector("u", u, iq + 1); | ||
| 521 | print_vector("d", d, nVars); | ||
| 522 | print_vector("A", A, iq + 1); | ||
| 523 | #endif | ||
| 524 | STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2A); | ||
| 525 | |||
| 526 | /* Step 2b: compute step length */ | ||
| 527 | START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2B); | ||
| 528 | 7766 | l = 0; | |
| 529 | /* Compute t1: partial step length (maximum step in dual space without | ||
| 530 | * violating dual feasibility */ | ||
| 531 | 7766 | t1 = inf; /* +inf */ | |
| 532 | /* find the index l s.t. it reaches the minimum of u+(x) / r */ | ||
| 533 | // l: index of constraint to drop (maybe) | ||
| 534 |
2/2✓ Branch 0 taken 69271 times.
✓ Branch 1 taken 7766 times.
|
77037 | for (k = nEqCon; k < iq; k++) { |
| 535 | double tmp; | ||
| 536 |
9/12✓ Branch 1 taken 69271 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 23926 times.
✓ Branch 4 taken 45345 times.
✓ Branch 6 taken 23926 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 23926 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 7664 times.
✓ Branch 12 taken 16262 times.
✓ Branch 13 taken 7664 times.
✓ Branch 14 taken 61607 times.
|
69271 | if (r(k) > 0.0 && ((tmp = u(k) / r(k)) < t1)) { |
| 537 | 7664 | t1 = tmp; | |
| 538 |
1/2✓ Branch 1 taken 7664 times.
✗ Branch 2 not taken.
|
7664 | l = A(k); |
| 539 | } | ||
| 540 | } | ||
| 541 | /* Compute t2: full step length (minimum step in primal space such that the | ||
| 542 | * constraint ip becomes feasible */ | ||
| 543 |
3/4✓ Branch 1 taken 7766 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5024 times.
✓ Branch 5 taken 2742 times.
|
15532 | if (std::abs(z.dot(z)) > |
| 544 | 7766 | std::numeric_limits<double>::epsilon()) // i.e. z != 0 | |
| 545 |
2/4✓ Branch 1 taken 5024 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5024 times.
✗ Branch 5 not taken.
|
5024 | t2 = -s(ip) / z.dot(np); |
| 546 | else | ||
| 547 | 2742 | t2 = inf; /* +inf */ | |
| 548 | |||
| 549 | /* the step is chosen as the minimum of t1 and t2 */ | ||
| 550 | 7766 | t = std::min(t1, t2); | |
| 551 | #ifdef TRACE_SOLVER | ||
| 552 | std::cout << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2 | ||
| 553 | << ") "; | ||
| 554 | #endif | ||
| 555 | STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2B); | ||
| 556 | |||
| 557 | /* Step 2c: determine new S-pair and take step: */ | ||
| 558 | START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C); | ||
| 559 | /* case (i): no step in primal or dual space */ | ||
| 560 |
2/2✓ Branch 0 taken 2014 times.
✓ Branch 1 taken 5752 times.
|
7766 | if (t >= inf) { |
| 561 | /* QPP is infeasible */ | ||
| 562 | 2014 | q = iq; | |
| 563 | STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C); | ||
| 564 | 2014 | return EIQUADPROG_FAST_UNBOUNDED; | |
| 565 | } | ||
| 566 | /* case (ii): step in dual space */ | ||
| 567 |
2/2✓ Branch 0 taken 728 times.
✓ Branch 1 taken 5024 times.
|
5752 | if (t2 >= inf) { |
| 568 | /* set u = u + t * [-r 1) and drop constraint l from the active set A */ | ||
| 569 |
4/8✓ Branch 1 taken 728 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 728 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 728 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 728 times.
✗ Branch 11 not taken.
|
728 | u.head(iq) -= t * r.head(iq); |
| 570 |
1/2✓ Branch 1 taken 728 times.
✗ Branch 2 not taken.
|
728 | u(iq) += t; |
| 571 |
1/2✓ Branch 1 taken 728 times.
✗ Branch 2 not taken.
|
728 | iai(l) = l; |
| 572 |
1/2✓ Branch 1 taken 728 times.
✗ Branch 2 not taken.
|
728 | delete_constraint(R, m_J, A, u, nEqCon, iq, l); |
| 573 | #ifdef TRACE_SOLVER | ||
| 574 | std::cout << " in dual space: " << f_value << std::endl; | ||
| 575 | print_vector("x", x, nVars); | ||
| 576 | print_vector("z", z, nVars); | ||
| 577 | print_vector("A", A, iq + 1); | ||
| 578 | #endif | ||
| 579 | STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C); | ||
| 580 | 728 | goto l2a; | |
| 581 | } | ||
| 582 | |||
| 583 | /* case (iii): step in primal and dual space */ | ||
| 584 |
2/4✓ Branch 1 taken 5024 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5024 times.
✗ Branch 5 not taken.
|
5024 | x += t * z; |
| 585 | /* update the solution value */ | ||
| 586 |
2/4✓ Branch 1 taken 5024 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5024 times.
✗ Branch 5 not taken.
|
5024 | f_value += t * z.dot(np) * (0.5 * t + u(iq)); |
| 587 | |||
| 588 |
4/8✓ Branch 1 taken 5024 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5024 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 5024 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 5024 times.
✗ Branch 11 not taken.
|
5024 | u.head(iq) -= t * r.head(iq); |
| 589 |
1/2✓ Branch 1 taken 5024 times.
✗ Branch 2 not taken.
|
5024 | u(iq) += t; |
| 590 | |||
| 591 | #ifdef TRACE_SOLVER | ||
| 592 | std::cout << " in both spaces: " << f_value << std::endl; | ||
| 593 | print_vector("x", x, nVars); | ||
| 594 | print_vector("u", u, iq + 1); | ||
| 595 | print_vector("r", r, iq + 1); | ||
| 596 | print_vector("A", A, iq + 1); | ||
| 597 | #endif | ||
| 598 | |||
| 599 |
2/2✓ Branch 0 taken 4805 times.
✓ Branch 1 taken 219 times.
|
5024 | if (t == t2) { |
| 600 | #ifdef TRACE_SOLVER | ||
| 601 | std::cout << "Full step has taken " << t << std::endl; | ||
| 602 | print_vector("x", x, nVars); | ||
| 603 | #endif | ||
| 604 | /* full step has taken */ | ||
| 605 | /* add constraint ip to the active set*/ | ||
| 606 |
2/4✓ Branch 1 taken 4805 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 4805 times.
|
4805 | if (!add_constraint(R, m_J, d, iq, R_norm)) { |
| 607 | ✗ | iaexcl(ip) = 0; | |
| 608 | ✗ | delete_constraint(R, m_J, A, u, nEqCon, iq, ip); | |
| 609 | #ifdef TRACE_SOLVER | ||
| 610 | print_matrix("R", R, nVars); | ||
| 611 | print_vector("A", A, iq); | ||
| 612 | #endif | ||
| 613 | ✗ | for (i = 0; i < nIneqCon; i++) iai(i) = i; | |
| 614 | ✗ | for (i = 0; i < iq; i++) { | |
| 615 | ✗ | A(i) = A_old(i); | |
| 616 | ✗ | iai(A(i)) = -1; | |
| 617 | ✗ | u(i) = u_old(i); | |
| 618 | } | ||
| 619 | ✗ | x = x_old; | |
| 620 | STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C); | ||
| 621 | ✗ | goto l2; /* go to step 2 */ | |
| 622 | } else | ||
| 623 |
1/2✓ Branch 1 taken 4805 times.
✗ Branch 2 not taken.
|
4805 | iai(ip) = -1; |
| 624 | #ifdef TRACE_SOLVER | ||
| 625 | print_matrix("R", R, nVars); | ||
| 626 | print_vector("A", A, iq); | ||
| 627 | #endif | ||
| 628 | STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C); | ||
| 629 | 4805 | goto l1; | |
| 630 | } | ||
| 631 | |||
| 632 | /* a partial step has been taken => drop constraint l */ | ||
| 633 |
1/2✓ Branch 1 taken 219 times.
✗ Branch 2 not taken.
|
219 | iai(l) = l; |
| 634 |
1/2✓ Branch 1 taken 219 times.
✗ Branch 2 not taken.
|
219 | delete_constraint(R, m_J, A, u, nEqCon, iq, l); |
| 635 |
4/8✓ Branch 1 taken 219 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 219 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 219 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 219 times.
✗ Branch 11 not taken.
|
219 | s(ip) = CI.row(ip).dot(x) + ci0(ip); |
| 636 | |||
| 637 | #ifdef TRACE_SOLVER | ||
| 638 | std::cout << "Partial step has taken " << t << std::endl; | ||
| 639 | print_vector("x", x, nVars); | ||
| 640 | print_matrix("R", R, nVars); | ||
| 641 | print_vector("A", A, iq); | ||
| 642 | print_vector("s", s, nIneqCon); | ||
| 643 | #endif | ||
| 644 | STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C); | ||
| 645 | |||
| 646 | 219 | goto l2a; | |
| 647 | } | ||
| 648 | |||
| 649 | ✗ | double trace_sparse(const EiquadprogFast::SpMat& Hess) { | |
| 650 | ✗ | double sum = 0; | |
| 651 | ✗ | for (int k = 0; k < Hess.outerSize(); ++k) sum += Hess.coeff(k, k); | |
| 652 | ✗ | return sum; | |
| 653 | } | ||
| 654 | |||
| 655 | ✗ | EiquadprogFast_status EiquadprogFast::solve_quadprog_sparse( | |
| 656 | const EiquadprogFast::SpMat& Hess, const VectorXd& g0, const MatrixXd& CE, | ||
| 657 | const VectorXd& ce0, const MatrixXd& CI, const VectorXd& ci0, VectorXd& x) { | ||
| 658 | ✗ | const int nVars = (int)g0.size(); | |
| 659 | ✗ | const int nEqCon = (int)ce0.size(); | |
| 660 | ✗ | const int nIneqCon = (int)ci0.size(); | |
| 661 | |||
| 662 | ✗ | if (nVars != m_nVars || nEqCon != m_nEqCon || nIneqCon != m_nIneqCon) | |
| 663 | ✗ | reset(nVars, nEqCon, nIneqCon); | |
| 664 | |||
| 665 | ✗ | assert(Hess.rows() == m_nVars && Hess.cols() == m_nVars); | |
| 666 | ✗ | assert(g0.size() == m_nVars); | |
| 667 | ✗ | assert(CE.rows() == m_nEqCon && CE.cols() == m_nVars); | |
| 668 | ✗ | assert(ce0.size() == m_nEqCon); | |
| 669 | ✗ | assert(CI.rows() == m_nIneqCon && CI.cols() == m_nVars); | |
| 670 | ✗ | assert(ci0.size() == m_nIneqCon); | |
| 671 | |||
| 672 | int i, k, l; // indices | ||
| 673 | int ip; // index of the chosen violated constraint | ||
| 674 | int iq; // current number of active constraints | ||
| 675 | double psi; // current sum of constraint violations | ||
| 676 | double c1; // Hessian trace | ||
| 677 | double c2; // Hessian Chowlesky factor trace | ||
| 678 | double ss; // largest constraint violation (negative for violation) | ||
| 679 | double R_norm; // norm of matrix R | ||
| 680 | ✗ | const double inf = std::numeric_limits<double>::infinity(); | |
| 681 | double t, t1, t2; | ||
| 682 | /* t is the step length, which is the minimum of the partial step length t1 | ||
| 683 | * and the full step length t2 */ | ||
| 684 | |||
| 685 | ✗ | iter = 0; // active-set iteration number | |
| 686 | |||
| 687 | /* | ||
| 688 | * Preprocessing phase | ||
| 689 | */ | ||
| 690 | /* compute the trace of the original matrix Hess */ | ||
| 691 | ✗ | c1 = trace_sparse(Hess); | |
| 692 | |||
| 693 | START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOWLESKY_DECOMPOSITION); | ||
| 694 | ✗ | Eigen::SimplicialLLT<SpMat, Eigen::Lower> cholsp_(Hess); | |
| 695 | STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOWLESKY_DECOMPOSITION); | ||
| 696 | |||
| 697 | /* initialize the matrix R */ | ||
| 698 | ✗ | d.setZero(nVars); | |
| 699 | ✗ | R.setZero(nVars, nVars); | |
| 700 | ✗ | R_norm = 1.0; | |
| 701 | |||
| 702 | /* compute the inverse of the factorized matrix Hess^-1, this is the initial | ||
| 703 | * value for H */ | ||
| 704 | // m_J = L^-T | ||
| 705 | ✗ | if (!is_inverse_provided_) { | |
| 706 | START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOWLESKY_INVERSE); | ||
| 707 | ✗ | m_J.setIdentity(nVars, nVars); | |
| 708 | #ifdef OPTIMIZE_HESSIAN_INVERSE | ||
| 709 | ✗ | cholsp_.matrixU().solve(m_J); | |
| 710 | #else | ||
| 711 | m_J = cholsp_.matrixU().solve(m_J); | ||
| 712 | #endif | ||
| 713 | STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_CHOWLESKY_INVERSE); | ||
| 714 | } | ||
| 715 | |||
| 716 | ✗ | c2 = m_J.trace(); | |
| 717 | #ifdef _SOLVER | ||
| 718 | print_matrix("m_J", m_J, nVars); | ||
| 719 | #endif | ||
| 720 | |||
| 721 | /* c1 * c2 is an estimate for cond(Hess) */ | ||
| 722 | |||
| 723 | /* | ||
| 724 | * Find the unconstrained minimizer of the quadratic form 0.5 * x Hess x + g0 | ||
| 725 | * x this is a feasible point in the dual space x = Hess^-1 * g0 | ||
| 726 | */ | ||
| 727 | START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM); | ||
| 728 | ✗ | if (is_inverse_provided_) { | |
| 729 | ✗ | x = m_J * (m_J.transpose() * g0); | |
| 730 | } else { | ||
| 731 | #ifdef OPTIMIZE_UNCONSTR_MINIM | ||
| 732 | // x = -g0; | ||
| 733 | ✗ | x = cholsp_.solve(-g0); | |
| 734 | } | ||
| 735 | #else | ||
| 736 | x = cholsp_.solve(g0); | ||
| 737 | } | ||
| 738 | x = -x; | ||
| 739 | #endif | ||
| 740 | /* and compute the current solution value */ | ||
| 741 | ✗ | f_value = 0.5 * g0.dot(x); | |
| 742 | #ifdef TRACE_SOLVER | ||
| 743 | std::cout << "Unconstrained solution: " << f_value << std::endl; | ||
| 744 | print_vector("x", x, nVars); | ||
| 745 | #endif | ||
| 746 | STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_UNCONSTR_MINIM); | ||
| 747 | |||
| 748 | /* Add equality constraints to the working set A */ | ||
| 749 | |||
| 750 | START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR); | ||
| 751 | ✗ | iq = 0; | |
| 752 | ✗ | for (i = 0; i < nEqCon; i++) { | |
| 753 | START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_1); | ||
| 754 | ✗ | np = CE.row(i); | |
| 755 | ✗ | compute_d(d, m_J, np); | |
| 756 | ✗ | update_z(z, m_J, d, iq); | |
| 757 | ✗ | update_r(R, r, d, iq); | |
| 758 | |||
| 759 | #ifdef TRACE_SOLVER | ||
| 760 | print_matrix("R", R, iq); | ||
| 761 | print_vector("z", z, nVars); | ||
| 762 | print_vector("r", r, iq); | ||
| 763 | print_vector("d", d, nVars); | ||
| 764 | #endif | ||
| 765 | |||
| 766 | /* compute full step length t2: i.e., the minimum step in primal space s.t. | ||
| 767 | the contraint becomes feasible */ | ||
| 768 | ✗ | t2 = 0.0; | |
| 769 | ✗ | if (std::abs(z.dot(z)) > | |
| 770 | ✗ | std::numeric_limits<double>::epsilon()) // i.e. z != 0 | |
| 771 | ✗ | t2 = (-np.dot(x) - ce0(i)) / z.dot(np); | |
| 772 | |||
| 773 | ✗ | x += t2 * z; | |
| 774 | |||
| 775 | /* set u = u+ */ | ||
| 776 | ✗ | u(iq) = t2; | |
| 777 | ✗ | u.head(iq) -= t2 * r.head(iq); | |
| 778 | |||
| 779 | /* compute the new solution value */ | ||
| 780 | ✗ | f_value += 0.5 * (t2 * t2) * z.dot(np); | |
| 781 | ✗ | A(i) = -i - 1; | |
| 782 | STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_1); | ||
| 783 | |||
| 784 | START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_2); | ||
| 785 | ✗ | if (!add_constraint(R, m_J, d, iq, R_norm)) { | |
| 786 | // Equality constraints are linearly dependent | ||
| 787 | ✗ | return EIQUADPROG_FAST_REDUNDANT_EQUALITIES; | |
| 788 | } | ||
| 789 | STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR_2); | ||
| 790 | } | ||
| 791 | STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_ADD_EQ_CONSTR); | ||
| 792 | |||
| 793 | /* set iai = K \ A */ | ||
| 794 | ✗ | for (i = 0; i < nIneqCon; i++) iai(i) = i; | |
| 795 | |||
| 796 | #ifdef USE_WARM_START | ||
| 797 | // DEBUG_STREAM("Gonna warm start using previous active | ||
| 798 | // set:\n"<<A.transpose()<<"\n") | ||
| 799 | for (i = nEqCon; i < q; i++) { | ||
| 800 | iai(i - nEqCon) = -1; | ||
| 801 | ip = A(i); | ||
| 802 | np = CI.row(ip); | ||
| 803 | compute_d(d, m_J, np); | ||
| 804 | update_z(z, m_J, d, iq); | ||
| 805 | update_r(R, r, d, iq); | ||
| 806 | |||
| 807 | /* compute full step length t2: i.e., the minimum step in primal space s.t. | ||
| 808 | the contraint becomes feasible */ | ||
| 809 | t2 = 0.0; | ||
| 810 | if (std::abs(z.dot(z)) > | ||
| 811 | std::numeric_limits<double>::epsilon()) // i.e. z != 0 | ||
| 812 | t2 = (-np.dot(x) - ci0(ip)) / z.dot(np); | ||
| 813 | else | ||
| 814 | DEBUG_STREAM("[WARM START] z=0\n") | ||
| 815 | |||
| 816 | x += t2 * z; | ||
| 817 | |||
| 818 | /* set u = u+ */ | ||
| 819 | u(iq) = t2; | ||
| 820 | u.head(iq) -= t2 * r.head(iq); | ||
| 821 | |||
| 822 | /* compute the new solution value */ | ||
| 823 | f_value += 0.5 * (t2 * t2) * z.dot(np); | ||
| 824 | |||
| 825 | if (!add_constraint(R, m_J, d, iq, R_norm)) { | ||
| 826 | // constraints are linearly dependent | ||
| 827 | std::cout << "[WARM START] Constraints are linearly dependent\n"; | ||
| 828 | return RT_EIQUADPROG_REDUNDANT_EQUALITIES; | ||
| 829 | } | ||
| 830 | } | ||
| 831 | #else | ||
| 832 | |||
| 833 | #endif | ||
| 834 | |||
| 835 | ✗ | l1: | |
| 836 | ✗ | iter++; | |
| 837 | ✗ | if (iter >= m_maxIter) { | |
| 838 | ✗ | q = iq; | |
| 839 | ✗ | return EIQUADPROG_FAST_MAX_ITER_REACHED; | |
| 840 | } | ||
| 841 | |||
| 842 | START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1); | ||
| 843 | |||
| 844 | #ifdef TRACE_SOLVER | ||
| 845 | print_vector("x", x, nVars); | ||
| 846 | #endif | ||
| 847 | /* step 1: choose a violated constraint */ | ||
| 848 | ✗ | for (i = nEqCon; i < iq; i++) { | |
| 849 | ✗ | ip = A(i); | |
| 850 | ✗ | iai(ip) = -1; | |
| 851 | } | ||
| 852 | |||
| 853 | /* compute s(x) = ci^T * x + ci0 for all elements of K \ A */ | ||
| 854 | START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_2); | ||
| 855 | ✗ | ss = 0.0; | |
| 856 | ✗ | ip = 0; /* ip will be the index of the chosen violated constraint */ | |
| 857 | |||
| 858 | #ifdef OPTIMIZE_STEP_1_2 | ||
| 859 | ✗ | s = ci0; | |
| 860 | ✗ | s.noalias() += CI * x; | |
| 861 | ✗ | iaexcl.setOnes(); | |
| 862 | ✗ | psi = (s.cwiseMin(VectorXd::Zero(nIneqCon))).sum(); | |
| 863 | #else | ||
| 864 | psi = 0.0; /* this value will contain the sum of all infeasibilities */ | ||
| 865 | for (i = 0; i < nIneqCon; i++) { | ||
| 866 | iaexcl(i) = 1; | ||
| 867 | s(i) = CI.row(i).dot(x) + ci0(i); | ||
| 868 | psi += std::min(0.0, s(i)); | ||
| 869 | } | ||
| 870 | #endif | ||
| 871 | STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1_2); | ||
| 872 | #ifdef TRACE_SOLVER | ||
| 873 | print_vector("s", s, nIneqCon); | ||
| 874 | #endif | ||
| 875 | |||
| 876 | STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_1); | ||
| 877 | |||
| 878 | ✗ | if (std::abs(psi) <= | |
| 879 | ✗ | nIneqCon * std::numeric_limits<double>::epsilon() * c1 * c2 * 100.0) { | |
| 880 | /* numerically there are not infeasibilities anymore */ | ||
| 881 | ✗ | q = iq; | |
| 882 | // DEBUG_STREAM("Optimal active | ||
| 883 | // set:\n"<<A.head(iq).transpose()<<"\n\n") | ||
| 884 | ✗ | return EIQUADPROG_FAST_OPTIMAL; | |
| 885 | } | ||
| 886 | |||
| 887 | /* save old values for u, x and A */ | ||
| 888 | ✗ | u_old.head(iq) = u.head(iq); | |
| 889 | ✗ | A_old.head(iq) = A.head(iq); | |
| 890 | ✗ | x_old = x; | |
| 891 | |||
| 892 | ✗ | l2: /* Step 2: check for feasibility and determine a new S-pair */ | |
| 893 | START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2); | ||
| 894 | // find constraint with highest violation (what about normalizing | ||
| 895 | // constraints?) | ||
| 896 | ✗ | for (i = 0; i < nIneqCon; i++) { | |
| 897 | ✗ | if (s(i) < ss && iai(i) != -1 && iaexcl(i)) { | |
| 898 | ✗ | ss = s(i); | |
| 899 | ✗ | ip = i; | |
| 900 | } | ||
| 901 | } | ||
| 902 | ✗ | if (ss >= 0.0) { | |
| 903 | ✗ | q = iq; | |
| 904 | // DEBUG_STREAM("Optimal active set:\n"<<A.transpose()<<"\n\n") | ||
| 905 | ✗ | return EIQUADPROG_FAST_OPTIMAL; | |
| 906 | } | ||
| 907 | |||
| 908 | /* set np = n(ip) */ | ||
| 909 | ✗ | np = CI.row(ip); | |
| 910 | /* set u = (u 0)^T */ | ||
| 911 | ✗ | u(iq) = 0.0; | |
| 912 | /* add ip to the active set A */ | ||
| 913 | ✗ | A(iq) = ip; | |
| 914 | |||
| 915 | // DEBUG_STREAM("Add constraint "<<ip<<" to active set\n") | ||
| 916 | |||
| 917 | #ifdef TRACE_SOLVER | ||
| 918 | std::cout << "Trying with constraint " << ip << std::endl; | ||
| 919 | print_vector("np", np, nVars); | ||
| 920 | #endif | ||
| 921 | STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2); | ||
| 922 | |||
| 923 | ✗ | l2a: /* Step 2a: determine step direction */ | |
| 924 | START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2A); | ||
| 925 | /* compute z = H np: the step direction in the primal space (through m_J, see | ||
| 926 | * the paper) */ | ||
| 927 | ✗ | compute_d(d, m_J, np); | |
| 928 | // update_z(z, m_J, d, iq); | ||
| 929 | ✗ | if (iq >= nVars) { | |
| 930 | // throw std::runtime_error("iq >= m_J.cols()"); | ||
| 931 | ✗ | z.setZero(); | |
| 932 | } else { | ||
| 933 | ✗ | update_z(z, m_J, d, iq); | |
| 934 | } | ||
| 935 | /* compute N* np (if q > 0): the negative of the step direction in the dual | ||
| 936 | * space */ | ||
| 937 | ✗ | update_r(R, r, d, iq); | |
| 938 | #ifdef TRACE_SOLVER | ||
| 939 | std::cout << "Step direction z" << std::endl; | ||
| 940 | print_vector("z", z, nVars); | ||
| 941 | print_vector("r", r, iq + 1); | ||
| 942 | print_vector("u", u, iq + 1); | ||
| 943 | print_vector("d", d, nVars); | ||
| 944 | print_vector("A", A, iq + 1); | ||
| 945 | #endif | ||
| 946 | STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2A); | ||
| 947 | |||
| 948 | /* Step 2b: compute step length */ | ||
| 949 | START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2B); | ||
| 950 | ✗ | l = 0; | |
| 951 | /* Compute t1: partial step length (maximum step in dual space without | ||
| 952 | * violating dual feasibility */ | ||
| 953 | ✗ | t1 = inf; /* +inf */ | |
| 954 | /* find the index l s.t. it reaches the minimum of u+(x) / r */ | ||
| 955 | // l: index of constraint to drop (maybe) | ||
| 956 | ✗ | for (k = nEqCon; k < iq; k++) { | |
| 957 | double tmp; | ||
| 958 | ✗ | if (r(k) > 0.0 && ((tmp = u(k) / r(k)) < t1)) { | |
| 959 | ✗ | t1 = tmp; | |
| 960 | ✗ | l = A(k); | |
| 961 | } | ||
| 962 | } | ||
| 963 | /* Compute t2: full step length (minimum step in primal space such that the | ||
| 964 | * constraint ip becomes feasible */ | ||
| 965 | ✗ | if (std::abs(z.dot(z)) > | |
| 966 | ✗ | std::numeric_limits<double>::epsilon()) // i.e. z != 0 | |
| 967 | ✗ | t2 = -s(ip) / z.dot(np); | |
| 968 | else | ||
| 969 | ✗ | t2 = inf; /* +inf */ | |
| 970 | |||
| 971 | /* the step is chosen as the minimum of t1 and t2 */ | ||
| 972 | ✗ | t = std::min(t1, t2); | |
| 973 | #ifdef TRACE_SOLVER | ||
| 974 | std::cout << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2 | ||
| 975 | << ") "; | ||
| 976 | #endif | ||
| 977 | STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2B); | ||
| 978 | |||
| 979 | /* Step 2c: determine new S-pair and take step: */ | ||
| 980 | START_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C); | ||
| 981 | /* case (i): no step in primal or dual space */ | ||
| 982 | ✗ | if (t >= inf) { | |
| 983 | /* QPP is infeasible */ | ||
| 984 | ✗ | q = iq; | |
| 985 | STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C); | ||
| 986 | ✗ | return EIQUADPROG_FAST_UNBOUNDED; | |
| 987 | } | ||
| 988 | /* case (ii): step in dual space */ | ||
| 989 | ✗ | if (t2 >= inf) { | |
| 990 | /* set u = u + t * [-r 1) and drop constraint l from the active set A */ | ||
| 991 | ✗ | u.head(iq) -= t * r.head(iq); | |
| 992 | ✗ | u(iq) += t; | |
| 993 | ✗ | iai(l) = l; | |
| 994 | ✗ | delete_constraint(R, m_J, A, u, nEqCon, iq, l); | |
| 995 | #ifdef TRACE_SOLVER | ||
| 996 | std::cout << " in dual space: " << f_value << std::endl; | ||
| 997 | print_vector("x", x, nVars); | ||
| 998 | print_vector("z", z, nVars); | ||
| 999 | print_vector("A", A, iq + 1); | ||
| 1000 | #endif | ||
| 1001 | STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C); | ||
| 1002 | ✗ | goto l2a; | |
| 1003 | } | ||
| 1004 | |||
| 1005 | /* case (iii): step in primal and dual space */ | ||
| 1006 | ✗ | x += t * z; | |
| 1007 | /* update the solution value */ | ||
| 1008 | ✗ | f_value += t * z.dot(np) * (0.5 * t + u(iq)); | |
| 1009 | |||
| 1010 | ✗ | u.head(iq) -= t * r.head(iq); | |
| 1011 | ✗ | u(iq) += t; | |
| 1012 | |||
| 1013 | #ifdef TRACE_SOLVER | ||
| 1014 | std::cout << " in both spaces: " << f_value << std::endl; | ||
| 1015 | print_vector("x", x, nVars); | ||
| 1016 | print_vector("u", u, iq + 1); | ||
| 1017 | print_vector("r", r, iq + 1); | ||
| 1018 | print_vector("A", A, iq + 1); | ||
| 1019 | #endif | ||
| 1020 | |||
| 1021 | ✗ | if (t == t2) { | |
| 1022 | #ifdef TRACE_SOLVER | ||
| 1023 | std::cout << "Full step has taken " << t << std::endl; | ||
| 1024 | print_vector("x", x, nVars); | ||
| 1025 | #endif | ||
| 1026 | /* full step has taken */ | ||
| 1027 | /* add constraint ip to the active set*/ | ||
| 1028 | ✗ | if (!add_constraint(R, m_J, d, iq, R_norm)) { | |
| 1029 | ✗ | iaexcl(ip) = 0; | |
| 1030 | ✗ | delete_constraint(R, m_J, A, u, nEqCon, iq, ip); | |
| 1031 | #ifdef TRACE_SOLVER | ||
| 1032 | print_matrix("R", R, nVars); | ||
| 1033 | print_vector("A", A, iq); | ||
| 1034 | #endif | ||
| 1035 | ✗ | for (i = 0; i < nIneqCon; i++) iai(i) = i; | |
| 1036 | ✗ | for (i = 0; i < iq; i++) { | |
| 1037 | ✗ | A(i) = A_old(i); | |
| 1038 | ✗ | iai(A(i)) = -1; | |
| 1039 | ✗ | u(i) = u_old(i); | |
| 1040 | } | ||
| 1041 | ✗ | x = x_old; | |
| 1042 | STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C); | ||
| 1043 | ✗ | goto l2; /* go to step 2 */ | |
| 1044 | } else | ||
| 1045 | ✗ | iai(ip) = -1; | |
| 1046 | #ifdef TRACE_SOLVER | ||
| 1047 | print_matrix("R", R, nVars); | ||
| 1048 | print_vector("A", A, iq); | ||
| 1049 | #endif | ||
| 1050 | STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C); | ||
| 1051 | ✗ | goto l1; | |
| 1052 | } | ||
| 1053 | |||
| 1054 | /* a partial step has been taken => drop constraint l */ | ||
| 1055 | ✗ | iai(l) = l; | |
| 1056 | ✗ | delete_constraint(R, m_J, A, u, nEqCon, iq, l); | |
| 1057 | ✗ | s(ip) = CI.row(ip).dot(x) + ci0(ip); | |
| 1058 | |||
| 1059 | #ifdef TRACE_SOLVER | ||
| 1060 | std::cout << "Partial step has taken " << t << std::endl; | ||
| 1061 | print_vector("x", x, nVars); | ||
| 1062 | print_matrix("R", R, nVars); | ||
| 1063 | print_vector("A", A, iq); | ||
| 1064 | print_vector("s", s, nIneqCon); | ||
| 1065 | #endif | ||
| 1066 | STOP_PROFILER_EIQUADPROG_FAST(EIQUADPROG_FAST_STEP_2C); | ||
| 1067 | |||
| 1068 | ✗ | goto l2a; | |
| 1069 | } | ||
| 1070 | |||
| 1071 | } /* namespace solvers */ | ||
| 1072 | } /* namespace tsid */ | ||
| 1073 |