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