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