def test_model(): # create solver instance s = Model() # add some variables x = s.addVar("x", vtype = 'C', obj = 1.0) y = s.addVar("y", vtype = 'C', obj = 2.0) assert x.getObj() == 1.0 assert y.getObj() == 2.0 s.setObjective(4.0 * y + 10.5, clear = False) assert x.getObj() == 1.0 assert y.getObj() == 4.0 assert s.getObjoffset() == 10.5 # add some constraint c = s.addCons(x + 2 * y >= 1.0) assert c.isLinear() s.chgLhs(c, 5.0) s.chgRhs(c, 6.0) assert s.getLhs(c) == 5.0 assert s.getRhs(c) == 6.0 # solve problem s.optimize() solution = s.getBestSol() # print solution assert (s.getVal(x) == s.getSolVal(solution, x)) assert (s.getVal(y) == s.getSolVal(solution, y)) assert round(s.getVal(x)) == 5.0 assert round(s.getVal(y)) == 0.0 assert s.getSlack(c, solution) == 0.0 assert s.getSlack(c, solution, 'lhs') == 0.0 assert s.getSlack(c, solution, 'rhs') == 1.0 assert s.getActivity(c, solution) == 5.0 s.writeProblem('model') s.writeProblem('model.lp') s.freeProb() s = Model() x = s.addVar("x", vtype = 'C', obj = 1.0) y = s.addVar("y", vtype = 'C', obj = 2.0) c = s.addCons(x + 2 * y <= 1.0) s.setMaximize() s.delCons(c) s.optimize() assert s.getStatus() == 'unbounded'
def test_model(): # create solver instance s = Model() # add some variables x = s.addVar("x", vtype='C', obj=1.0) y = s.addVar("y", vtype='C', obj=2.0) assert x.getObj() == 1.0 assert y.getObj() == 2.0 s.setObjective(4.0 * y + 10.5, clear=False) assert x.getObj() == 1.0 assert y.getObj() == 4.0 assert s.getObjoffset() == 10.5 # add some constraint c = s.addCons(x + 2 * y >= 1.0) s.chgLhs(c, 5.0) s.chgRhs(c, 6.0) assert s.getLhs(c) == 5.0 assert s.getRhs(c) == 6.0 badsolution = s.createSol() s.setSolVal(badsolution, x, 2.0) s.setSolVal(badsolution, y, 2.0) assert s.getSlack(c, badsolution) == 0.0 assert s.getSlack(c, badsolution, 'lhs') == 1.0 assert s.getSlack(c, badsolution, 'rhs') == 0.0 assert s.getActivity(c, badsolution) == 6.0 s.freeSol(badsolution) # solve problem s.optimize() solution = s.getBestSol() # print solution assert (s.getVal(x) == s.getSolVal(solution, x)) assert (s.getVal(y) == s.getSolVal(solution, y)) assert round(s.getVal(x)) == 5.0 assert round(s.getVal(y)) == 0.0 assert s.getSlack(c, solution) == 0.0 assert s.getSlack(c, solution, 'lhs') == 0.0 assert s.getSlack(c, solution, 'rhs') == 1.0 assert s.getActivity(c, solution) == 5.0 s.freeProb() s = Model() x = s.addVar("x", vtype='C', obj=1.0) y = s.addVar("y", vtype='C', obj=2.0) c = s.addCons(x + 2 * y <= 1.0) s.setMaximize() s.delCons(c) s.optimize() assert s.getStatus() == 'unbounded'
def test_lp(): # create solver instance s = Model() # add some variables x = s.addVar("x", vtype='C', obj=1.0) y = s.addVar("y", vtype='C', obj=2.0) assert x.getObj() == 1.0 assert y.getObj() == 2.0 s.setObjective(4.0 * y, clear=False) assert x.getObj() == 1.0 assert y.getObj() == 4.0 # add some constraint c = s.addCons(x + 2 * y >= 1.0) s.chgLhs(c, 5.0) s.chgRhs(c, 5.0) # solve problem s.optimize() solution = s.getBestSol() # print solution assert (s.getVal(x) == s.getSolVal(solution, x)) assert (s.getVal(y) == s.getSolVal(solution, y)) assert round(s.getVal(x)) == 5.0 assert round(s.getVal(y)) == 0.0 s.freeProb() s = Model() x = s.addVar("x", vtype='C', obj=1.0) y = s.addVar("y", vtype='C', obj=2.0) c = s.addCons(x + 2 * y <= 1.0) s.setMaximize() s.delCons(c) s.optimize() assert s.getStatus() == 'unbounded'
def test_model(): # create solver instance s = Model() # test parameter methods pric = s.getParam('lp/pricing') s.setParam('lp/pricing', 'q') assert 'q' == s.getParam('lp/pricing') s.setParam('lp/pricing', pric) s.setParam('visual/vbcfilename', 'vbcfile') assert 'vbcfile' == s.getParam('visual/vbcfilename') assert 'lp/pricing' in s.getParams() s.setParams({'visual/vbcfilename': '-'}) assert '-' == s.getParam('visual/vbcfilename') # add some variables x = s.addVar("x", vtype='C', obj=1.0) y = s.addVar("y", vtype='C', obj=2.0) assert x.getObj() == 1.0 assert y.getObj() == 2.0 s.setObjective(4.0 * y + 10.5, clear=False) assert x.getObj() == 1.0 assert y.getObj() == 4.0 assert s.getObjoffset() == 10.5 # add some constraint c = s.addCons(x + 2 * y >= 1.0) assert c.isLinear() s.chgLhs(c, 5.0) s.chgRhs(c, 6.0) assert s.getLhs(c) == 5.0 assert s.getRhs(c) == 6.0 # solve problem s.optimize() solution = s.getBestSol() # print solution assert (s.getVal(x) == s.getSolVal(solution, x)) assert (s.getVal(y) == s.getSolVal(solution, y)) assert round(s.getVal(x)) == 5.0 assert round(s.getVal(y)) == 0.0 assert s.getSlack(c, solution) == 0.0 assert s.getSlack(c, solution, 'lhs') == 0.0 assert s.getSlack(c, solution, 'rhs') == 1.0 assert s.getActivity(c, solution) == 5.0 # check expression evaluations expr = x * x + 2 * x * y + y * y expr2 = x + 1 assert s.getVal(expr) == s.getSolVal(solution, expr) assert s.getVal(expr2) == s.getSolVal(solution, expr2) assert round(s.getVal(expr)) == 25.0 assert round(s.getVal(expr2)) == 6.0 s.writeProblem('model') s.writeProblem('model.lp') s.freeProb() s = Model() x = s.addVar("x", vtype='C', obj=1.0) y = s.addVar("y", vtype='C', obj=2.0) c = s.addCons(x + 2 * y <= 1.0) s.setMaximize() s.delCons(c) s.optimize() assert s.getStatus() == 'unbounded'
class PathSolver: def __init__(self, u_max, xInit, xT, A, B, horizon, margin=None, poly_nsides=50, horizon_cost=0.5): ''' Mixed integer programming based path planning ''' # Initialization parameters self.xInit = xInit self.xT = xT # create solver instance if usePuLP: self.prob = pulp.LpProblem("MILP Path Solver", pulp.LpMinimize) # Remove noOverlap to update the constraints on recursive calls self.prob.noOverlap = 0 else: self.prob = Model('Quadratic Path Solver') self.cnstMap = {} # Result x and y positions self.wayPoint = [] self.activePoints = [] self.activePtsIdx = [] self.activeObstIdx = [] self.initNames = [] self.nNodes = None # Receding horizon related parameter self.poly_nsides = poly_nsides self.receding_horizon = horizon self.horizon_cost = horizon_cost if self.receding_horizon: # Check for the range of horizon cost # ranges from [0,1] for computing a convex combination if ((self.horizon_cost < 0) or (self.horizon_cost > 1)): raise Exception( 'Horizon cost should be between 0 and 1 (Convex combination parameter)' ) else: self.horizon_cost = 0 # Local hidden variables self.__N = None self.__u_max = u_max self.u_max = u_max self.__A = A self.__B = B self.gap = None self.margin = margin if self.margin != None: if self.margin < 0: raise Exception('Margin should be positive') self.msg = 1 self.__nu = len(self.__B[0]) self.__nx = len(self.__A[0]) self.__M = 10000 self.__epsilon = 0.0001 self.__nObst = None self.__dObst = None self.__obstIdx = {} self.__active = None self.__zVal = [] self.__H = [] self.__V = None self.__G = [] self.__sol = [] return def __createVar(self): # Real variables if usePuLP: self.u = pulp.LpVariable.dicts("u", (range(self.__N), range(self.__nu)), \ -self.__u_max, self.__u_max) self.absu = pulp.LpVariable.dicts("absu", (range(self.__N), range(self.__nu)), \ self.__epsilon, self.__u_max) self.x = pulp.LpVariable.dicts( "x", (range(self.__N + 1), range(self.__nx))) if self.receding_horizon: # Define the distance between the last time step and goal self.dlast = pulp.LpVariable("dlast", lowBound=0) self.d = pulp.LpVariable.dicts("d", range(self.__N - 1), lowBound=0) else: self.d = pulp.LpVariable.dicts("d", range(self.__N), lowBound=0) self.dlast = None else: self.u = createDict(self.prob, self.__N, self.__nu, type_="CONTINUOUS", \ lb=-1*self.__u_max, ub=self.__u_max, str_="u") self.absu = createDict(self.prob, self.__N, self.__nu, type_="CONTINUOUS", \ lb=self.__epsilon, ub=self.__u_max, str_="absu") self.x = createDict(self.prob, self.__N + 1, self.__nx, type_="CONTINUOUS", str_="x") if self.receding_horizon: # Define the distance between the last time step and goal self.dlast = self.prob.addVar(name="dlast", lb=0) self.d = create1Dict(self.prob, self.__N - 1, str_="d", lb=0) else: self.d = create1Dict(self.prob, self.__N, str_="d", lb=0) self.dlast = None return def __addObjective(self): # Problem Objective # self.d == 0 when recending horizon is off if usePuLP: if not self.receding_horizon: self.prob += pulp.lpSum([self.d[i] for i in range(self.__N)]) else: self.prob += (1 - self.horizon_cost) * pulp.lpSum([ self.d[i] for i in range(self.__N - 1) ]) + self.horizon_cost * self.dlast else: if not self.receding_horizon: obj = 0 for i in range(self.__N): obj += self.d[i] self.prob.setObjective(obj) else: obj = 0 for i in range(self.__N): obj += self.d[i] self.prob.setObjective((1 - self.horizon_cost) * obj + self.horizon_cost * self.dlast) return def addAllZConstraints(self): for k in range(self.__N): for i in range(self.__nObst): self.prob.addConstraint(pulp.lpSum([self.z[k][i][j] for j in range(self.__dObst)]) == \ self.__dObst-1, name='z_%d_%d'%(k, i)) return def __addSCIPVarConstraint(self, zInit): ''' Add constraints on the variables (Includes state transition constraint) ''' # Constraints on state parameters # x[0] == xInit count = 0 for x_var, xi in zip(self.x[0].values(), self.xInit): self.prob.addCons(x_var == xi, name='constraint%d' % count) count = count + 1 for x_var, xi in zip(self.x[self.__N].values(), self.xT): self.prob.addCons(x_var == xi, name='constraint%d' % count) count = count + 1 # Constraints on intermediate variables if self.receding_horizon: limit = self.__N - 1 else: limit = self.__N for k in range(limit): # absu >= u # absu + u >= 0 for i in range(self.__nu): self.prob.addCons(self.absu[k][i] - self.u[k][i] >= 0, name='constraint%d' % count) count = count + 1 self.prob.addCons(self.absu[k][i] + self.u[k][i] >= 0, name='constraint%d' % count) count = count + 1 # State Transition modelled as a constraint # x[k+1] == A*x[k] + B*u[:,k] for x_var, a, b in zip(self.x[k + 1].values(), self.__A, self.__B): cns1 = 0 for ai, xi in zip(a, self.x[k].values()): cns1 += ai * xi cns2 = 0 for bi, ui in zip(b, self.u[k].values()): cns2 += bi * ui self.prob.addCons((x_var - cns1 - cns2) == 0, name='constraint%d' % count) count = count + 1 # Lower bound on the horizon radius if self.receding_horizon: self.prob.addCons(self.dlast >= 0, name='constraint%d' % count) count = count + 1 # Constraints the distrance between adjacent points for kk in range(1, self.__N + 1 - self.receding_horizon): currx = self.x[kk].values() prevx = self.x[kk - 1].values() self.prob.addCons(self.d[kk - 1] >= 0, name='constraint%d' % count) count = count + 1 for side in range(self.poly_nsides): # parameters that determine the side of the polygon line_angle = [math.cos(2*side*math.pi/self.poly_nsides), \ math.sin(2*side*math.pi/self.poly_nsides)] # Add constraints between the last step and the goal point # This ensures that the goal is within the horizon of the last step cns = 0 for m, x1, x2 in zip(line_angle, currx, prevx): cns += m * (x1 - x2) self.prob.addCons(cns <= self.d[kk - 1], name='constraint%d' % count) count = count + 1 if self.receding_horizon: xlaststep = self.x[self.__N - 1].values() xgoal = self.x[self.__N].values() for side in range(500): # parameters that determine the side of the polygon line_angle = [math.cos(2*side*math.pi/self.poly_nsides), \ math.sin(2*side*math.pi/self.poly_nsides)] # Add constraints between the last step and the goal point # This ensures that the goal is within the horizon of the last step cns = 0 for m, x1, x2 in zip(line_angle, xgoal, xlaststep): cns += m * (x1 - x2) self.prob.addCons(cns <= self.dlast, name='constraint%d' % count) count = count + 1 def __addVarConstraint(self, zInit): ''' Add constraints on the variables (Includes state transition constraint) ''' # Constraints on state parameters # x[0] == xInit for x_var, xi in zip(self.x[0].values(), self.xInit): self.prob.addConstraint(x_var == xi) for x_var, xi in zip(self.x[self.__N].values(), self.xT): self.prob.addConstraint(x_var == xi) if False: #if self.margin != None: # Add the boundary constraints xmin = min(self.xInit[0], self.xT[0]) - self.margin xmax = max(self.xInit[0], self.xT[0]) + self.margin ymin = min(self.xInit[1], self.xT[1]) - self.margin ymax = max(self.xInit[1], self.xT[1]) + self.margin for idx in range(1, self.__N): for x_var, xm in zip(self.x[idx].values(), [xmin, ymin]): self.prob.addConstraint(x_var >= xm) for x_var, xm in zip(self.x[idx].values(), [xmax, ymax]): self.prob.addConstraint(x_var <= xm) # Constraints on intermediate variables if self.receding_horizon: limit = self.__N - 1 else: limit = self.__N for k in range(limit): # absu >= u # absu + u >= 0 for i in range(self.__nu): self.prob.addConstraint(self.absu[k][i] - self.u[k][i] >= 0) self.prob.addConstraint(self.absu[k][i] + self.u[k][i] >= 0) # State Transition modelled as a constraint # x[k+1] == A*x[k] + B*u[:,k] for x_var, a, b in zip(self.x[k + 1].values(), self.__A, self.__B): self.prob.addConstraint((x_var - pulp.lpSum([(ai * xi) for ai, xi in zip(a, self.x[k].values())]) \ - pulp.lpSum([(bi * ui) for bi, ui in zip(b, self.u[k].values())])) == 0) # Lower bound on the horizon radius if self.receding_horizon: self.prob.addConstraint(self.dlast >= 0) # Constraints the distrance between adjacent points for kk in range(1, self.__N + 1 - self.receding_horizon): currx = self.x[kk].values() prevx = self.x[kk - 1].values() self.prob.addConstraint(self.d[kk - 1] >= 0) for side in range(self.poly_nsides): # parameters that determine the side of the polygon line_angle = [math.cos(2*side*math.pi/self.poly_nsides), \ math.sin(2*side*math.pi/self.poly_nsides)] # Add constraints between the last step and the goal point # This ensures that the goal is within the horizon of the last step self.prob.addConstraint( pulp.lpSum([ m * (x1 - x2) for m, x1, x2 in zip(line_angle, currx, prevx) ]) <= self.d[kk - 1]) if self.receding_horizon: xlaststep = self.x[self.__N - 1].values() xgoal = self.x[self.__N].values() for side in range(500): # parameters that determine the side of the polygon line_angle = [math.cos(2*side*math.pi/self.poly_nsides), \ math.sin(2*side*math.pi/self.poly_nsides)] # Add constraints between the last step and the goal point # This ensures that the goal is within the horizon of the last step self.prob.addConstraint( pulp.lpSum([ m * (x1 - x2) for m, x1, x2 in zip(line_angle, xgoal, xlaststep) ]) <= self.dlast) # \sum_{i} z_{i} == dim(z_{i}) - 1 constraint for k in range(limit): for i in range(self.__nObst): self.prob.addConstraint(pulp.lpSum([self.z[k][i][j] for j in range(self.__dObst)]) == \ self.__dObst-1, name='z_%d_%d'%(k, i)) def getUPoints(self): # Get the solution for the control inputs usol = [] for stepnum in range(self.__N): cusol = [] for unum in range(self.__nu): if usePuLP: cusol.append(pulp.value(self.u[stepnum][unum])) else: cusol.append(self.prob.getVal(self.u[stepnum][unum])) usol.append(cusol) return usol def getZsol(self): ''' Get the solution for z variable ''' zsol = {} for stepnum in range(self.__N): for obstnum in range(self.__nObst): zval = [] for j in range(self.__dObst): zval.append(pulp.value(self.z[stepnum][obstnum][j])) zzval = [np.around(x) for x in zval] # get the side being used try: cside = zzval.index(0) except: cside = -1 # Create the dictionary zsol[(stepnum, obstnum)] = cside return zsol def getZValues(self): ''' Get the solution for z variable ''' zsol = [] for stepnum in range(self.__N): for obstnum in range(self.__nObst): zval = [] for j in range(self.__dObst): zval.append(pulp.value(self.z[stepnum][obstnum][j])) zzval = [np.around(x) for x in zval] # get the side being used try: cside = zzval.index(0) except: cside = -1 # Create the dictionary zsol.append(cside / 4) return zsol def __addSCIPObstPt(self, xidx, kidx, delta): ''' Adds obstacle constraint for each way point ''' x = self.x[xidx].values()[:2] threshold = self.u_max * np.sqrt(2) # H*x[k+1] - M*z[k] <= G nConstraint = 0 for kk, (mDelta, Ci, Ri, Vi) in enumerate(zip(delta, self.__C, self.__R, self.__V)): # For each obstacle cns = 0 for xi, ci in zip(x, Ci): cns += (xi - ci)**2 if 'constraint%d_%d_%d' % (nConstraint, kidx, xidx) in self.cnstMap.keys(): self.prob.delCons(self.cnstMap['constraint%d_%d_%d' % (nConstraint, kidx, xidx)]) assert mDelta[0] >= 0, "Should be positive" cnx = self.prob.addCons(cns >= (Ri + mDelta[0])**2, name='constraint%d_%d_%d' % (nConstraint, kidx, xidx)) self.cnstMap['constraint%d_%d_%d' % (nConstraint, kidx, xidx)] = cnx nConstraint = nConstraint + 1 #import pdb; pdb.set_trace() def __addObstPt(self, zidx, xidx, delta): ''' Adds obstacle constraint for each way point ''' z = self.z[zidx] x = self.x[xidx].values()[:2] threshold = self.u_max * np.sqrt(2) # H*x[k+1] - M*z[k] <= G nConstraint = 0 for mDelta, Hi, Gi, Zi, Vi in zip(delta, self.__H, self.__G, z.values(), self.__V): # For each obstacle for m, h, g, zi in zip(mDelta, Hi, Gi, Zi.values()): # For each hyperplane of the obstacle if (str('constraint%d_%d_%d' % (nConstraint, zidx, xidx)) in self.prob.constraints): # if key already exists (http://www.coin-or.org/PuLP/pulp.html) # it still needs to be updated # (that's what were doing -- overwriting the old key with the new data, so del self.prob.constraints['constraint%d_%d_%d' % (nConstraint, zidx, xidx)] # delete the old key first, then add the new one below # this gets rid of the pulp # "('Warning: overlapping constraint names:', 'constraint43_19')" # types of message-printouts # (http://pulp.readthedocs.org/en/latest/user-guide/troubleshooting.html) self.prob.addConstraint((pulp.lpSum([((-hi)*xi) for hi, xi in zip(h,x)]) \ - self.__M*zi + m + g) <= 0, name='constraint%d_%d_%d'%(nConstraint, zidx, xidx)) # Used for naming the constraints to replace on recursive calls with delta nConstraint = nConstraint + 1 def writeOut(self, name): ''' Write out the solution ''' if usePuLP: self.prob.writeLP(name + '.lp') else: self.prob.writeProblem(name + '.mps') return def __addObstConstraint(self, mdelta=None): ''' Adds obstacle constraints based on the H and G matrix to all points ''' num = self.__N - int(self.receding_horizon) if mdelta is None: mdelta = np.zeros((num, self.__H.shape[0], self.__H.shape[1])) mdelta = [ mdelta, np.zeros((num - 1, self.__H.shape[0], self.__H.shape[1])) ] # Initial state constraint zeroDelta = np.zeros((self.__H.shape[0], self.__H.shape[1])) if usePuLP: self.__addObstPt(0, 0, zeroDelta) else: self.__addSCIPObstPt(0, 0, zeroDelta) for k in range(num - 1): # Adding constraints on the obstacle for each point if usePuLP: self.__addObstPt(k, k + 1, mdelta[0][k]) self.__addObstPt(k + 1, k + 1, mdelta[1][k]) else: self.__addSCIPObstPt(k, k + 1, mdelta[0][k]) self.__addSCIPObstPt(k + 1, k + 1, mdelta[1][k]) # Last step k = num - 1 if usePuLP: self.__addObstPt(k, k + 1, mdelta[0][k]) else: self.__addSCIPObstPt(k, k + 1, mdelta[0][k]) return def activeObstWayPt(self, x, mDelta, Hi, Gi, Zi): ''' Test is the way points is active for a particular waypoint ''' for m, h, g, z in zip(mDelta, Hi, Gi, Zi): hDotx = np.sum([hi * xi for hi, xi in zip(h, x)]) # To acount for floating point inaccuracy if ((hDotx - self.__M * z - m - g) >= 0): return True return False #def getActiveBoundaries(self): # ''' # Looks at both the active edges # chooses the one that is closest as the active edges # Because now each z corresponds to 2 state vectors # ''' # active_boundaries = [[None for i in range(self.__nObst)]\ # for j in range(self.__N)] # active_dist = self.measureDist() # for t_i in range(self.__N): # for obs_j in range(self.__nObst): # idx = np.where(active_dist[:, t_i, obs_j] < 0)[0] # active_boundaries[t_i][obs_j] = idx[np.argmax(active_dist[idx, t_i, obs_j])] # return active_boundaries def getActiveBoundaries(self): ''' Check the active boundaries at each time point for each obstacle ''' num = self.__N - self.receding_horizon active_boundaries = [[None for i in range(self.__nObst)]\ for j in range(num)] for t_i in range(num): for obs_j in range(self.__nObst): for dobs_k in range(self.__dObst): if pulp.value(self.z[t_i][obs_j][dobs_k]) < 1: ## debug # print 'Z: ' + str(self.z[t_i][obs_j][dobs_k]) # print 'Z value' + str(pulp.value(self.z[t_i][obs_j][dobs_k])) # wp = self.wayPoint[t_i][:2] # input("Press Enter to continue...") ##debug active_boundaries[t_i][obs_j] = dobs_k break return np.array(active_boundaries) def measureDist(self): ''' Adds obstacle constraints based on the H and G matrix to all points ''' active_distance = [[[None for i in range(self.__nObst)]\ for j in range(self.__N)] for k in range(self.__dObst)] # Initial point zeroDelta = np.zeros((self.__H.shape[0], self.__H.shape[1])) for k in range(self.__N): self.__measureDist(k + 1, active_distance) #self.__measureDist(k+1, k+1, active_distance[1]) return np.array(active_distance) def __measureDist(self, xidx, dist): ''' Adds obstacle constraint for each way point ''' x = self.x[xidx].values()[:2] # H*x[k+1] - M*z[k] - G nConstraint = 0 for j, (Hi, Gi) in enumerate(zip(self.__H, self.__G)): # For each obstacle for n, (h, g) in enumerate(zip(Hi, Gi)): dist[n][xidx - 1][j] = np.sum([((-hi) * pulp.value(xi)) for hi, xi in zip(h, x)]) + g return def activewayPoint(self, idx, delta, mask=None): ''' Checks if waypoint with index is active ''' z = self.__zVal[idx] x = self.wayPoint[idx + 1][:2] if mask is None: mask = [False for i in range(self.__nObst)] # H*x[k+1] - M*z[k] <= G for j, (mDelta, Hi, Gi, Zi, cMask) in enumerate(zip(delta, self.__H, self.__G, z, mask)): # Mask is used to look at only the obstacles not already tested for delta update if cMask is False: if self.activeObstWayPt(x, mDelta, Hi, Gi, Zi) is False: continue else: return j else: # Obstacle already parsed continue return None def addObstacles(self, N, H, G, V, zInit=[]): ''' Adds obstacles with H and G. This function initiates all the constraints for optimization ''' # create problem variables self.__N = N self.__createVar() nObst = len(G) self.__H = H self.__G = G self.__V = V self.__C = np.mean(V, axis=1) diff = np.square(V[-1] - np.repeat([self.__C[-1]], 4, axis=0)) radius = np.sqrt(np.sum(diff, axis=1)) self.__R = [radius[0]] * len(self.__C) self.__nObst = self.__G.shape[0] self.__dObst = self.__G[0].shape[0] # z variable dim (nPoints x nObst) num = self.__N - self.receding_horizon self.__addObjective() if usePuLP: self.z = pulp.LpVariable.dicts("z", (range(num), range(self.__nObst), \ range(self.__dObst)), 0, 1, pulp.LpInteger) self.__addVarConstraint(zInit) else: self.__addSCIPVarConstraint(zInit) self.__addObstConstraint() def solve(self, mdelta=None, outF=None, create=False): ''' Solves and extracts the output variables into xVal and yVal ''' # Modify the constraints with new delta if mdelta != None: self.__addObstConstraint(mdelta) if create: return # Solve the optimization problem with tempfile.NamedTemporaryFile() as fp: # using temporary file to write log if usePuLP: self.prob.solve( pulp.GUROBI_CMD(msg=self.msg, options={ "ResultFile": 'temp.sol', "NodeLimit": 200000 })) else: self.prob.optimize() ## Read log file to get number of nodes explored #fp.seek(0) #data = fp.read() ## Read the explored nodes #self.exploreNodes = re.findall("Explored (\d+) nodes", data)[0] #self.exploreNodes = 0 #if self.gap == None: # # Do this only for the first time # self.gap = re.findall("gap (\d+.\d+)", data)[0] #self.prob.solve(pulp.GUROBI_CMD(msg=self.msg, options={})) self.msg = 0 # Populate the solution variable # print("Status: ", pulp.LpStatus[self.prob.status]) if usePuLP: if 'Optimal' in pulp.LpStatus[self.prob.status]: self.feasible = True else: self.feasible = False else: if 'optimal' in self.prob.getStatus(): self.feasible = True else: self.feasible = False # Get solution from waypoints if self.feasible: self.wayPoint = [] for i in range(self.__N + 1): if usePuLP: self.wayPoint.append( [pulp.value(self.x[i][0]), pulp.value(self.x[i][1])]) else: self.wayPoint.append([ self.prob.getVal(self.x[i][0]), self.prob.getVal(self.x[i][1]) ]) if usePuLP: self.__zVal = [] for i in range(self.__N - self.receding_horizon): self.__zVal.append([[pulp.value(self.z[i][j][k]) for k in range(self.__dObst)] \ for j in range(self.__nObst)]) self._sol = {} for var in self.prob._variables: self._sol[str(var)] = pulp.value(var) if outF is not None: with open(outF + '.yaml', 'w') as outfile: Y.dump(self._sol, outfile, default_flow_style=False, explicit_start=True) return True else: return False def plot(self, fname): fig = plt.figure() ax = fig.add_subplot(111, aspect='equal') # Plot obstacles for g in self.__G: ax.add_patch( patches.Rectangle((g[0], g[1]), np.abs(g[0] + g[2]), np.abs(g[1] + g[3]), alpha=0.5)) # Plot Waypoints for pt1, pt2 in zip(self.wayPoint[:-1], self.wayPoint[1:]): ax.plot([pt1[0], pt2[0]], [pt1[1], pt2[1]], c='b') ax.scatter(pt1[0], pt1[1], c='g') ax.scatter(pt2[0], pt2[1], c='g') plt.savefig('./' + str(fname) + '.png') plt.close('all') # Class Interface functions def getWayPoints(self): return self.wayPoint def getActivePoints(self): return (self.activePtsIdx, self.activeObstIdx) def getHorizonRadius(self): if usePuLP: return pulp.value(self.dlast) else: return self.prob.getVal(self.dlast) def getObjective(self): if usePuLP: return pulp.value(self.prob.objective) else: return self.prob.getObjVal()