def re_verification(s, state_X, state_end, factor=0.99):
"""
This is a method to deal with numerical inaccuracies in high dimensioanl MILPs that are terminated early.
Requires solving a linear program. If feasible, the computed control startegy is recomputed.
If infeasible, report back, raise a flag, and abort the tree extention
Inputs: state_X, state_end
Output: flag, u, theta
"""
model = Model("verifying tree extention")
G_dynamic = np.empty((s.n, s.n), dtype='object')
theta = np.empty((s.m, s.n), dtype='object')
u = np.empty((s.m, 1), dtype='object')
x_bar = np.empty((s.n, 1), dtype='object')
for row in range(s.n):
for column in range(s.n):
G_dynamic[row, column] = model.addVar(lb=-GRB.INFINITY,
ub=GRB.INFINITY)
for row in range(s.m):
u[row, 0] = model.addVar(lb=-GRB.INFINITY, ub=GRB.INFINITY)
for column in range(s.n):
theta[row, column] = model.addVar(lb=-GRB.INFINITY,
ub=GRB.INFINITY)
for row in range(s.n):
x_bar[row, 0] = model.addVar(lb=-GRB.INFINITY, ub=GRB.INFINITY)
model.update()
i = state_X.mode
for row in range(s.n):
for column in range(s.n):
AG = LinExpr()
for k in range(s.n):
AG.add(s.A[i][row, k] * state_X.G[k, column])
for k in range(s.m):
AG.add(s.B[i][row, k] * theta[k, column])
model.addConstr(G_dynamic[row, column] == AG * factor)
for row in range(s.n):
Bu = LinExpr()
for k in range(s.m):
Bu.add(s.B[i][row, k] * u[k, 0])
model.addConstr(x_bar[row, 0] == np.dot(s.A[i], state_X.x)[row, 0] +
Bu + s.c[i][row, 0])
subset(model, G_dynamic, s.Pi, state_end.polytope.H, state_end.polytope.h,
x_bar)
subset(model, theta, s.Pi, s.F[i], s.f[i], u)
model.setParam("OutputFlag", False)
model.optimize()
if model.Status == 2:
print("Tree extention verified succesuflly!")
return (valuation(u), valuation(theta))
else:
print("\n-" * 10, "Tree extention failed!", "\n-" * 10)
return None
def anchor_point(polytope):
"""
A point in H,h
"""
model = Model("Polytope Sampling")
n = polytope.H.shape[1]
x = np.empty((n, 1), dtype="object")
rho = np.empty((polytope.H.shape[0], 1), dtype="object")
for row in range(n):
x[row, 0] = model.addVar(lb=-GRB.INFINITY, ub=GRB.INFINITY)
for row in range(polytope.H.shape[0]):
rho[row, 0] = model.addVar(lb=0, ub=GRB.INFINITY)
model.update()
J = QuadExpr(0)
for row in range(polytope.H.shape[0]):
a = LinExpr()
for column in range(polytope.H.shape[1]):
a.add(polytope.H[row, column] * x[column, 0])
model.addConstr(a + rho[row, 0] == polytope.h[row])
J.add(rho[row, 0] * rho[row, 0])
model.setParam('OutputFlag', False)
model.setObjective(J)
model.optimize()
return valuation(x)
def state_trajectory(s, x0, state_end, T):
model = Model("trajectory of polytopes")
x = {}
u = {}
theta = {}
z = {}
G_bound = 100
# Mode 1:
for t in range(T):
x[t] = np.empty((s.n, 1), dtype='object') # n*1
u[t] = np.empty((s.m, 1), dtype='object') # m*1
theta[t] = np.empty((s.m, s.n), dtype='object') # n*m
for row in range(s.n):
x[t][row, 0] = model.addVar(lb=-G_bound, ub=G_bound)
for row in range(s.m):
u[t][row, 0] = model.addVar(lb=-G_bound, ub=G_bound)
for row in range(s.m):
for column in range(s.n):
theta[t][row, column] = 0
for t in range(T + 1):
for i in s.modes:
z[t, i] = model.addVar(vtype=GRB.BINARY)
x[T] = np.empty((s.n, 1), dtype='object') # Final state in Mode i
for row in range(s.n):
x[T][row, 0] = model.addVar(lb=-G_bound, ub=G_bound)
G = {}
for t in range(T + 1):
G[t] = np.empty((s.n, s.n), dtype='object')
for row in range(s.n):
for column in range(s.n):
G[t][row, column] = 0
model.update()
# Trajectory Constraints:
# Mode i:
bigM = 100
for i in s.modes:
for t in range(T):
for row in range(s.n):
Ax = LinExpr()
for k in range(s.n):
Ax.add(s.A[i][row, k] * x[t][k, 0])
for k in range(s.m):
Ax.add(s.B[i][row, k] * u[t][k, 0])
model.addConstr(x[t + 1][row, 0] <= Ax + s.c[i][row] + bigM -
bigM * z[t, i])
model.addConstr(x[t + 1][row, 0] >= Ax + s.c[i][row] - bigM +
bigM * z[t, i])
# Generator Dynamics Constraints:
for i in s.modes:
for t in range(T):
for row in range(s.n):
for column in range(s.n):
AG = LinExpr()
for k in range(s.n):
AG.add(s.A[i][row, k] * G[t][k, column])
for k in range(s.m):
AG.add(s.B[i][row, k] * theta[t][k, column])
model.addConstr(
G[t + 1][row, column] <= AG + bigM - bigM * z[t, i])
model.addConstr(
G[t + 1][row, column] >= AG - bigM + bigM * z[t, i])
# Constraints of modes:
for t in range(T + 1):
sum_z = LinExpr()
for i in s.modes:
sum_z.add(z[t, i])
model.addConstr(sum_z == 1)
# Constraints of mode subsets
for t in range(T):
for i in s.modes:
subset_MILP(model, G[t], s.Pi, s.H[i], s.h[i], x[t], z[t, i])
subset_MILP(model, theta[t], s.Pi, s.F[i], s.f[i], u[t], z[t, i])
# set objective
model.update()
# Terminal Constraint
terminal_constraint(s, x, G, T, model, state_end)
# Starting Point
i_start = find_mode(s, x0)
for i in s.modes:
model.addConstr(z[0, i] == int(i == i_start))
model.setParam('OutputFlag', True)
for row in range(s.n):
model.addConstr(x[0][row, 0] == x0[row, 0])
model.optimize()
if model.Status != 2 and model.Status != 11:
flag = False
# print("*"*20,"Flag is False and Status is",model.Status)
return (x, u, z, flag)
else:
flag = True
# print("*"*20,"Flag is True and Status is",model.Status)
x_n = valuation(x)
u_n = valuation(u)
z_n = mode_sequence(s, z)
return (x_n, u_n, z_n, flag)
def polytope_trajectory(s,
x0,
state_end,
T,
alpha_start,
eps=0.1,
coin=random()):
(model, x, u, G, theta, z) = s.library[T]
n_vars = len(model.getVars())
n_constraints = len(model.getConstrs())
new_var_count = 0
new_constraint_count = 0
J_area = LinExpr()
d_min = model.addVar(lb=0.0001, name="new var %d" % new_var_count)
new_var_count += 1
beta = 10**2 # Weight of infinity norm
model.update()
print("coin=", coin)
for row in range(s.n):
for column in range(s.n):
if coin < 0.1:
if row < column:
model.addConstr(G[0][row, column] == 0,
name="constraint %d" %
new_constraint_count)
new_constraint_count += 1
elif coin > 0.9:
if row > column:
model.addConstr(G[0][row, column] == 0,
name="constraint %d" %
new_constraint_count)
new_constraint_count += 1
if row == column:
model.addConstr(G[0][row, column] >= d_min / s.weight[row],
name="constraint %d" % new_constraint_count)
new_constraint_count += 1
J_area.add(-d_min * T * s.n * beta)
for row in range(s.n):
for t in range(T):
J_area.add(-G[t][row, row] * s.weight[row])
# Terminal Constraint
terminal_constraint(s, x, G, T, model, state_end)
# Starting Point
i_start = find_mode(s, x0)
for i in s.modes:
model.addConstr(z[0, i] == int(i == i_start))
# model.setParam('OutputFlag',False)
if alpha_start == -1:
x_delta = {}
for row in range(s.n):
x_delta[row] = model.addVar(lb=-eps / s.weight[row],
ub=eps / s.weight[row])
model.update()
for row in range(s.n):
model.addConstr(x[0][row, 0] == x0[row, 0] + x_delta[row])
model.setObjective(J_area)
model.optimize()
else:
model.setObjective(J_area)
model.optimize()
if model.Status != 2 and model.Status != 11:
flag = False
print("*" * 20, "False flag", model.Status)
final = (x, u, G, theta, z, flag)
else:
flag = True
x_n = valuation(x)
u_n = valuation(u)
G_n = valuation(G)
theta_n = valuation(theta)
z_n = mode_sequence(s, z)
# if abs(np.linalg.det(G_n[0]))<10**-15:
# flag=False
final = (x_n, u_n, G_n, theta_n, z_n, flag)
print("starting removal process")
print("start=", n_vars, "variables and ", n_constraints, " constraints")
new_n_vars = len(model.getVars())
new_n_constraints = len(model.getConstrs())
print("new:", new_n_vars, "variables and ", new_n_constraints,
" constraints")
time_start = time()
remove_new_constraints(s, model, T)
print("end of removal in ", time() - time_start, " seconds")
return final
def polytopic_trajectory_to_set_of_polytopes(s,x0,T,list_of_polytopes,eps=0,method="bigM",timelimit=60):
(model,x,u,G,theta,z)=s.library[T]
print("Initial: The number of variables is %d and # constraints is %d"%(len(model.getVars()),len(model.getConstrs())))
J_area=LinExpr()
d_min=model.addVar(lb=0.0001,name="new var")
beta=10**2 # Weight of infinity norm
model.update()
coin=random()
for row in range(s.n):
for column in range(s.n):
if coin<0.1:
if row<column:
model.addConstr(G[0][row,column]==0,name="constrain")
elif coin>0.9:
if row>column:
model.addConstr(G[0][row,column]==0)
if row==column:
model.addConstr(G[0][row,column]>=d_min/s.weight[row])
J_area.add(-d_min*T*s.n*beta)
for row in range(s.n):
for t in range(T):
J_area.add(-G[t][row,row]*s.weight[row])
# Starting Point and initial condition
i_start=find_mode(s,x0)
for i in s.modes:
model.addConstr(z[0,i]==int(i==i_start))
x_delta={}
for row in range(s.n):
x_delta[row]=model.addVar(lb=-eps/s.weight[row],ub=eps/s.weight[row])
model.update()
for row in range(s.n):
model.addConstr(x[0][row,0]==x0[row,0]+x_delta[row])
model.setParam('OutputFlag',True)
model.setParam('TimeLimit', timelimit)
print("number of constraints is",len(model.getConstrs()),len(model.getGenConstrs()))
# Terminal Constraint
if method=="bigM":
z_pol=terminal_constraint_set_bigM(s,x[T],G[T],model,list_of_polytopes)
elif method in ["convexhull","Convex_hull","convex_hull","chull"]:
z_pol=terminal_constraint_convex_hull(s,x[T],G[T],model,list_of_polytopes)
else:
raise(method," was not recognized. Enter either 'big-M' or 'Convex_hull' as method")
model.setObjective(J_area)
print("number of constraints is",len(model.getConstrs()),len(model.getGenConstrs()))
model.optimize()
if model.SolCount>0:
flag=True
print("+"*20,"Flag is True and Status is",model.Status)
x_n=valuation(x)
u_n=valuation(u)
G_n=valuation(G)
theta_n=valuation(theta)
z_n=mode_sequence(s,z)
# if abs(np.linalg.det(G_n[0]))<10**-15:
# flag=False
state_end_list=[y for y in list_of_polytopes if abs(z_pol[y].X-1)<0.1]
print(len(state_end_list))
assert (len(state_end_list)==1)
state_end=state_end_list[0]
final=(x_n,u_n,G_n,theta_n,z_n,flag,state_end)
elif model.Status!=2 and model.Status!=11:
flag=False
print("-"*20,"False flag",model.Status)
final=(x,u,G,theta,z,flag,s.goal)
else:
pass
print(model.getConstrByName("sadra%d"%T) in model.getConstrs(),model.getConstrByName("sadra%d"%T))
remove_new_constraints(s,model,T)
print(model.getConstrByName("sadra%d"%T) in model.getConstrs(),model.getConstrByName("sadra%d"%T))
return final