-
Notifications
You must be signed in to change notification settings - Fork 7
/
ecosqp.m
260 lines (232 loc) · 6.78 KB
/
ecosqp.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
function [X,fval,exitflag,output,lambda,Tsolve,c,G,h,dims,Aeq,beq] = ecosqp(H,f,A,B,Aeq,Beq,lb,ub,opts)
% ECOSQP - solve quadratic program using the ECOS second-order cone solver.
% The interface mimics MATLAB's quadprog interface.
%
% X = ECOSQP(H,f,A,b) attempts to solve the quadratic program
%
% minimize 1/2*x'*H*x + f'*x
% subject to A*x <= b
%
%
% X = ECOSQP(H,f,A,b,Aeq,beq) attempts to solve the QP from above
% with additional equality constraints on variable x:
%
% minimize 1/2*x'*H*x + f'*x
% subject to A*x <= b
% Aeq*x = beq
%
%
% X = ECOSQP(H,f,A,b,Aeq,beq,LB,UB) attempts to solve the problem with
% lower and upper bounds on the design variables, X, so that the solution
% is in the range LB <= X <= UB. Use empty matrices for LB and UB if no
% bounds exist. Set LB(i) = -Inf if X(i) is unbounded below; set
% UB(i) = +Inf if X(i) is unbounded above.
%
% X = ECOSQP(...,OPTIONS) solves the QP as above and uses the settings as
% defined in the OPTIONS structure. See ECOSOPTIMSET for more information
% on how to set settings in ECOS.
%
% [X,FVAL] = ECOSQP(...) returns in addition to the minimizer X also
% the optimal function value of the QP.
%
% [X,FVAL,EXITFLAG] = ECOSQP(...) returns the exitflag with the following
% meaning:
%
% 1 KKT optimality conditions satisfied - optimal solution found.
% 0 Maximum number of iterations exceeded.
% -2 Problem is (primal) infeasible.
% -3 Problem is unbounded (dual infeasible).
%
%
% [X,FVAL,EXITFLAG,INFO] = ECOSQP(...) returns in addition the
% INFO struct as returned for the converted problem by ECOS. See the ECOS
% documentation for more help on the particular fields of the INFO
% struct.
%
% [X,FVAL,EXITFLAG,INFO,TSOLVE] = ECOSQP(...) returns the runtime for
% solving the converted problem as returned by ECOS. Note that this can
% be different from a "tic; ecosqp(...); toc;" since an initial "square
% root factorization" of the Hessian H is to be performed first.
%
%
% This file is an interface for ECOS, and rewrites the QP as a second-order
% cone program (SOCP) that can be solved by ECOS. The rewriting occurs
% through an epigraph reformulation of the objective function,
%
% || Wx ||
% || (t - f'*x + 1)/sqrt(2)||_2 <= (t-f'*x-1)/sqrt(2) <==> 1/2*x'*H*x + f'*x <= t,
%
% assuming that the Hessian H is positive definite, and therefore admits a
% Cholesky decomposition H = W'*W. The following problem is solved when
% calling ECOS after the rewriting:
%
% minimize t
% subject to A*x <= b
% [ lb <= x <= ub ]
% [ Aeq*x == beq ]
%
% || Wx ||
% || (t-f'*x+1)/sqrt(2) ||_2 <= (t-f'*x-1)/sqrt(2)
%
%
% If the Hessian H is positive semi-definite, an eigenvalue decomposition
% is computed to obtain W=H^(1/2). This might be slow for large matrices,
% but work well for diagonal Hessians. Eigenvalues which are zero are
% discarded from the computation of W.
%
%
% See also ECOS ECOSOPTIMSET QUADPROG ECOS_LICENSE
%
% (c) A. Domahidi, ETH Zurich & embotech GmbH, Zurich, Switzerland, 2012-15.
%% dimension and argument checking
if( isempty(H) )
error('Quadratic programming requires a non-empty, non-zero Hessian');
end
% number of variables
n = size(H,2);
% check size of cost terms
assert(size(H,1)==n,'Hessian must be a square matrix');
if( isempty(f) )
f = zeros(n,1);
else
assert( size(f,1)==n,sprintf('Linear term f must be a column vector of length %d',n));
assert( size(f,2)==1,'Linear term f must be a column vector');
end
if( nargin < 8 )
ub = [];
end
if( nargin < 7 )
lb = [];
end
if( nargin == 7 && isstruct(lb) )
opts = lb;
lb = [];
end
if( nargin == 8 && isstruct(ub) )
opts = ub;
ub = [];
end
if( nargin < 5 )
Aeq = [];
Beq = [];
end
if( nargin == 5 )
if( ~isstruct(Aeq) )
error('Either Aeq or an options struct expected as fifth argument');
else
opts = Aeq;
Aeq = [];
end
end
if( nargin < 3 )
A = [];
B = [];
end
if( ~exist('opts','var') )
opts.verbose = 1;
end
%% display that we can begin with conversion
if( opts.verbose > 0 )
disp('ECOSQP: Converting QP to SOCP...');
end
%% precondition
scale = max(abs(f));
scale = 1;
f = f./scale;
H = H./scale;
%% check if Cholesky decomposition of H exists,
% i.e. whether we have a positive definite Hessian
assert( ~isempty(H),'Quadratic programming requires a Hessian.');
try
tic
W = chol(H,'upper');
if( opts.verbose > 0 )
fprintf('ECOSQP: Time for Cholesky: %4.2f seconds\n', toc);
end
catch %#ok<CTCH>
warning('Hessian not positive definite, using sqrt(H) instead of chol');
W = sqrt(H);
k = 0;
eliminateIdx = [];
for i = 1:size(W,1)
if( all(W(i,:) == 0) )
k = k+1;
eliminateIdx(k) = i; %#ok<AGROW>
end
end
W(eliminateIdx,:) = [];
if( opts.verbose > 0 ), fprintf('%d zero rows in square root of Hessian eliminated\n',k-1); end
end
%% set up SOCP problem
% The new variable is stacked as [x, t]
c = [zeros(n,1); 1];
% upper bounds
if( exist('ub','var') && ~isempty(ub) )
% find indices which are upper bounded
Aub = speye(n);
Aub(isinf(ub),:) = [];
Bub = ub( ~isinf(ub) );
A = [A; Aub];
B = [B; Bub];
end
% lower bounds
if( exist('lb','var') && ~isempty(lb) )
% find indices which are lower bounded
Alb = -speye(n);
Alb(isinf(lb),:) = [];
Blb = -lb( ~isinf(lb) );
A = [A; Alb];
B = [B; Blb];
end
% pad Aeq with a zero column for t
if( ~isempty(Aeq) )
Aeq = [Aeq, zeros(size(Aeq,1),1)];
beq = Beq;
else
Aeq = [];
beq = [];
end
% create second-order cone constraint for objective function
fhalf = f./sqrt(2);
zerocolumn = zeros(size(W,1),1);
Gquad = [fhalf', -1/sqrt(2);
-W, zerocolumn;
-fhalf', +1/sqrt(2)];
hquad = [1/sqrt(2); zerocolumn; 1/sqrt(2)];
if( isempty(A) )
G = Gquad;
h = hquad;
dims.l = 0;
dims.q = size(W,1)+2;
else
G = [A, zeros(size(A,1),1); Gquad];
h = [B; hquad];
dims.l = size(A,1);
dims.q = size(W,1)+2;
end
%% sparsify
G = sparse(G);
Aeq = sparse(Aeq);
%% solve
if( opts.verbose > 0 ), fprintf('Conversion completed. Calling ECOS...\n'); end
if( isempty(Aeq) )
save noscale c G h dims opts
[x,y,info,s,z] = ecos(c,G,h,dims,opts); %#ok<ASGLU>
else
[x,y,info,s,z] = ecos(c,G,h,dims,Aeq,beq,opts); %#ok<ASGLU>
end
%% prepare return variables
X = x(1:n)*scale;
fval = x(end);
switch( info.exitflag )
case 1, exitflag = -2;
case 2, exitflag = -3;
case 0, exitflag = 1;
otherwise, exitflag = -100;
end
output.statusstring = info.infostring;
output.iterations = info.iter;
output.time = info.timing.runtime;
lambda = [z(1:dims.l); y];
Tsolve = info.timing.runtime;
output.ecosinfo = info;