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()
