fix_sparsity.py 5.79 KB
 stevet committed Oct 14, 2019 1 2 3 ``````import numpy as np `````` Pierre Fernbach committed Oct 15, 2019 4 5 6 ``````from sl1m.constants_and_tools import * from sl1m import planner_l1 as pl1 from sl1m import planner as pl `````` stevet committed Oct 14, 2019 7 `````` `````` stevet committed Oct 28, 2019 8 ``````import qp `````` stevet committed Oct 14, 2019 9 10 `````` `````` stevet committed Oct 28, 2019 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 committed Oct 14, 2019 17 `````` `````` stevet committed Oct 28, 2019 18 19 ``````except ImportError: pass `````` stevet committed Oct 14, 2019 20 21 22 23 24 25 `````` from time import clock np.set_printoptions(formatter={'float': lambda x: "{0:0.1f}".format(x)}) `````` stevet committed Oct 28, 2019 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 committed Jan 31, 2020 30 ``````def solve(pb,surfaces, draw_scene = None, plot = True): `````` stevet committed Oct 14, 2019 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 committed Jan 31, 2020 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 committed Oct 14, 2019 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 committed Oct 28, 2019 54 `````` pl.plotQPRes(pb, res, ax=ax) `````` stevet committed Oct 14, 2019 55 `````` `````` daeun committed Jan 31, 2020 56 57 `````` return pb, res, timMs(t1,t3) # return pb, coms, footpos, allfeetpos, res `````` stevet committed Oct 14, 2019 58 59 `````` `````` stevet committed Oct 28, 2019 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 committed Jan 31, 2020 67 68 69 70 `````` try: res = qp.quadprog_solve_qp(C, c,A,b,E,e) except: return 4, 4, 4 `````` stevet committed Oct 28, 2019 71 72 73 74 `````` ok = pl1.isSparsityFixed(pb, res) solutionIndices = None solutionComb = None `````` daeun committed Jan 31, 2020 75 76 `````` pbs = None `````` stevet committed Oct 28, 2019 77 78 79 80 81 `````` if not ok: pbs = pl1.generateAllFixedScenariosWithFixedSparsity(pb, res) t3 = clock() `````` daeun committed Jan 31, 2020 82 83 84 85 `````` if pbs == 1: print "CASE1: too big combinatorial" return 1, 1, 1 `````` stevet committed Oct 28, 2019 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 committed Feb 11, 2020 92 `````` if pl1.isSparsityFixed(pbComb, res): `````` stevet committed Oct 28, 2019 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 committed Jan 31, 2020 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 committed Oct 28, 2019 124 `````` `````` stevet committed Oct 14, 2019 125 `````` `````` stevet committed Oct 28, 2019 126 ``````############### MIXED-INTEGER SOLVER ############### `````` stevet committed Oct 14, 2019 127 128 129 130 `````` def tovals(variables): return array([el.value for el in variables]) `````` stevet committed Oct 28, 2019 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 `````` stevet committed Oct 16, 2019 136 137 `````` gurobipy.setParam('LogFile', '') gurobipy.setParam('OutputFlag', 0) `````` stevet committed Oct 14, 2019 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): `````` stevet committed Oct 16, 2019 162 `````` if i!= 0 and el - previousL > 2.: `````` stevet committed Oct 14, 2019 163 164 `````` assert len(currentSum) > 0 constraints = constraints + [sum(currentSum) == len(currentSum) -1 ] `````` stevet committed Jan 23, 2020 165 `````` currentSum = [boolvars[i]] `````` stevet committed Oct 14, 2019 166 167 168 `````` elif el !=0: currentSum = currentSum + [boolvars[i]] previousL = el `````` stevet committed Jan 23, 2020 169 170 171 `````` if len(currentSum) > 1: constraints = constraints + [sum(currentSum) == len(currentSum) -1 ] obj = cp.Minimize(ones(numSlackVariables) * boolvars) `````` stevet committed Oct 14, 2019 172 173 `````` prob = cp.Problem(obj, constraints) t1 = clock() `````` stevet committed Oct 16, 2019 174 `````` res = prob.solve(solver=cp.GUROBI, verbose=False ) `````` stevet committed Oct 14, 2019 175 176 177 `````` t2 = clock() res = tovals(varReal) print "time to solve MIP ", timMs(t1,t2) `````` stevet committed Oct 16, 2019 178 `````` `````` stevet committed Oct 14, 2019 179 `````` `````` stevet committed Oct 16, 2019 180 181 182 `````` plot = plot and draw_scene is not None if plot: ax = draw_scene(surfaces) `````` stevet committed Oct 28, 2019 183 `````` pl1.plotQPRes(pb, res, ax=ax) `````` stevet committed Oct 14, 2019 184 `````` `````` daeun committed Jan 31, 2020 185 186 `````` # return timMs(t1,t2) return pb, res, timMs(t1,t2) `````` stevet committed Oct 16, 2019 187 `` ``