fix_sparsity.py 5.79 KB
Newer Older
stevet's avatar
stevet committed
1
2
3
import numpy as np


4
5
6
from sl1m.constants_and_tools import *
from sl1m import planner_l1 as pl1
from sl1m import planner    as pl
stevet's avatar
stevet committed
7

stevet's avatar
stevet committed
8
import qp
stevet's avatar
stevet committed
9
10


stevet's avatar
stevet committed
11
12
13
14
15
16
# try to import mixed integer solver
MIP_OK = False  
try:
    import gurobipy
    import cvxpy as cp
    MIP_OK = True
stevet's avatar
stevet committed
17

stevet's avatar
stevet committed
18
19
except ImportError:
    pass
stevet's avatar
stevet committed
20
21
22
23
24
25


from time import clock

np.set_printoptions(formatter={'float': lambda x: "{0:0.1f}".format(x)})

stevet's avatar
stevet committed
26
27
28
29


### This solver is called when the sparsity is fixed. It assumes the first contact surface for each phase
### is the one used for contact creation.
daeun's avatar
daeun committed
30
def solve(pb,surfaces, draw_scene = None, plot = True):  
stevet's avatar
stevet committed
31
32
33
34
35
36
        
    t1 = clock()
    A, b, E, e = pl.convertProblemToLp(pb)    
    C = identity(A.shape[1])
    c = zeros(A.shape[1])
    t2 = clock()
daeun's avatar
daeun committed
37
38
39
40
41
42
    try: 
        res = qp.quadprog_solve_qp(C, c,A,b,E,e)    ######
        # res = qp.solve_lp(c,A,b,E,e)
    except:
        print "CASE3: turned out to be infeasible"
        return 3, 3, 3
stevet's avatar
stevet committed
43
44
45
46
47
48
49
50
51
52
53
    t3 = clock()
    
    print "time to set up problem" , timMs(t1,t2)
    print "time to solve problem"  , timMs(t2,t3)
    print "total time"             , timMs(t1,t3)
    
    coms, footpos, allfeetpos = pl.retrieve_points_from_res(pb, res)
    
    plot = plot and draw_scene is not None 
    if plot:
        ax = draw_scene(surfaces)
stevet's avatar
stevet committed
54
        pl.plotQPRes(pb, res, ax=ax)
stevet's avatar
stevet committed
55
    
daeun's avatar
daeun committed
56
57
    return pb, res, timMs(t1,t3)
    # return pb, coms, footpos, allfeetpos, res
stevet's avatar
stevet committed
58
59


stevet's avatar
stevet committed
60
61
62
63
64
65
66
### Calls the sl1m solver. Brute-forcedly tries to solve non fixed sparsity by handling the combinatorial.
### Ultimately calls solve which provides the approriate cost function
def solveL1(pb, surfaces, draw_scene = None, plot = True):     
    A, b, E, e = pl1.convertProblemToLp(pb)    
    C = identity(A.shape[1]) * 0.00001
    c = pl1.slackSelectionMatrix(pb)
        
daeun's avatar
daeun committed
67
68
69
70
    try:    
        res = qp.quadprog_solve_qp(C, c,A,b,E,e)
    except:
        return 4, 4, 4
stevet's avatar
stevet committed
71
72
73
74
        
    ok = pl1.isSparsityFixed(pb, res)
    solutionIndices = None
    solutionComb = None
daeun's avatar
daeun committed
75
76
    pbs = None
    
stevet's avatar
stevet committed
77
78
79
80
81
    if not ok:
        pbs = pl1.generateAllFixedScenariosWithFixedSparsity(pb, res)
        
        t3 = clock()
        
daeun's avatar
daeun committed
82
83
84
85
        if pbs == 1:
            print "CASE1: too big combinatorial"
            return 1, 1, 1
        
stevet's avatar
stevet committed
86
87
88
89
90
91
        for (pbComb, comb, indices) in pbs:
            A, b, E, e = pl1.convertProblemToLp(pbComb, convertSurfaces = False)
            C = identity(A.shape[1]) * 0.00001
            c = pl1.slackSelectionMatrix(pbComb)
            try:
                res = qp.quadprog_solve_qp(C, c,A,b,E,e)
daeun's avatar
daeun committed
92
                if pl1.isSparsityFixed(pbComb, res):       
stevet's avatar
stevet committed
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
                    coms, footpos, allfeetpos = pl1.retrieve_points_from_res(pbComb, res)
                    pb = pbComb
                    ok = True
                    solutionIndices = indices[:]
                    solutionComb = comb
                    if plot:
                        ax = draw_scene(surfaces)
                        pl1.plotQPRes(pb, res, ax=ax)
                    break
            except:
                print "unfeasible problem"
                pass
            
        t4 = clock()      
        
        print "time to solve combinatorial ", timMs(t3,t4)
    
    if ok:
        surfacesret, indices = pl1.bestSelectedSurfaces(pb, res)        
        for i, phase in enumerate(pb["phaseData"]): 
            phase["S"] = [surfaces[i][indices[i]]]
        if solutionIndices is not None:
            for i, idx in enumerate(solutionIndices):
                pb["phaseData"][idx]["S"] = [surfaces[idx][solutionComb[i]]]
        
daeun's avatar
daeun committed
118
119
120
121
122
123
        return solve (pb, surfaces, draw_scene, plot)  
    
    print "CASE2: combinatorials all sparsity not fixed"
    return 2, 2, 2   
        
        # return solve(pb,surfaces, draw_scene = draw_scene, plot = True )  
stevet's avatar
stevet committed
124

stevet's avatar
stevet committed
125

stevet's avatar
stevet committed
126
############### MIXED-INTEGER SOLVER ###############
stevet's avatar
stevet committed
127
128
129
130

def tovals(variables):
    return array([el.value for el in variables])

stevet's avatar
stevet committed
131
132
133
134
135
def solveMIP(pb, surfaces, MIP = True, draw_scene = None, plot = True):  
    if not MIP_OK:
        print "Mixed integer formulation requires gurobi packaged in cvxpy"
        raise ImportError
        
136
137
    gurobipy.setParam('LogFile', '')
    gurobipy.setParam('OutputFlag', 0)
stevet's avatar
stevet committed
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
       
    A, b, E, e = pl1.convertProblemToLp(pb)   
    slackMatrix = pl1.slackSelectionMatrix(pb)
    
    rdim = A.shape[1]
    varReal = cp.Variable(rdim)
    constraints = []
    constraintNormalIneq = A * varReal <= b
    constraintNormalEq   = E * varReal == e
    
    constraints = [constraintNormalIneq, constraintNormalEq]
    #creating boolean vars
    
    slackIndices = [i for i,el in enumerate (slackMatrix) if el > 0]
    numSlackVariables = len([el for el in slackMatrix if el > 0])
    boolvars = cp.Variable(numSlackVariables, boolean=True)    
    obj = cp.Minimize(slackMatrix * varReal)
    
    if MIP:    
        constraints = constraints + [varReal[el] <= 100. * boolvars[i] for i, el in enumerate(slackIndices)]   
    
        currentSum = []
        previousL = 0
        for i, el in enumerate(slackIndices):
162
            if i!= 0 and el - previousL > 2.:
stevet's avatar
stevet committed
163
164
                assert len(currentSum) > 0
                constraints = constraints + [sum(currentSum) == len(currentSum) -1 ]
165
                currentSum = [boolvars[i]]
stevet's avatar
stevet committed
166
167
168
            elif el !=0:
                currentSum = currentSum + [boolvars[i]]
            previousL  = el
169
170
171
        if len(currentSum) > 1:
            constraints = constraints + [sum(currentSum) == len(currentSum) -1 ]
    obj = cp.Minimize(ones(numSlackVariables) * boolvars)
stevet's avatar
stevet committed
172
173
    prob = cp.Problem(obj, constraints)
    t1 = clock()
174
    res = prob.solve(solver=cp.GUROBI, verbose=False )
stevet's avatar
stevet committed
175
176
177
    t2 = clock()
    res = tovals(varReal)
    print "time to solve MIP ", timMs(t1,t2)
178

stevet's avatar
stevet committed
179
    
180
181
182
    plot = plot and draw_scene is not None 
    if plot:
        ax = draw_scene(surfaces)
stevet's avatar
stevet committed
183
        pl1.plotQPRes(pb, res, ax=ax)
stevet's avatar
stevet committed
184
    
daeun's avatar
daeun committed
185
186
    # return timMs(t1,t2)
    return pb, res, timMs(t1,t2)
187