1 /*
2 * ECOS - Embedded Conic Solver.
3 * Copyright (C) 2012-2015 A. Domahidi [domahidi@embotech.com],
4 * Automatic Control Lab, ETH Zurich & embotech GmbH, Zurich, Switzerland.
5 *
6 * This program is free software: you can redistribute it and/or modify
7 * it under the terms of the GNU 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 * This program 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 General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with this program. If not, see <http://www.gnu.org/licenses/>.
18 */
19
20
21 /* The KKT module.
22 * Handles all computation related to KKT matrix:
23 * - updating the matrix
24 * - its factorization
25 * - solving for search directions
26 * - etc.
27 */
28
29 #include "kkt.h"
30 #include "ldl.h"
31 #include "splamm.h"
evalExpHessian(pfloat * w,pfloat * v,pfloat mu)32 #include "ecos.h"
33 #include "cone.h"
34
35 #include <math.h>
36
37 /* Factorization of KKT matrix. Just a wrapper for some LDL code */
38 #if PROFILING > 1
39 idxint kkt_factor(kkt* KKT, pfloat eps, pfloat delta, pfloat *t1, pfloat* t2)
40 #else
41 idxint kkt_factor(kkt* KKT, pfloat eps, pfloat delta)
42 #endif
43 {
44 idxint nd;
45
46 /* returns n if successful, k if D (k,k) is zero */
47 nd = LDL_numeric2(
48 KKT->PKPt->n, /* K and L are n-by-n, where n >= 0 */
49 KKT->PKPt->jc, /* input of size n+1, not modified */
50 KKT->PKPt->ir, /* input of size nz=Kjc[n], not modified */
51 KKT->PKPt->pr, /* input of size nz=Kjc[n], not modified */
52 KKT->L->jc, /* input of size n+1, not modified */
53 KKT->Parent, /* input of size n, not modified */
54 KKT->Sign, /* input, permuted sign vector for regularization */
55 eps, /* input, inverse permutation vector */
56 delta, /* size of dynamic regularization */
57 KKT->Lnz, /* output of size n, not defn. on input */
58 KKT->L->ir, /* output of size lnz=Lp[n], not defined on input */
59 KKT->L->pr, /* output of size lnz=Lp[n], not defined on input */
60 KKT->D, /* output of size n, not defined on input */
61 KKT->work1, /* workspace of size n, not defn. on input or output */
62 KKT->Pattern, /* workspace of size n, not defn. on input or output */
63 KKT->Flag /* workspace of size n, not defn. on input or output */
64 #if PROFILING > 1
65 , t1, t2
66 #endif
67 );
68
69 return nd == KKT->PKPt->n ? KKT_OK : KKT_PROBLEM;
70 }
71
evalBarrierValue(pfloat * siter,pfloat * ziter,idxint fc,idxint nexc)72
73 /**
74 * Solves the permuted KKT system and returns the unpermuted search directions.
75 *
76 * On entry, the factorization of the permuted KKT matrix, PKPt,
77 * is assumed to be up to date (call kkt_factor beforehand to achieve this).
78 * The right hand side, Pb, is assumed to be already permuted.
79 *
80 * On exit, the resulting search directions are written into dx, dy and dz,
81 * where these variables are permuted back to the original ordering.
82 *
83 * KKT->nitref iterative refinement steps are applied to solve the linear system.
84 *
85 * Returns the number of iterative refinement steps really taken.
86 */
87 idxint kkt_solve(kkt* KKT, spmat* A, spmat* G, pfloat* Pb, pfloat* dx, pfloat* dy, pfloat* dz, idxint n, idxint p, idxint m, cone* C, idxint isinit, idxint nitref)
88 {
89
90 #if CONEMODE == 0
91 #define MTILDE (m+2*C->nsoc)
92 #else
93 #define MTILDE (m)
94 #endif
95
96 idxint i, k, l, j, kk, kItRef;
97 #if (defined STATICREG) && (STATICREG > 0)
98 idxint dzoffset;
99 #endif
100 idxint* Pinv = KKT->Pinv;
101 pfloat* Px = KKT->work1;
102 pfloat* dPx = KKT->work2;
103 pfloat* e = KKT->work3;
104 pfloat* Pe = KKT->work4;
105 pfloat* truez = KKT->work5;
106 pfloat* Gdx = KKT->work6;
107 pfloat* ex = e;
108 pfloat* ey = e + n;
109 pfloat* ez = e + n+p;
110 pfloat bnorm = 1.0 + norminf(Pb, n+p+MTILDE);
111 pfloat nex = 0;
112 pfloat ney = 0;
113 pfloat nez = 0;
114 pfloat nerr;
115 pfloat nerr_prev = (pfloat)ECOS_NAN;
116 pfloat error_threshold = bnorm*LINSYSACC;
117 idxint nK = KKT->PKPt->n;
118
119 /* forward - diagonal - backward solves: Px holds solution */
120 LDL_lsolve2(nK, Pb, KKT->L->jc, KKT->L->ir, KKT->L->pr, Px );
121 LDL_dsolve(nK, Px, KKT->D);
122 LDL_ltsolve(nK, Px, KKT->L->jc, KKT->L->ir, KKT->L->pr);
123
124 #if PRINTLEVEL > 2
125 if( p > 0 ){
126 PRINTTEXT("\nIR: it ||ex|| ||ey|| ||ez|| (threshold: %4.2e)\n", error_threshold);
127 PRINTTEXT(" --------------------------------------------------\n");
128 } else {
129 PRINTTEXT("\nIR: it ||ex|| ||ez|| (threshold: %4.2e)\n", error_threshold);
130 PRINTTEXT(" -----------------------------------------\n");
131 }
132 #endif
133
134 /* iterative refinement */
135 for( kItRef=0; kItRef <= nitref; kItRef++ ){
136
137 /* unpermute x & copy into arrays */
138 unstretch(n, p, C, Pinv, Px, dx, dy, dz);
139
140 /* compute error term */
141 k=0; j=0;
142
143 /* 1. error on dx*/
144 #if (defined STATICREG) && (STATICREG > 0)
145 /* ex = bx - A'*dy - G'*dz - DELTASTAT*dx */
146 for( i=0; i<n; i++ ){ ex[i] = Pb[Pinv[k++]] - DELTASTAT*dx[i]; }
147 #else
148 /* ex = bx - A'*dy - G'*dz */
149 for( i=0; i<n; i++ ){ ex[i] = Pb[Pinv[k++]]; }
150 #endif
151 if(A) sparseMtVm(A, dy, ex, 0, 0);
152 sparseMtVm(G, dz, ex, 0, 0);
153 nex = norminf(ex,n);
154
155 /* error on dy */
156 if( p > 0 ){
157 #if (defined STATICREG) && (STATICREG > 0)
158 /* ey = by - A*dx + DELTASTAT*dy */
159 for( i=0; i<p; i++ ){ ey[i] = Pb[Pinv[k++]] + DELTASTAT*dy[i]; }
160 #else
161 /* ey = by - A*dx */
162 for( i=0; i<p; i++ ){ ey[i] = Pb[Pinv[k++]]; }
163 #endif
164 sparseMV(A, dx, ey, -1, 0);
165 ney = norminf(ey,p);
166 }
167
168
169 /* --> 3. ez = bz - G*dx + V*dz_true */
170 kk = 0; j=0;
171 #if (defined STATICREG) && (STATICREG > 0)
172 dzoffset=0;
173 #endif
174 sparseMV(G, dx, Gdx, 1, 1);
175 for( i=0; i<C->lpc->p; i++ ){
176 #if (defined STATICREG) && (STATICREG > 0)
177 ez[kk++] = Pb[Pinv[k++]] - Gdx[j++] + DELTASTAT*dz[dzoffset++];
178 #else
179 ez[kk++] = Pb[Pinv[k++]] - Gdx[j++];
180 #endif
181 }
182 for( l=0; l<C->nsoc; l++ ){
183 for( i=0; i<C->soc[l].p; i++ ){
184 #if (defined STATICREG) && (STATICREG > 0)
185 ez[kk++] = i<(C->soc[l].p-1) ? Pb[Pinv[k++]] - Gdx[j++] + DELTASTAT*dz[dzoffset++] : Pb[Pinv[k++]] - Gdx[j++] - DELTASTAT*dz[dzoffset++];
186 #else
187 ez[kk++] = Pb[Pinv[k++]] - Gdx[j++];
188 #endif
189 }
190 #if CONEMODE == 0
191 ez[kk] = 0;
192 ez[kk+1] = 0;
193 k += 2;
194 kk += 2;
195 #endif
196 }
197 #ifdef EXPCONE
198 for(l=0; l<C->nexc; l++)
199 {
200 for(i=0;i<3;i++)
201 {
202 #if (defined STATICREG) && (STATICREG > 0)
203 ez[kk++] = Pb[Pinv[k++]] - Gdx[j++] + DELTASTAT*dz[dzoffset++];
204 #else
205 ez[kk++] = Pb[Pinv[k++]] - Gdx[j++];
206 #endif
207 }
208 }
209 #endif
210 for( i=0; i<MTILDE; i++) { truez[i] = Px[Pinv[n+p+i]]; }
211 if( isinit == 0 ){
212 scale2add(truez, ez, C);
213 } else {
214 vadd(MTILDE, truez, ez);
215 }
216 nez = norminf(ez,MTILDE);
217
218
219 #if PRINTLEVEL > 2
220 if( p > 0 ){
221 PRINTTEXT(" %2d %3.1e %3.1e %3.1e\n", (int)kItRef, nex, ney, nez);
222 } else {
223 PRINTTEXT(" %2d %3.1e %3.1e\n", (int)kItRef, nex, nez);
224 }
225 #endif
226
227 /* maximum error (infinity norm of e) */
228 nerr = MAX( nex, nez);
229 if( p > 0 ){ nerr = MAX( nerr, ney ); }
230
231 /* CHECK WHETHER REFINEMENT BROUGHT DECREASE - if not undo and quit! */
232 if( kItRef > 0 && nerr > nerr_prev ){
233 /* undo refinement */
234 for( i=0; i<nK; i++ ){ Px[i] -= dPx[i]; }
235 kItRef--;
236 break;
237 }
238
239 /* CHECK WHETHER TO REFINE AGAIN */
240 if( kItRef == nitref || ( nerr < error_threshold ) || ( kItRef > 0 && nerr_prev < IRERRFACT*nerr ) ){
241 break;
242 }
243 nerr_prev = nerr;
244
245 /* permute */
246 for( i=0; i<nK; i++) { Pe[Pinv[i]] = e[i]; }
247
248 /* forward - diagonal - backward solves: dPx holds solution */
249 LDL_lsolve2(nK, Pe, KKT->L->jc, KKT->L->ir, KKT->L->pr, dPx);
250 LDL_dsolve(nK, dPx, KKT->D);
251 LDL_ltsolve(nK, dPx, KKT->L->jc, KKT->L->ir, KKT->L->pr);
252
253 /* add refinement to Px */
254 for( i=0; i<nK; i++ ){ Px[i] += dPx[i]; }
255 }
256
257 #if PRINTLEVEL > 2
258 PRINTTEXT("\n");
259 #endif
260
261 /* copy solution out into the different arrays, permutation included */
262 unstretch(n, p, C, Pinv, Px, dx, dy, dz);
263
264 return kItRef;
265 }
266
267
268 /**
269 * Updates the permuted KKT matrix by copying in the new scalings.
270 */
271 void kkt_update(spmat* PKP, idxint* P, cone *C)
272 {
273 idxint i, j, k, conesize;
274 pfloat eta_square, *q;
275 #if CONEMODE == 0
276 pfloat d1, u0, u1, v1;
277 idxint conesize_m1;
278 #else
279 pfloat a, w, c, d, eta_square_d, qj;
280 idxint thiscolstart;
281 #endif
282
283 /* LP cone */
284 for( i=0; i < C->lpc->p; i++ ){ PKP->pr[P[C->lpc->kkt_idx[i]]] = -C->lpc->v[i] - DELTASTAT; }
285
286 /* Second-order cone */
287 for( i=0; i<C->nsoc; i++ ){
288
289 #if CONEMODE == 0
290 getSOCDetails(&C->soc[i], &conesize, &eta_square, &d1, &u0, &u1, &v1, &q);
291 conesize_m1 = conesize - 1;
292
293 /* D */
294 PKP->pr[P[C->soc[i].Didx[0]]] = -eta_square * d1 - DELTASTAT;
295 for (k=1; k < conesize; k++) {
296 PKP->pr[P[C->soc[i].Didx[k]]] = -eta_square - DELTASTAT;
297 }
298
299 /* v */
300 j=1;
301 for (k=0; k < conesize_m1; k++) {
302 PKP->pr[P[C->soc[i].Didx[conesize_m1] + j++]] = -eta_square * v1 * q[k];
303 }
304 PKP->pr[P[C->soc[i].Didx[conesize_m1] + j++]] = -eta_square;
305
306 /* u */
307 PKP->pr[P[C->soc[i].Didx[conesize_m1] + j++]] = -eta_square * u0;
308 for (k=0; k < conesize_m1; k++) {
309 PKP->pr[P[C->soc[i].Didx[conesize_m1] + j++]] = -eta_square * u1 * q[k];
310 }
311 PKP->pr[P[C->soc[i].Didx[conesize_m1] + j++]] = +eta_square + DELTASTAT;
312 #endif
313
314 #if CONEMODE > 0
315 conesize = C->soc[i].p;
316 eta_square = C->soc[i].eta_square;
317 a = C->soc[i].a;
318 w = C->soc[i].w;
319 c = C->soc[i].c;
320 d = C->soc[i].d;
321 q = C->soc[i].q;
322 eta_square_d = eta_square * d;
323
324 /* first column - only diagonal element */
325 PKP->pr[P[C->soc[i].colstart[0]]] = -eta_square * (a*a + w);
326
327 /* next conesize-1 columns */
328 for (j=1; j<conesize; j++) {
329
330 thiscolstart = C->soc[i].colstart[j];
331
332 /* first element in column (=c*q) */
333 qj = q[j-1];
334 PKP->pr[P[thiscolstart]] = -eta_square * c * qj;
335
336 /* the rest of the column (=I + d*qq') */
337 for (k=1; k<j; k++) {
338 PKP->pr[P[thiscolstart+k]] = -eta_square_d * q[k-1]*qj; /* super-diagonal elements */
339 }
340 PKP->pr[P[thiscolstart+j]] = -eta_square * (1.0 + d * qj*qj); /* diagonal element */
341 }
342 #endif
343 }
344 #if defined EXPCONE
345 /* Exponential cones */
346 for( i=0; i < C->nexc; i++){
347 PKP->pr[P[C->expc[i].colstart[0]]] = -C->expc[i].v[0]-DELTASTAT;
348 PKP->pr[P[C->expc[i].colstart[1]]] = -C->expc[i].v[1];
349 PKP->pr[P[C->expc[i].colstart[1]+1]] = -C->expc[i].v[2]-DELTASTAT;
350 PKP->pr[P[C->expc[i].colstart[2]]] = -C->expc[i].v[3];
351 PKP->pr[P[C->expc[i].colstart[2]+1]] = -C->expc[i].v[4];
352 PKP->pr[P[C->expc[i].colstart[2]+2]] = -C->expc[i].v[5]-DELTASTAT;
353 }
354 #endif
355
356
357 }
358
359
360
361 /**
362 * Initializes the (3,3) block of the KKT matrix to produce the matrix
363 *
364 * [0 A' G']
365 * K = [A 0 0 ]
366 * [G 0 -I ]
367 *
368 * It is assumed that the A,G have been already copied in appropriately,
369 * and that enough memory has been allocated (this is done in preproc.c module).
370 *
371 * Note that the function works on the permuted KKT matrix.
372 */
373 void kkt_init(spmat* PKP, idxint* P, cone *C)
374 {
375 idxint i, j, k, conesize;
376 pfloat eta_square, *q;
377 #if CONEMODE == 0
378 pfloat d1, u0, u1, v1;
379 idxint conesize_m1;
380 #else
381 pfloat a, w, c, d, eta_square_d, qj;
382 idxint thiscolstart;
383 #endif
384
385 /* LP cone */
386 for( i=0; i < C->lpc->p; i++ ){ PKP->pr[P[C->lpc->kkt_idx[i]]] = -1.0; }
387
388 /* Second-order cone */
389 for( i=0; i<C->nsoc; i++ ){
390
391 #if CONEMODE == 0
392 getSOCDetails(&C->soc[i], &conesize, &eta_square, &d1, &u0, &u1, &v1, &q);
393 conesize_m1 = conesize - 1;
394
395 /* D */
396 PKP->pr[P[C->soc[i].Didx[0]]] = -1.0;
397 for (k=1; k < conesize; k++) {
398 PKP->pr[P[C->soc[i].Didx[k]]] = -1.0;
399 }
400
401 /* v */
402 j=1;
403 for (k=0; k < conesize_m1; k++) {
404 PKP->pr[P[C->soc[i].Didx[conesize_m1] + j++]] = 0.0;
405 }
406 PKP->pr[P[C->soc[i].Didx[conesize_m1] + j++]] = -1.0;
407
408 /* u */
409 PKP->pr[P[C->soc[i].Didx[conesize_m1] + j++]] = 0.0;
410 for (k=0; k < conesize_m1; k++) {
411 PKP->pr[P[C->soc[i].Didx[conesize_m1] + j++]] = 0.0;
412 }
413 PKP->pr[P[C->soc[i].Didx[conesize_m1] + j++]] = +1.0;
414 #endif
415
416 #if CONEMODE > 0
417 conesize = C->soc[i].p;
418 eta_square = C->soc[i].eta_square;
419 a = C->soc[i].a;
420 w = C->soc[i].w;
421 c = C->soc[i].c;
422 d = C->soc[i].d;
423 q = C->soc[i].q;
424 eta_square_d = eta_square * d;
425
426 /* first column - only diagonal element */
427 PKP->pr[P[C->soc[i].colstart[0]]] = -1.0;
428
429 /* next conesize-1 columns */
430 for (j=1; j<conesize; j++) {
431
432 thiscolstart = C->soc[i].colstart[j];
433
434 /* first element in column (=c*q) */
435 qj = q[j-1];
436 PKP->pr[P[thiscolstart]] = 0.0;
437
438 /* the rest of the column (=I + d*qq') */
439 for (k=1; k<j; k++) {
440 PKP->pr[P[thiscolstart+k]] = 0.0; /* super-diagonal elements */
441 }
442 PKP->pr[P[thiscolstart+j]] = -1.0; /* diagonal element */
443 }
444 #endif
445 }
446 }
447