def apply_dynamic(input, gurobi_model: grb.Model, env_input_size): ''' :param input: :param gurobi_model: :param t: :return: ''' p = input[0] v = input[1] dt = 0.1 z = gurobi_model.addMVar(shape=(env_input_size, ), lb=float("-inf"), name=f"x_prime") # pos_max = gurobi_model.addMVar(shape=(1,), lb=float("-inf"), name=f"pos_max") v_second = v - 9.81 * dt p_second = p + dt * v_second gurobi_model.addConstr(z[1] == v_second, name=f"dyna_constr_1") # gurobi_model.addConstr(pos_max == grb.max_([p_second, 0]), name=f"dyna_constr_2") # gurobi_model.addConstr(z[1] == p_second, name=f"dyna_constr_2") max_switch = gurobi_model.addMVar(lb=0, ub=1, shape=p_second.shape, vtype=grb.GRB.INTEGER, name=f"max_switch") M = 10e6 # gurobi_model.addConstr(v == grb.max_(0, gurobi_vars[-1])) gurobi_model.addConstr(z[0] >= p_second) gurobi_model.addConstr(z[0] <= p_second + M * max_switch) gurobi_model.addConstr(z[0] >= 0) gurobi_model.addConstr(z[0] <= M - M * max_switch) return z
def generate_angle_milp(gurobi_model: grb.Model, input, sin_cos_table: List[Tuple]): """MILP method input: theta, thetadot output: thetadotdot, xdotdot (edited) l_{theta, i}, l_{thatdot,i}, l_{thetadotdot, i}, l_{xdotdot, i}, u_.... sum_{i=1}^k l_{x,i} - l_{x,i}*z_i <= x <= sum_{i=1}^k u_{x,i} - u_{x,i}*z_i, for each variable x sum_{i=1}^k l_{theta,i} - l_{theta,i}*z_i <= theta <= sum_{i=1}^k u_{theta,i} - u_{theta,i}*z_i """ theta = input[2] theta_dot = input[3] k = len(sin_cos_table) zs = [] thetaacc = gurobi_model.addMVar(shape=(1,), lb=float("-inf"), name="thetaacc") xacc = gurobi_model.addMVar(shape=(1,), lb=float("-inf"), name="xacc") for i in range(k): z = gurobi_model.addMVar(lb=0, ub=1, shape=(1,), vtype=grb.GRB.INTEGER, name=f"part_{i}") zs.append(z) gurobi_model.addConstr(k - 1 == sum(zs), name=f"const_milp1") theta_lb = 0 theta_ub = 0 theta_dot_lb = 0 theta_dot_ub = 0 thetaacc_lb = 0 thetaacc_ub = 0 xacc_lb = 0 xacc_ub = 0 for i in range(k): theta_interval, theta_dot_interval, theta_acc_interval, xacc_interval = sin_cos_table[i] theta_lb += theta_interval[0].inf - theta_interval[0].inf * zs[i] theta_ub += theta_interval[0].sup - theta_interval[0].sup * zs[i] theta_dot_lb += theta_dot_interval[0].inf - theta_dot_interval[0].inf * zs[i] theta_dot_ub += theta_dot_interval[0].sup - theta_dot_interval[0].sup * zs[i] thetaacc_lb += theta_acc_interval[0].inf - theta_acc_interval[0].inf * zs[i] thetaacc_ub += theta_acc_interval[0].sup - theta_acc_interval[0].sup * zs[i] xacc_lb += xacc_interval[0].inf - xacc_interval[0].inf * zs[i] xacc_ub += xacc_interval[0].sup - xacc_interval[0].sup * zs[i] gurobi_model.addConstr(theta >= theta_lb, name=f"theta_guard1") gurobi_model.addConstr(theta <= theta_ub, name=f"theta_guard2") gurobi_model.addConstr(theta_dot >= theta_dot_lb, name=f"theta_dot_guard1") gurobi_model.addConstr(theta_dot <= theta_dot_ub, name=f"theta_dot_guard2") gurobi_model.addConstr(thetaacc >= thetaacc_lb, name=f"thetaacc_guard1") gurobi_model.addConstr(thetaacc <= thetaacc_ub, name=f"thetaacc_guard2") gurobi_model.addConstr(xacc >= xacc_lb, name=f"xacc_guard1") gurobi_model.addConstr(xacc <= xacc_ub, name=f"xacc_guard2") gurobi_model.update() gurobi_model.optimize() # assert gurobi_model.status == 2, "LP wasn't optimally solved" return thetaacc, xacc
def apply_dynamic(input, gurobi_model: grb.Model, action, env_input_size): ''' :param input: :param gurobi_model: :param action_ego: :param t: :return: lead 100km/h 28m/s ego 130km/h 36.1 m/s ''' v_lead = 28 max_speed = 36.0 delta_x = input[0] v_ego = input[1] z = gurobi_model.addMVar(shape=(env_input_size, ), lb=float("-inf"), name=f"x_prime") const_acc = 3 dt = .1 # seconds if action == 0: acceleration = -const_acc elif action == 1: acceleration = const_acc else: acceleration = 0 v_ego_prime_temp1 = gurobi_model.addVar() v_ego_prime_temp2 = gurobi_model.addVar() v_next = v_ego._vararr[0] + acceleration * dt gurobi_model.addConstr(v_ego_prime_temp1 == v_next, name=f"v_constr") gurobi_model.addConstr(v_ego_prime_temp2 == grb.min_( max_speed, v_ego_prime_temp1), name=f"v_constr") v_ego_prime = grb.MVar(v_ego_prime_temp2) # convert from Var to MVar v_lead_prime = v_lead delta_prime_v = v_lead_prime - v_ego_prime delta_prime_v_temp = gurobi_model.addMVar(shape=(1, ), lb=float("-inf"), name=f"delta_prime_v_temp") gurobi_model.addConstr(delta_prime_v_temp == delta_prime_v, name=f"delta_prime_v_constr") delta_x_prime = delta_x + delta_prime_v_temp * dt # x_lead_prime = x_lead + v_lead_prime * dt # x_ego_prime = x_ego + v_ego_prime * dt gurobi_model.addConstr(z[0] == delta_x_prime, name=f"dyna_constr_1") gurobi_model.addConstr(z[1] == v_ego_prime, name=f"dyna_constr_3") return z
def apply_dynamic(self, input, gurobi_model: grb.Model, newthdot, newtheta, env_input_size, action): ''' :param costheta: gurobi variable containing the range of costheta values :param sintheta: gurobi variable containin the range of sintheta values :param input: :param gurobi_model: :param t: :return: ''' tau = self.tau # 0.001 # seconds between state updates # x = input[0] # x_dot = input[1] # theta = input[2] # theta_dot = input[3] # battery = input[4] z = gurobi_model.addMVar(shape=(env_input_size, ), lb=float("-inf"), name=f"x_prime") # x_prime = x + tau * x_dot # x_dot_prime = x_dot + tau * xacc # theta_prime = theta + tau * theta_dot # theta_dot_prime = theta_dot + tau * thetaacc # action_cost = 0 # if action != 0: # action_cost = 0.5 gurobi_model.addConstr(z[0] == newtheta, name=f"dyna_constr_1") gurobi_model.addConstr(z[1] == newthdot, name=f"dyna_constr_2") return z
def apply_dynamic(input, gurobi_model: grb.Model, action, env_input_size): ''' :param input: :param gurobi_model: :param action_ego: :param t: :return: lead 100km/h 28m/s ego 130km/h 36.1 m/s ''' x_lead = input[0] x_ego = input[1] v_ego = input[2] # gurobi_model.addConstr(action[0] <= 12) # cap the acceleration to +12 m/s*2 # gurobi_model.addConstr(action[0] >= -12) # cap the acceleration to -12 m/s*2 z = gurobi_model.addMVar(shape=(3,), lb=float("-inf"), name=f"x_prime") dt = .1 # seconds acceleration = action[0] a_ego_prime = acceleration v_ego_prime = v_ego + acceleration * dt gurobi_model.addConstr(v_ego_prime <= max_speed, name=f"v_constr") # gurobi_model.addConstr(v_ego_prime >= -max_speed, name=f"v_constr") # gurobi_model.addConstr(a_lead == 0, name="a_lead_constr") v_lead_prime = v_lead x_lead_prime = x_lead + v_lead_prime * dt x_ego_prime = x_ego + v_ego_prime * dt gurobi_model.addConstr(z[0] == x_lead_prime, name=f"dyna_constr_1") gurobi_model.addConstr(z[1] == x_ego_prime, name=f"dyna_constr_2") gurobi_model.addConstr(z[2] == v_ego_prime, name=f"dyna_constr_3") return z
def apply_dynamic(self, input, gurobi_model: grb.Model, thetaacc, xacc, env_input_size): ''' :param costheta: gurobi variable containing the range of costheta values :param sintheta: gurobi variable containin the range of sintheta values :param input: :param gurobi_model: :param t: :return: ''' tau = self.tau # 0.001 # seconds between state updates x = input[0] x_dot = input[1] theta = input[2] theta_dot = input[3] z = gurobi_model.addMVar(shape=(env_input_size, ), lb=float("-inf"), name=f"x_prime") x_prime = x + tau * x_dot x_dot_prime = x_dot + tau * xacc theta_prime = theta + tau * theta_dot theta_dot_prime = theta_dot + tau * thetaacc gurobi_model.addConstr(z[0] == x_prime, name=f"dyna_constr_1") gurobi_model.addConstr(z[1] == x_dot_prime, name=f"dyna_constr_2") gurobi_model.addConstr(z[2] == theta_prime, name=f"dyna_constr_3") gurobi_model.addConstr(z[3] == theta_dot_prime, name=f"dyna_constr_4") return z
def apply_dynamic(input, gurobi_model: grb.Model, action, env_input_size): ''' :param input: :param gurobi_model: :param action_ego: :param t: :return: lead 100km/h 28m/s ego 130km/h 36.1 m/s ''' v_lead = 28 max_speed = 36 x_lead = input[0] x_ego = input[1] # v_lead = input[2] v_ego = input[2] # a_lead = input[4] # a_ego = input[5] z = gurobi_model.addMVar(shape=(3, ), lb=float("-inf"), name=f"x_prime") const_acc = 3 dt = .1 # seconds if action == 0: acceleration = -const_acc elif action == 1: acceleration = const_acc else: acceleration = 0 v_ego_prime = v_ego + acceleration * dt gurobi_model.addConstr(v_ego_prime <= max_speed, name=f"v_constr") # gurobi_model.addConstr(v_ego_prime >= -max_speed, name=f"v_constr") # gurobi_model.addConstr(a_lead == 0, name="a_lead_constr") v_lead_prime = v_lead x_lead_prime = x_lead + v_lead_prime * dt x_ego_prime = x_ego + v_ego_prime * dt # delta_x_prime = (x_lead + (v_lead + (a_lead + 0) * dt) * dt) - (x_ego + (v_ego + (a_ego + acceleration) * dt) * dt) # delta_v_prime = (v_lead + (a_lead + 0) * dt) - (v_ego + (a_ego + acceleration) * dt) gurobi_model.addConstr(z[0] == x_lead_prime, name=f"dyna_constr_1") gurobi_model.addConstr(z[1] == x_ego_prime, name=f"dyna_constr_2") gurobi_model.addConstr(z[2] == v_ego_prime, name=f"dyna_constr_3") # gurobi_model.addConstr(z[3] == v_ego_prime, name=f"dyna_constr_4") # gurobi_model.addConstr(z[4] == 0, name=f"dyna_constr_5") # no change in a_lead # gurobi_model.addConstr(z[5] == acceleration, name=f"dyna_constr_6") return z
def apply_dynamic(input, gurobi_model: grb.Model, action, env_input_size): ''' in this case ACTION is a variable :param input: :param gurobi_model: :param action_ego: :param t: :return: lead 100km/h 28m/s ego 130km/h 36.1 m/s ''' v_lead = 28 max_speed = 36 x_lead = input[0] x_ego = input[1] v_ego = input[2] z = gurobi_model.addMVar(shape=(3,), lb=float("-inf"), name=f"x_prime") const_acc = 3 dt = .1 # seconds acceleration = gurobi_model.addMVar(shape=(1,), lb=float("-inf")) gurobi_model.addConstr(acceleration[0] == (2 * action[0] - 1) * const_acc, name=f"action_constr") v_ego_prime = v_ego + acceleration * dt gurobi_model.addConstr(v_ego_prime <= max_speed, name=f"v_constr") # gurobi_model.addConstr(v_ego_prime >= -max_speed, name=f"v_constr") # gurobi_model.addConstr(a_lead == 0, name="a_lead_constr") v_lead_prime = v_lead x_lead_prime = x_lead + v_lead_prime * dt x_ego_prime = x_ego + v_ego_prime * dt gurobi_model.addConstr(z[0] == x_lead_prime, name=f"dyna_constr_1") gurobi_model.addConstr(z[1] == x_ego_prime, name=f"dyna_constr_2") gurobi_model.addConstr(z[2] == v_ego_prime, name=f"dyna_constr_3") return z
def generate_nn_guard(gurobi_model: grb.Model, input, nn: torch.nn.Sequential, action_ego=0, M=1e6): gurobi_vars = [] gurobi_vars.append(input) for i, layer in enumerate(nn): # print(layer) if type(layer) is torch.nn.Linear: v = gurobi_model.addMVar(lb=float("-inf"), shape=(layer.out_features), name=f"layer_{i}") weights: np.ndarray = layer.weight.data.numpy() weights.round(6) lin_expr = weights @ gurobi_vars[-1] if layer.bias is not None: lin_expr = lin_expr + layer.bias.data.numpy() gurobi_model.addConstr(v == lin_expr, name=f"linear_constr_{i}") gurobi_vars.append(v) elif type(layer) is torch.nn.ReLU: v = gurobi_model.addMVar( lb=float("-inf"), shape=gurobi_vars[-1].shape, name=f"layer_{i}") # same shape as previous z = gurobi_model.addMVar(lb=0, ub=1, shape=gurobi_vars[-1].shape, vtype=grb.GRB.INTEGER, name=f"relu_{i}") eps = 0 # 1e-9 # gurobi_model.addConstr(v == grb.max_(0, gurobi_vars[-1])) gurobi_model.addConstr(v >= gurobi_vars[-1], name=f"relu_constr_1_{i}") gurobi_model.addConstr(v <= eps + gurobi_vars[-1] + M * z, name=f"relu_constr_2_{i}") gurobi_model.addConstr(v >= 0, name=f"relu_constr_3_{i}") gurobi_model.addConstr(v <= eps + M - M * z, name=f"relu_constr_4_{i}") gurobi_vars.append(v) # gurobi_model.update() # gurobi_model.optimize() # assert gurobi_model.status == 2, "LP wasn't optimally solved" """ y = Relu(x) 0 <= z <= 1, z is integer y >= x y <= x + Mz y >= 0 y <= M - Mz""" # gurobi_model.update() # gurobi_model.optimize() # assert gurobi_model.status == 2, "LP wasn't optimally solved" # gurobi_model.setObjective(v[action_ego].sum(), grb.GRB.MAXIMIZE) # maximise the output last_layer = gurobi_vars[-1] if action_ego == 0: gurobi_model.addConstr(last_layer[0] >= last_layer[1], name="last_layer") else: gurobi_model.addConstr(last_layer[1] >= last_layer[0], name="last_layer") gurobi_model.update() gurobi_model.optimize() # assert gurobi_model.status == 2, f"LP wasn't optimally solved, gurobi status {gurobi_model.status}" return gurobi_model.status == 2
def apply_dynamic(input, gurobi_model: grb.Model, action, env_input_size): ''' :param input: :param gurobi_model: :param t: :return: ''' n_actions = 10 K = 1.0 r = 0.3 n_fish = input[0] quota = (action / n_actions) * K fish_population = gurobi_model.addMVar(shape=(1, ), lb=float("-inf"), name=f"fish_population") gurobi_model.addConstr(fish_population[0] == (n_fish + 1) * K, name=f"dyna_constr_1") fish_population_prime = gurobi_model.addMVar( shape=(1, ), lb=0, name=f"fish_population_prime") # lb set to 0 gurobi_model.addConstr(fish_population_prime[0] == (fish_population - quota), name=f"dyna_constr_2") # gurobi_model.setObjective(fish_population_prime[0].sum(), grb.GRB.MAXIMIZE) # gurobi_model.optimize() # max_fish_population_prime = fish_population_prime[0].X # todo switch to fish_population_prime # gurobi_model.setObjective(fish_population_prime[0].sum(), grb.GRB.MINIMIZE) # gurobi_model.optimize() # min_fish_population_prime = fish_population_prime[0].X # step_fish_population_prime = 0.1 # split_fish_population_prime = np.arange(min(min_fish_population_prime, 0), min(max_fish_population_prime, 0), step_fish_population_prime) # split = [] # for fish_pop in split_fish_population_prime: # lb = fish_pop # ub = min(fish_pop + step_fish_population_prime, max_fish_population_prime) # split.append((interval([lb, ub]))) # fish_growth_table = [] # while (len(split)): # fish_pop_interval = split.pop() fish_population_post = gurobi_model.addMVar( shape=(1, ), lb=float("-inf"), name=f"fish_population_post") growth = r * fish_population_prime # * (1.0 - (fish_population_prime[0] / K)) growth1 = gurobi_model.addMVar(shape=(1, ), lb=float("-inf"), name=f"growth1") gurobi_model.addConstr(growth1 == growth, name=f"dyna_constr_3") second_growth = (1.0 - (fish_population_prime / K)) growth2 = gurobi_model.addMVar(shape=(1, ), lb=float("-inf"), name=f"growth2") gurobi_model.addConstr(growth2 == second_growth, name=f"dyna_constr_4") third_growth = growth1 @ growth2 gurobi_model.addConstr(fish_population_post == fish_population_prime + third_growth, name=f"dyna_constr_5") x_prime = gurobi_model.addMVar(shape=(1, ), lb=float("-inf"), name=f"x_prime") gurobi_model.addConstr(x_prime[0] == (fish_population_post / K) - 1, name=f"dyna_constr_6") return x_prime