forked from RobotLocomotion/drake
-
Notifications
You must be signed in to change notification settings - Fork 0
/
gurobi_lcp.m
93 lines (78 loc) · 2.47 KB
/
gurobi_lcp.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
function z = gurobi_lcp(M, q, l)
% Solve a Linear Complimentarity Problem as a mixed-integer linear program using Gurobi.
% This is meant to be a drop-in replacement for pathlcp(M, q, l). For the full documentation
% see pathlcp.m
% In general, this function is ~10x slower than pathlcp, but it does provide a guarantee
% of global optimality, subject to numerical precision issues.
% WARNING: in order to produce a well-scaled formulation, we hard-limit z and (Mz + q) to
% be less than a predefined constant called 'big'.
% checkDependency('gurobi');
big = 1e2;
DEBUG = false;
persistent x_seed;
n = size(M, 2);
nv = n * 3;
z_ndx = 1:n;
z_abs_ndx = n + (1:n);
z_zero_ndx = 2*n + (1:n);
lb = [l; zeros(n, 1); zeros(n, 1)];
ub = [inf + zeros(2*n, 1); ones(n, 1)];
vtype = [repmat('C', 2*n, 1); repmat('B', n, 1)];
A = zeros(5*n, nv);
b = zeros(size(A, 1), 1);
offset = 0;
% z_abs >= abs(z)
% z_abs >= z, z_abs >= -z
A(offset+(1:n), z_ndx) = eye(n);
A(offset+(1:n), z_abs_ndx) = -eye(n);
offset = offset + n;
A(offset+(1:n), z_ndx) = -eye(n);
A(offset+(1:n), z_abs_ndx) = -eye(n);
offset = offset + n;
% z_zero -> z == l
% z - l <= 1e6 * (1-z_zero)
A(offset+(1:n), z_ndx) = eye(n);
A(offset+(1:n), z_zero_ndx) = big * eye(n);
b(offset+(1:n)) = big + l;
% -inf as a lower bound creates a special case in which z_zero must be false
inf_mask = isinf(l);
inf_idx = find(inf_mask);
A(offset+inf_idx, :) = 0;
b(offset+inf_idx) = 0;
ub(z_zero_ndx(inf_mask)) = 0;
offset = offset + n;
% Mz + q >= 0
A(offset+(1:n), z_ndx) = -M;
b(offset+(1:n)) = q;
offset = offset + n;
% ~z_zero -> Mz + q == 0
% Mz + q <= 1e6 * z_zero
A(offset+(1:n), z_ndx) = M;
A(offset+(1:n), z_zero_ndx) = -big * eye(n);
% again, -inf as a lower bound creates a special case where z_zero is always false
A(offset+inf_idx, z_zero_ndx) = 0;
b(offset+(1:n)) = -q;
offset = offset + n;
if DEBUG
assert(offset == size(A, 1));
end
c = zeros(nv, 1);
c(z_abs_ndx) = 1;
model = struct('A', sparse(A), 'rhs', b, 'sense', '<', 'obj', c, 'lb', lb, 'ub', ub, 'vtype', vtype);
if ~isempty(x_seed) && length(x_seed) == length(lb)
model.start = x_seed;
end
params = struct('outputflag', 0, 'Threads', 1);%, 'IntFeasTol', 1e-5, 'FeasibilityTol', 1e-6);
result = gurobi(model, params);
z = result.x(z_ndx);
x_seed = result.x;
if DEBUG
try
assert(all(z >= l - 1e-4));
assert(all(M * z + q >= -1e-3), sprintf('%f', min(M * z + q)));
assert(all((abs(z - l) <= 1e-3) | (abs(M * z + q) <= 1e-3)));
catch e
e.getReport()
keyboard();
end
end