def add_random_constraints(self):
"""Add the random constraints in the docplex models"""
for index, subroot in enumerate(self._subroots(self.scenario_tree), 1):
model = self._docplex_models[subroot.address]
rand_cts = []
with _timeit(self, f"\r Add random constraints at subtree #{index}... ", 3, index):
# indicator constraints
try:
ct_type = ('random', 'indicator')
next(self._gen_constraints(subroot, ct_type, model)) # test whether generator is empty
rand_cts += model.add_indicators(*zip(*self._gen_constraints(subroot, ct_type, model)))
except StopIteration:
pass
# quadratic constraints (must be added last)
try:
ct_type = ('random', 'quadratic')
next(self._gen_constraints(subroot, ct_type, model)) # test whether generator is empty
for ct in self._gen_constraints(subroot, ct_type, model):
rand_cts.append(model.add_constraint(ct)) # one by one
except StopIteration:
pass
# linear constraints (must be added last)
try:
ct_type = ('random', 'linear')
next(self._gen_constraints(subroot, ct_type, model)) # test whether generator is empty
rand_cts += model.add_constraints(self._gen_constraints(subroot, ct_type, model))
except StopIteration:
pass
self._random_cts[subroot.address] = rand_cts
def do_highlevel_control(self, params):
"""Decide parameters.
TODO finish description
"""
segs_polytopes, goal, seg_psi_bounds = self.compute_segs_polytopes_and_goal(
params)
"""Apply motion planning problem"""
n_segs = len(segs_polytopes)
Gamma, nu, nx = params.Gamma, params.nu, params.nx
T = self.__control_horizon
max_a, max_delta = self.__params.max_a, self.__params.max_delta
model = docplex.mp.model.Model(name="proposed_problem")
min_u = np.vstack((np.zeros(T), np.full(T,
-0.5 * max_delta))).T.ravel()
max_u = np.vstack(
(np.full(T, max_a), np.full(T, 0.5 * max_delta))).T.ravel()
u = np.array(model.continuous_var_list(nu * T,
lb=min_u,
ub=max_u,
name='u'),
dtype=object)
# State variables
X = (params.initial_rollout.local + util.obj_matmul(Gamma, u)).reshape(
T + 1, nx)
X = params.transform(X)
X = X[1:]
# Control variables
U = u.reshape(T, nu)
"""Apply motion dynamics constraints"""
model.add_constraints(
self.compute_dyamics_constraints(params, X[:, 3], X[:, 4]))
"""Apply road boundary constraints"""
if self.road_boundary_constraints:
# Slack variables from road obstacles
Omicron = np.array(model.binary_var_list(n_segs * T,
name="omicron"),
dtype=object).reshape(n_segs, T)
M_big, diag = self.__params.M, self.__params.diag
psi_0 = params.initial_state.world[2]
for t in range(self.__control_horizon):
for seg_idx, (A, b) in enumerate(segs_polytopes):
lhs = util.obj_matmul(A, X[t, :2]) - np.array(
M_big * (1 - Omicron[seg_idx, t]))
rhs = b + diag
"""Constraints on road boundaries"""
model.add_constraints([l <= r for (l, r) in zip(lhs, rhs)])
"""Constraints on angle boundaries"""
# lhs = X[t, 2] - psi_0 - M_big*(1 - Omicron[seg_idx, t])
# model.add_constraint(lhs <= seg_psi_bounds[seg_idx][1])
# lhs = X[t, 2] - psi_0 + M_big*(1 - Omicron[seg_idx, t])
# model.add_constraint(lhs >= seg_psi_bounds[seg_idx][0])
model.add_constraint(np.sum(Omicron[:, t]) >= 1)
"""Start from current vehicle position and minimize the objective"""
w_ch_accel = 0.
w_ch_turning = 0.
w_accel = 0.
w_turning = 0.
# final destination objective
cost = (X[-1, 0] - goal[0])**2 + (X[-1, 1] - goal[1])**2
# change in acceleration objective
for u1, u2 in util.pairwise(U[:, 0]):
_u = (u1 - u2)
cost += w_ch_accel * _u * _u
# change in turning rate objective
for u1, u2 in util.pairwise(U[:, 1]):
_u = (u1 - u2)
cost += w_ch_turning * _u * _u
cost += w_accel * np.sum(U[:, 0]**2)
# turning rate objective
cost += w_turning * np.sum(U[:, 1]**2)
model.minimize(cost)
# TODO: warmstarting
# if self.U_warmstarting is not None:
# # Warm start inputs if past iteration was run.
# warm_start = model.new_solution()
# for i, u in enumerate(self.U_warmstarting[self.__control_horizon:]):
# warm_start.add_var_value(f"u_{2*i}", u[0])
# warm_start.add_var_value(f"u_{2*i + 1}", u[1])
# # add delta_0 as hotfix to MIP warmstart as it needs
# # at least 1 integer value set.
# warm_start.add_var_value('delta_0', 0)
# model.add_mip_start(warm_start)
# model.print_information()
# model.parameters.read.datacheck = 1
if self.log_cplex:
model.parameters.mip.display = 2
s = model.solve(log_output=True)
else:
model.solve()
# model.print_solution()
f = lambda x: x if isinstance(x, numbers.Number) else x.solution_value
cost = cost.solution_value
U_star = util.obj_vectorize(f, U)
X_star = util.obj_vectorize(f, X)
return util.AttrDict(cost=cost,
U_star=U_star,
X_star=X_star,
goal=goal)
def do_highlevel_control(self, params, ovehicles):
"""Decide parameters.
Since we're doing multiple coinciding, we want to compute...
TODO finish description
"""
vertices = self.__compute_vertices(params, ovehicles)
A_unions, b_unions = self.__compute_overapproximations(
vertices, params, ovehicles)
"""Apply motion planning problem"""
N_traj, L, T, K, O, Gamma, nu, nx = params.N_traj, self.__params.L, self.__params.T, params.K, \
params.O, self.__params.Gamma, self.__params.nu, self.__params.nx
model = docplex.mp.model.Model(name='obstacle_avoidance')
# set up controls variables for each trajectory
u = np.array(
model.continuous_var_list(N_traj * T * nu,
lb=-np.inf,
ub=np.inf,
name='control'))
Delta = np.array(
model.binary_var_list(O * L * N_traj * T,
name='delta')).reshape(N_traj, T, O, L)
# U has shape (N_traj, T*nu)
U = u.reshape(N_traj, -1)
# Gamma has shape (nx*(T + 1), nu*T) so X has shape (N_traj, nx*(T + 1))
X = util.obj_matmul(U, Gamma.T)
# X, U have shapes (N_traj, T, nx) and (N_traj, T, nu) resp.
X = (X + params.States_free_init).reshape(N_traj, -1, nx)[..., 1:, :]
U = U.reshape(N_traj, -1, nu)
for _U, _X in zip(U, X):
# _X, _U have shapes (T, nx) and (T, nu) resp.
p_x, p_y = _X[..., 0], _X[..., 1]
v_x, v_y = _X[..., 2], _X[..., 3]
u_x, u_y = _U[..., 0], _U[..., 1]
model.add_constraints(self.compute_boundary_constraints(p_x, p_y))
model.add_constraints(self.compute_velocity_constraints(v_x, v_y))
model.add_constraints(
self.compute_acceleration_constraints(u_x, u_y))
# set up obstacle constraints
M_big, N_traj, T, diag = self.__params.M_big, params.N_traj, self.__params.T, self.__params.diag
for n in range(N_traj):
# for each obstacle/trajectory
A_union, b_union = A_unions[n], b_unions[n]
for t in range(T):
# for each timestep
As, bs = A_union[t], b_union[t]
for o, (A, b) in enumerate(zip(As, bs)):
lhs = util.obj_matmul(
A, X[n, t, :2]) + M_big * (1 - Delta[n, t, o])
rhs = b + diag
model.add_constraints([l >= r for (l, r) in zip(lhs, rhs)])
model.add_constraint(np.sum(Delta[n, t, o]) >= 1)
# set up coinciding constraints
for t in range(0, self.__n_coincide):
for x1, x2 in util.pairwise(X[:, t]):
model.add_constraints([l == r for (l, r) in zip(x1, x2)])
# start from current vehicle position and minimize the objective
p_x, p_y, _ = carlautil.to_location_ndarray(self.__ego_vehicle,
flip_y=True)
if self.__goal.is_relative:
goal_x, goal_y = p_x + self.__goal.x, p_y + self.__goal.y
else:
goal_x, goal_y = self.__goal.x, self.__goal.y
start = np.array([p_x, p_y])
goal = np.array([goal_x, goal_y])
cost = np.sum((X[:, -1, :2] - goal)**2)
model.minimize(cost)
# model.print_information()
if self.log_cplex:
model.parameters.mip.display = 5
s = model.solve(log_output=True)
else:
model.solve()
# model.print_solution()
f = lambda x: x if isinstance(x, numbers.Number) else x.solution_value
U_star = util.obj_vectorize(f, U)
cost = cost.solution_value
X_star = util.obj_vectorize(f, X)
return util.AttrDict(cost=cost,
U_star=U_star,
X_star=X_star,
A_unions=A_unions,
b_unions=b_unions,
vertices=vertices,
start=start,
goal=goal)
def do_single_control(self, params, ovehicles):
"""Decide parameters.
TODO: acceleration limit is hardcoded to 8 m/s^2
TODO finish description
"""
vertices = self.__compute_vertices(params, ovehicles)
A_union, b_union = self.__compute_overapproximations(
vertices, params, ovehicles)
segs_polytopes, goal = self.compute_segs_polytopes_and_goal(params)
"""Apply motion planning problem"""
n_segs = len(segs_polytopes)
L, T, K, Gamma, nu, nx = self.__params.L, self.__params.T, params.K, \
self.__params.Gamma, self.__params.nu, self.__params.nx
u_max = self.__params.u_max
model = docplex.mp.model.Model(name="proposed_problem")
u = np.array(model.continuous_var_list(nu * T,
lb=-u_max,
ub=u_max,
name='u'),
dtype=object)
Delta = np.array(model.binary_var_list(L * np.sum(K) * T,
name='delta'),
dtype=object).reshape(np.sum(K), T, L)
Omicron = np.array(model.binary_var_list(n_segs * T, name="omicron"),
dtype=object).reshape(n_segs, T)
X = (params.States_free_init + util.obj_matmul(Gamma, u)).reshape(
T + 1, nx)
X = X[1:]
U = u.reshape(T, nu)
"""Apply motion dynamics constraints"""
model.add_constraints(
self.compute_velocity_constraints(X[:, 2], X[:, 3]))
model.add_constraints(
self.compute_acceleration_constraints(U[:, 0], U[:, 1]))
"""Apply road boundaries constraints"""
M_big, T, diag = self.__params.M_big, self.__params.T, self.__params.diag
for t in range(T):
for seg_idx, (A, b) in enumerate(segs_polytopes):
lhs = util.obj_matmul(A, X[t, :2]) - np.array(
M_big * (1 - Omicron[seg_idx, t]))
rhs = b + diag
model.add_constraints([l <= r for (l, r) in zip(lhs, rhs)])
model.add_constraint(np.sum(Omicron[:, t]) >= 1)
"""Apply collision constraints"""
T, K, diag, M_big = self.__params.T, params.K, \
self.__params.diag, self.__params.M_big
for ov_idx, ovehicle in enumerate(ovehicles):
n_states = ovehicle.n_states
sum_clu = np.sum(K[:ov_idx])
for latent_idx in range(n_states):
for t in range(T):
A_obs = A_union[t][latent_idx][ov_idx]
b_obs = b_union[t][latent_idx][ov_idx]
indices = sum_clu + latent_idx
lhs = util.obj_matmul(
A_obs, X[t, :2]) + M_big * (1 - Delta[indices, t])
rhs = b_obs + diag
model.add_constraints([l >= r for (l, r) in zip(lhs, rhs)])
model.add_constraint(np.sum(Delta[indices, t]) >= 1)
"""Start from current vehicle position and minimize the objective"""
start = params.x0[:2]
cost = (X[-1, 0] - goal[0])**2 + (X[-1, 1] - goal[1])**2
model.minimize(cost)
# TODO: warmstarting
# if self.U_warmstarting is not None:
# # Warm start inputs if past iteration was run.
# warm_start = model.new_solution()
# for i, u in enumerate(self.U_warmstarting[self.__control_horizon:]):
# warm_start.add_var_value(f"u_{2*i}", u[0])
# warm_start.add_var_value(f"u_{2*i + 1}", u[1])
# # add delta_0 as hotfix to MIP warmstart as it needs
# # at least 1 integer value set.
# warm_start.add_var_value('delta_0', 0)
# model.add_mip_start(warm_start)
# model.print_information()
# model.parameters.read.datacheck = 1
if self.log_cplex:
model.parameters.mip.display = 2
s = model.solve(log_output=True)
else:
model.solve()
# model.print_solution()
f = lambda x: x if isinstance(x, numbers.Number) else x.solution_value
cost = cost.solution_value
U_star = util.obj_vectorize(f, U)
X_star = util.obj_vectorize(f, X)
return util.AttrDict(cost=cost,
U_star=U_star,
X_star=X_star,
A_union=A_union,
b_union=b_union,
vertices=vertices,
start=start,
goal=goal)
def do_multiple_control(self, params, ovehicles):
"""Decide parameters.
Since we're doing multiple coinciding, we want to compute...
TODO finish description
"""
vertices = self.__compute_vertices(params, ovehicles)
A_unions, b_unions = self.__compute_overapproximations(
vertices, params, ovehicles)
segs_polytopes, goal = self.compute_segs_polytopes_and_goal(params)
"""Optimize all trajectories if MCC, else partial trajectories."""
# params.N_select params.N_traj params.subtraj_indices
N_select = params.N_select if self.__agent_type == "rmcc" else params.N_traj
"""Common to MCC+RMCC: setup CPLEX variables"""
L, T, K, O, Gamma, nu, nx = self.__params.L, self.__params.T, \
params.K, params.O, self.__params.Gamma, self.__params.nu, self.__params.nx
n_segs = len(segs_polytopes)
u_max = self.__params.u_max
model = docplex.mp.model.Model(name='obstacle_avoidance')
# set up controls variables for each trajectory
u = np.array(
model.continuous_var_list(N_select * T * nu,
lb=-u_max,
ub=u_max,
name='control'))
Delta = np.array(
model.binary_var_list(O * L * N_select * T,
name='delta')).reshape(N_select, T, O, L)
Omicron = np.array(model.binary_var_list(N_select * n_segs * T,
name="omicron"),
dtype=object).reshape(N_select, n_segs, T)
"""Common to MCC+RMCC: compute state from input variables"""
# U has shape (N_select, T*nu)
U = u.reshape(N_select, -1)
# Gamma has shape (nx*(T + 1), nu*T) so X has shape (N_select, nx*(T + 1))
X = util.obj_matmul(U, Gamma.T)
# X, U have shapes (N_select, T, nx) and (N_select, T, nu) resp.
X = (X + params.States_free_init).reshape(N_select, -1, nx)[..., 1:, :]
U = U.reshape(N_select, -1, nu)
"""Common to MCC+RMCC: apply dynamics constraints to trajectories"""
for _U, _X in zip(U, X):
# _X, _U have shapes (T, nx) and (T, nu) resp.
v_x, v_y = _X[..., 2], _X[..., 3]
u_x, u_y = _U[..., 0], _U[..., 1]
model.add_constraints(self.compute_velocity_constraints(v_x, v_y))
model.add_constraints(
self.compute_acceleration_constraints(u_x, u_y))
"""Common to MCC+RMCC: apply road boundaries constraints to trajectories"""
M_big, T, diag = self.__params.M_big, self.__params.T, self.__params.diag
for n in range(N_select):
# for each trajectory
for t in range(T):
for seg_idx, (A, b) in enumerate(segs_polytopes):
lhs = util.obj_matmul(A, X[n, t, :2]) - np.array(
M_big * (1 - Omicron[n, seg_idx, t]))
rhs = b + diag
model.add_constraints([l <= r for (l, r) in zip(lhs, rhs)])
model.add_constraint(np.sum(Omicron[n, :, t]) >= 1)
"""Apply collision constraints"""
traj_indices = params.subtraj_indices if self.__agent_type == "rmcc" else range(
N_select)
M_big, T, diag = self.__params.M_big, self.__params.T, self.__params.diag
for n, i in enumerate(traj_indices):
# select outerapprox. by index i
# if MCC then iterates through every combination of obstacles
A_union, b_union = A_unions[i], b_unions[i]
for t in range(T):
# for each timestep
As, bs = A_union[t], b_union[t]
for o, (A, b) in enumerate(zip(As, bs)):
lhs = util.obj_matmul(
A, X[n, t, :2]) + M_big * (1 - Delta[n, t, o])
rhs = b + diag
model.add_constraints([l >= r for (l, r) in zip(lhs, rhs)])
model.add_constraint(np.sum(Delta[n, t, o]) >= 1)
"""Common to MCC+RMCC: set up coinciding constraints"""
for t in range(0, self.__n_coincide):
for x1, x2 in util.pairwise(X[:, t]):
model.add_constraints([l == r for (l, r) in zip(x1, x2)])
"""Common to MCC+RMCC: set objective and run solver"""
start = params.x0[:2]
cost = np.sum((X[:, -1, :2] - goal)**2)
model.minimize(cost)
# model.print_information()
if self.log_cplex:
model.parameters.mip.display = 5
s = model.solve(log_output=True)
else:
model.solve()
# model.print_solution()
f = lambda x: x if isinstance(x, numbers.Number) else x.solution_value
U_star = util.obj_vectorize(f, U)
cost = cost.solution_value
X_star = util.obj_vectorize(f, X)
return util.AttrDict(cost=cost,
U_star=U_star,
X_star=X_star,
A_unions=A_unions,
b_unions=b_unions,
vertices=vertices,
start=start,
goal=goal)
def do_highlevel_control(self, params, ovehicles):
"""Decide parameters"""
# TODO: assign eps^k_o and beta^k_o to the vehicles.
# Skipping that for now.
"""Compute the approximation of the union of obstacle sets"""
# Find vertices of sampled obstacle sets
T, K, n_ov, truck = params.T, params.K, params.O, params.truck
vertices = np.empty((T, np.max(K), n_ov), dtype=object).tolist()
for ov_idx, ovehicle in enumerate(ovehicles):
for latent_idx in range(ovehicle.n_states):
for t in range(T):
ps = ovehicle.pred_positions[latent_idx]
yaws = ovehicle.pred_yaws[latent_idx]
n_p = ps.shape[0]
vertices[t][latent_idx][ov_idx] = np.zeros((n_p, 8))
for k in range(n_p):
vertices[t][latent_idx][ov_idx][
k] = get_vertices_from_center(
ps[k, t], yaws[k, t], truck.d)
plot_vertices = False
if plot_vertices:
latent_idx = 0
ov_idx = 0
fig, axes = plt.subplots(2, 2, figsize=(10, 10))
axes = axes.ravel()
for t, ax in zip(range(1, params.T, 2), axes):
# for ovehicle in scene.ovehicles:
X = vertices[t][latent_idx][ov_idx][:, 0:2].T
ax.scatter(X[0], X[1], c='r', s=2)
X = vertices[t][latent_idx][ov_idx][:, 2:4].T
ax.scatter(X[0], X[1], c='b', s=2)
X = vertices[t][latent_idx][ov_idx][:, 4:6].T
ax.scatter(X[0], X[1], c='g', s=2)
X = vertices[t][latent_idx][ov_idx][:, 6:8].T
ax.scatter(X[0], X[1], c='orange', s=2)
ax.set_title(f"t = {t}")
ax.set_aspect('equal')
plt.show()
"""Cluster the samples"""
T, K, n_ov = params.T, params.K, params.O
A_union = np.empty((
T,
np.max(K),
n_ov,
), dtype=object).tolist()
b_union = np.empty((
T,
np.max(K),
n_ov,
), dtype=object).tolist()
for ov_idx, ovehicle in enumerate(ovehicles):
for latent_idx in range(ovehicle.n_states):
for t in range(T):
yaws = ovehicle.pred_yaws[latent_idx][:, t]
vertices_k = vertices[t][latent_idx][ov_idx]
mean_theta_k = np.mean(yaws)
A_union_k, b_union_k = get_approx_union(
mean_theta_k, vertices_k)
A_union[t][latent_idx][ov_idx] = A_union_k
b_union[t][latent_idx][ov_idx] = b_union_k
"""Plot the overapproximation"""
plot_overapprox = False
if plot_overapprox:
fig, ax = plt.subplots()
t = 7
ov_idx = 0
ovehicle = ovehicles[ov_idx]
for latent_idx in range(ovehicle.n_states):
ps = ovehicle.pred_positions[latent_idx][:, t].T
ax.scatter(ps[0], ps[1], c='r', s=2)
try:
plot_h_polyhedron(ax,
A_union[t][latent_idx][ov_idx],
b_union[t][0][ov_idx],
ec='k',
alpha=0.3)
except scipy.spatial.qhull.QhullError as e:
print(e)
ax.set_aspect('equal')
plt.show()
"""Apply motion planning problem"""
model = docplex.mp.model.Model(name="proposed_problem")
L, T, K, Gamma, nu, nx = params.L, params.T, params.K, params.Gamma, params.nu, params.nx
u = np.array(model.continuous_var_list(nu * T, lb=-np.inf, name='u'),
dtype=object)
delta_tmp = model.binary_var_matrix(L * np.sum(K), T, name='delta')
delta = np.empty((
L * np.sum(K),
T,
), dtype=object)
for k, v in delta_tmp.items():
delta[k] = v
x_future = params.States_free_init + obj_matmul(Gamma, u)
big_M = 1000 # conservative upper-bound
x1 = x_future[nx::nx]
x2 = x_future[nx + 1::nx]
x3 = x_future[nx + 2::nx]
x4 = x_future[nx + 3::nx]
# 3rd and 4th states have COUPLED CONSTRAINTS
# x4 - c1*x3 <= - c3
# x4 - c1*x3 >= - c2
# x4 + c1*x3 <= c2
# x4 + c1*x3 >= c3
c1, c2, c3 = params.c1, params.c2, params.c3
model.add_constraints([z <= -c3 for z in x4 - c1 * x3])
model.add_constraints([z <= c2 for z in -x4 + c1 * x3])
model.add_constraints([z <= c2 for z in x4 + c1 * x3])
model.add_constraints([z <= -c3 for z in -x4 - c1 * x3])
u1 = u[0::nu]
u2 = u[1::nu]
# Inputs have COUPLED CONSTRAINTS:
# u2 - t1*u1 <= - t3
# u2 - t1*u1 >= - t2
# u2 + t1*u1 <= t2
# u2 + t1*u1 >= t3
t1, t2, t3 = params.t1, params.t2, params.t3
model.add_constraints([z <= -t3 for z in u2 - t1 * u1])
model.add_constraints([z <= t2 for z in -u2 + t1 * u1])
model.add_constraints([z <= t2 for z in u2 + t1 * u1])
model.add_constraints([z <= -t3 for z in -u2 - t1 * u1])
X = np.stack([x1, x2], axis=1)
T, K, diag = params.T, params.K, params.diag
for ov_idx, ovehicle in enumerate(ovehicles):
n_states = ovehicle.n_states
sum_clu = np.sum(K[:ov_idx])
for latent_idx in range(n_states):
for t in range(T):
A_obs = A_union[t][latent_idx][ov_idx]
b_obs = b_union[t][latent_idx][ov_idx]
indices = 4 * (sum_clu + latent_idx) + np.arange(0, 4)
lhs = obj_matmul(A_obs,
X[t]) + big_M * (1 - delta[indices, t])
rhs = b_obs + diag
model.add_constraints([l >= r for (l, r) in zip(lhs, rhs)])
model.add_constraint(np.sum(delta[indices, t]) >= 1)
# start from current vehicle position and minimize the objective
p_x, p_y, _ = carlautil.actor_to_location_ndarray(self.__ego_vehicle)
p_y = -p_y # need to flip about x-axis
if self.__goal.is_relative:
goal_x, goal_y = p_x + self.__goal.x, p_y + self.__goal.y
else:
goal_x, goal_y = self.__goal.x, self.__goal.y
start = np.array([p_x, p_y])
goal = np.array([goal_x, goal_y])
cost = (x1[-1] - goal_x)**2 + (x2[-1] - goal_y)**2
model.minimize(cost)
# model.print_information()
s = model.solve()
# model.print_solution()
u_star = np.array([ui.solution_value for ui in u])
cost = cost.solution_value
x1 = np.array([x1i.solution_value for x1i in x1])
x2 = np.array([x2i.solution_value for x2i in x2])
X_star = np.stack((x1, x2)).T
return util.AttrDict(cost=cost,
u_star=u_star,
X_star=X_star,
A_union=A_union,
b_union=b_union,
vertices=vertices,
start=start,
goal=goal)
def do_highlevel_control(self, params, ovehicles):
"""Decide parameters"""
# TODO: assign eps^k_o and beta^k_o to the vehicles.
# Skipping that for now.
vertices = self.__compute_vertices(params, ovehicles)
A_union, b_union = self.__compute_overapproximations(
vertices, params, ovehicles)
"""Apply motion planning problem"""
L, T, K, Gamma, nu, nx = self.__params.L, self.__params.T, params.K, \
self.__params.Gamma, self.__params.nu, self.__params.nx
u_max = self.__params.u_max
model = docplex.mp.model.Model(name="proposed_problem")
u = np.array(model.continuous_var_list(nu * T,
lb=-u_max,
ub=u_max,
name='u'),
dtype=object)
Delta = np.array(model.binary_var_list(L * np.sum(K) * T,
name='delta'),
dtype=object).reshape(np.sum(K), T, L)
X = (params.States_free_init + util.obj_matmul(Gamma, u)).reshape(
T + 1, nx)
X = X[1:]
U = u.reshape(T, nu)
"""Apply motion constraints"""
model.add_constraints(
self.compute_boundary_constraints(X[:, 0], X[:, 1]))
model.add_constraints(
self.compute_velocity_constraints(X[:, 2], X[:, 3]))
model.add_constraints(
self.compute_acceleration_constraints(U[:, 0], U[:, 1]))
"""Apply collision constraints"""
T, K, diag, M_big = self.__params.T, params.K, \
self.__params.diag, self.__params.M_big
for ov_idx, ovehicle in enumerate(ovehicles):
n_states = ovehicle.n_states
sum_clu = np.sum(K[:ov_idx])
for latent_idx in range(n_states):
for t in range(T):
A_obs = A_union[t][latent_idx][ov_idx]
b_obs = b_union[t][latent_idx][ov_idx]
indices = sum_clu + latent_idx
lhs = util.obj_matmul(
A_obs, X[t, :2]) + M_big * (1 - Delta[indices, t])
rhs = b_obs + diag
model.add_constraints([l >= r for (l, r) in zip(lhs, rhs)])
model.add_constraint(np.sum(Delta[indices, t]) >= 1)
"""Start from current vehicle position and minimize the objective"""
p_x, p_y, _ = carlautil.actor_to_location_ndarray(self.__ego_vehicle,
flip_y=True)
if self.__goal.is_relative:
goal_x, goal_y = p_x + self.__goal.x, p_y + self.__goal.y
else:
goal_x, goal_y = self.__goal.x, self.__goal.y
start = np.array([p_x, p_y])
goal = np.array([goal_x, goal_y])
cost = (X[-1, 0] - goal_x)**2 + (X[-1, 1] - goal_y)**2
model.minimize(cost)
if self.U_warmstarting is not None:
# Warm start inputs if past iteration was run.
warm_start = model.new_solution()
for i, u in enumerate(
self.U_warmstarting[self.__control_horizon:]):
warm_start.add_var_value(f"u_{2*i}", u[0])
warm_start.add_var_value(f"u_{2*i + 1}", u[1])
# add delta_0 as hotfix to MIP warmstart as it needs
# at least 1 integer value set.
warm_start.add_var_value('delta_0', 0)
model.add_mip_start(warm_start)
# model.print_information()
# model.parameters.read.datacheck = 1
if self.log_cplex:
model.parameters.mip.display = 2
s = model.solve(log_output=True)
else:
model.solve()
# model.print_solution()
f = lambda x: x if isinstance(x, numbers.Number) else x.solution_value
cost = cost.solution_value
U_star = util.obj_vectorize(f, U)
X_star = util.obj_vectorize(f, X)
return util.AttrDict(cost=cost,
U_star=U_star,
X_star=X_star,
A_union=A_union,
b_union=b_union,
vertices=vertices,
start=start,
goal=goal)
def do_highlevel_control(self, params, ovehicles):
"""Decide parameters"""
# TODO: assign eps^k_o and beta^k_o to the vehicles.
# Skipping that for now.
vertices = self.__compute_vertices(params, ovehicles)
A_union, b_union = self.__compute_overapproximations(
vertices, params, ovehicles)
"""Apply motion planning problem"""
model = docplex.mp.model.Model(name="proposed_problem")
L, T, K, Gamma, nu, nx = self.__params.L, self.__params.T, params.K, \
self.__params.Gamma, self.__params.nu, self.__params.nx
u = np.array(model.continuous_var_list(nu * T, lb=-np.inf, name='u'),
dtype=object)
delta_tmp = model.binary_var_matrix(L * np.sum(K), T, name='delta')
delta = np.empty((
L * np.sum(K),
T,
), dtype=object)
for k, v in delta_tmp.items():
delta[k] = v
x_future = params.States_free_init + obj_matmul(Gamma, u)
# TODO: hardcoded value, need to specify better
big_M = 1000 # conservative upper-bound
x1 = x_future[nx::nx]
x2 = x_future[nx + 1::nx]
x3 = x_future[nx + 2::nx]
x4 = x_future[nx + 3::nx]
u1 = u[0::nu]
u2 = u[1::nu]
model.add_constraints(self.compute_boundary_constraints(x1, x2))
model.add_constraints(self.compute_velocity_constraints(x3, x4))
model.add_constraints(self.compute_acceleration_constraints(u1, u2))
X = np.stack([x1, x2], axis=1)
T, K, diag = self.__params.T, params.K, self.__params.diag
for ov_idx, ovehicle in enumerate(ovehicles):
n_states = ovehicle.n_states
sum_clu = np.sum(K[:ov_idx])
for latent_idx in range(n_states):
for t in range(T):
A_obs = A_union[t][latent_idx][ov_idx]
b_obs = b_union[t][latent_idx][ov_idx]
indices = 4 * (sum_clu + latent_idx) + np.arange(0, 4)
lhs = obj_matmul(A_obs,
X[t]) + big_M * (1 - delta[indices, t])
rhs = b_obs + diag
model.add_constraints([l >= r for (l, r) in zip(lhs, rhs)])
model.add_constraint(np.sum(delta[indices, t]) >= 1)
# start from current vehicle position and minimize the objective
p_x, p_y, _ = carlautil.actor_to_location_ndarray(self.__ego_vehicle)
p_y = -p_y # need to flip about x-axis
if self.__goal.is_relative:
goal_x, goal_y = p_x + self.__goal.x, p_y + self.__goal.y
else:
goal_x, goal_y = self.__goal.x, self.__goal.y
start = np.array([p_x, p_y])
goal = np.array([goal_x, goal_y])
cost = (x1[-1] - goal_x)**2 + (x2[-1] - goal_y)**2
model.minimize(cost)
# model.print_information()
# model.parameters.read.datacheck = 1
if self.log_cplex:
model.parameters.mip.display = 5
s = model.solve(log_output=True)
else:
model.solve()
# model.print_solution()
u_star = np.array([ui.solution_value for ui in u])
cost = cost.solution_value
x1 = np.array([x1i.solution_value for x1i in x1])
x2 = np.array([x2i.solution_value for x2i in x2])
X_star = np.stack((x1, x2)).T
return util.AttrDict(cost=cost,
u_star=u_star,
X_star=X_star,
A_union=A_union,
b_union=b_union,
vertices=vertices,
start=start,
goal=goal)
def do_highlevel_control(self, params, ovehicles):
"""Decide parameters."""
"""Compute road segments and goal."""
(segs_polytopes, goal,
seg_psi_bounds) = self.compute_segs_polytopes_and_goal(params)
"""Apply motion planning problem"""
N_select = params.N_select
x_bar, u_bar, Gamma = params.x_bar, params.u_bar, params.Gamma
nx, nu = params.nx, params.nu
T = self.__control_horizon
max_a, min_a = self.__params.max_a, self.__params.min_a
max_delta = self.__params.max_delta
"""Model, control and state variables"""
model = docplex.mp.model.Model(name="proposed_problem")
min_u = np.vstack((np.full(T, min_a), np.full(T, -max_delta))).T
min_u = np.repeat(min_u[None], N_select, axis=0).ravel()
max_u = np.vstack((np.full(T, max_a), np.full(T, max_delta))).T
max_u = np.repeat(max_u[None], N_select, axis=0).ravel()
# Slack variables for control
u = model.continuous_var_list(N_select * nu * T,
lb=min_u,
ub=max_u,
name="u")
# U has shape (N_select, T*nu)
U = np.array(u, dtype=object).reshape(N_select, -1)
U_delta = U - u_bar
x = util.obj_matmul(U_delta, Gamma.T) + x_bar
# State variables x, y, psi, v
X = x.reshape(N_select, T, nx)
# Control variables a, delta
U = U.reshape(N_select, T, nu)
"""Apply state constraints"""
for _X in X:
model.add_constraints(self.compute_state_constraints(params, _X))
"""Apply road boundary constraints"""
if self.road_boundary_constraints:
n_segs = len(segs_polytopes)
# Slack variables from road obstacles
Omicron = model.binary_var_list(N_select * n_segs * T,
name="omicron")
Omicron = np.array(Omicron,
dtype=object).reshape(N_select, n_segs, T)
M_big = self.__params.M_big
# diag = self.__params.diag
psi_0 = params.initial_state.world[2]
T = self.__control_horizon
for n in range(N_select):
for t in range(T):
for seg_idx, (A, b) in enumerate(segs_polytopes):
lhs = util.obj_matmul(A, X[n, t, :2]) - np.array(
M_big * (1 - Omicron[n, seg_idx, t]))
rhs = b # - diag
"""Constraints on road boundaries"""
model.add_constraints(
[l <= r for (l, r) in zip(lhs, rhs)])
"""Constraints on angle boundaries"""
if self.angle_boundary_constraints:
lhs = X[n, t, 2] - psi_0 - M_big * (
1 - Omicron[seg_idx, t])
model.add_constraint(
lhs <= seg_psi_bounds[n, seg_idx][1])
lhs = X[n, t, 2] - psi_0 + M_big * (
1 - Omicron[n, seg_idx, t])
model.add_constraint(
lhs >= seg_psi_bounds[seg_idx][0])
model.add_constraint(np.sum(Omicron[n, :, t]) >= 1)
"""Apply vehicle collision constraints"""
if params.O > 0:
vertices = self.__compute_vertices(params, ovehicles)
A_unions, b_unions = self.__compute_overapproximations(
vertices, params, ovehicles)
O, L = params.O, self.__params.L
# Slack variables for vehicle obstacles
Delta = model.binary_var_list(O * L * N_select * T, name='delta')
Delta = np.array(Delta, dtype=object).reshape(N_select, T, O, L)
M_big = self.__params.M_big
diag = self.__params.diag
# for n, i in enumerate(params.subtraj_indices):
# # select outerapprox. by index i
# A_union, b_union = A_unions[i], b_unions[i]
# for t in range(T):
# As, bs = A_union[t], b_union[t]
# for o, (A, b) in enumerate(zip(As, bs)):
# lhs = util.obj_matmul(A, X[n,t,:2]) + M_big*(1 - Delta[n,t,o])
# rhs = b + diag
# model.add_constraints([l >= r for (l,r) in zip(lhs, rhs)])
# model.add_constraint(np.sum(Delta[n,t,o]) >= 1)
for n in range(N_select):
# select outerapprox. by index n
A_union, b_union = A_unions[n], b_unions[n]
for t in range(T):
As, bs = A_union[t], b_union[t]
for o, (A, b) in enumerate(zip(As, bs)):
lhs = util.obj_matmul(
A, X[n, t, :2]) + M_big * (1 - Delta[n, t, o])
rhs = b + diag
model.add_constraints(
[l >= r for (l, r) in zip(lhs, rhs)])
model.add_constraint(np.sum(Delta[n, t, o]) >= 1)
else:
vertices = A_unions = b_unions = None
"""Set up coinciding constraints"""
for t in range(0, self.__n_coincide):
for u1, u2 in util.pairwise(U[:, t]):
model.add_constraints([l == r for (l, r) in zip(u1, u2)])
"""Compute and minimize objective"""
cost = 0
for _U, _X in zip(U, X):
cost += self.compute_objective(_X, _U, goal)
model.minimize(cost)
# TODO: add warmstarting to MCC
# if self.road_boundary_constraints and self.__U_warmstarting is not None:
# # Warm start inputs if past iteration was run.
# warm_start = model.new_solution()
# for i, u in enumerate(self.__U_warmstarting[self.__step_horizon :]):
# warm_start.add_var_value(f"u_{2*i}", u[0])
# warm_start.add_var_value(f"u_{2*i + 1}", u[1])
# # add omicron_0 as hotfix to MIP warmstart as it needs
# # at least 1 integer value set.
# warm_start.add_var_value("omicron_0", 1)
# model.add_mip_start(
# warm_start, write_level=docplex.mp.constants.WriteLevel.AllVars
# )
# model.print_information()
# model.parameters.read.datacheck = 1
if self.log_cplex:
model.parameters.mip.display = 2
s = model.solve(log_output=True)
else:
model.parameters.mip.display = 0
model.solve()
# model.print_solution()
"""Extract solution"""
cost = cost.solution_value
f = lambda x: x if isinstance(x, numbers.Number) else x.solution_value
U_star = util.obj_vectorize(f, U)
X_star = util.obj_vectorize(f, X)
return util.AttrDict(cost=cost,
U_star=U_star,
X_star=X_star,
goal=goal,
A_unions=A_unions,
b_unions=b_unions,
vertices=vertices)
def do_highlevel_control(self, params):
"""Decide parameters."""
segs_polytopes, goal, seg_psi_bounds = self.compute_segs_polytopes_and_goal(
params)
"""Apply motion planning problem"""
n_segs = len(segs_polytopes)
Gamma, nu, nx = params.Gamma, params.nu, params.nx
T = self.__control_horizon
max_a, min_a = self.__params.max_a, self.__params.min_a
max_delta = self.__params.max_delta
model = docplex.mp.model.Model(name="proposed_problem")
min_u = np.vstack((np.full(T, min_a), np.full(T,
-max_delta))).T.ravel()
max_u = np.vstack((np.full(T, max_a), np.full(T, max_delta))).T.ravel()
# Slack variables for control
u = np.array(model.continuous_var_list(nu * T,
lb=min_u,
ub=max_u,
name='u'),
dtype=object)
# State variables x, y, psi, v
X = (params.initial_rollout.local + util.obj_matmul(Gamma, u)).reshape(
T + 1, nx)
X = params.transform(X)
X = X[1:]
# Control variables a, delta
U = u.reshape(T, nu)
"""Apply motion dynamics constraints"""
model.add_constraints(self.compute_dyamics_constraints(
params, X[:, 3]))
"""Apply road boundary constraints"""
if self.road_boundary_constraints:
# Slack variables from road obstacles
Omicron = np.array(model.binary_var_list(n_segs * T,
name="omicron"),
dtype=object).reshape(n_segs, T)
M_big, diag = self.__params.M_big, self.__params.diag
psi_0 = params.initial_state.world[2]
for t in range(self.__control_horizon):
for seg_idx, (A, b) in enumerate(segs_polytopes):
lhs = util.obj_matmul(A, X[t, :2]) \
- np.array(M_big*(1 - Omicron[seg_idx, t]))
rhs = b # - diag
"""Constraints on road boundaries"""
model.add_constraints([l <= r for (l, r) in zip(lhs, rhs)])
"""Constraints on angle boundaries"""
if self.angle_boundary_constraints:
lhs = X[t,
2] - psi_0 - M_big * (1 - Omicron[seg_idx, t])
model.add_constraint(lhs <= seg_psi_bounds[seg_idx][1])
lhs = X[t,
2] - psi_0 + M_big * (1 - Omicron[seg_idx, t])
model.add_constraint(lhs >= seg_psi_bounds[seg_idx][0])
model.add_constraint(np.sum(Omicron[:, t]) >= 1)
"""Compute and minimize objective"""
cost = self.compute_objective(X, U, goal)
model.minimize(cost)
if self.road_boundary_constraints and self.__U_warmstarting is not None:
# Warm start inputs if past iteration was run.
warm_start = model.new_solution()
for i, u in enumerate(self.__U_warmstarting[self.__step_horizon:]):
warm_start.add_var_value(f"u_{2*i}", u[0])
warm_start.add_var_value(f"u_{2*i + 1}", u[1])
# add omicron_0 as hotfix to MIP warmstart as it needs
# at least 1 integer value set.
warm_start.add_var_value("omicron_0", 1)
model.add_mip_start(
warm_start,
write_level=docplex.mp.constants.WriteLevel.AllVars)
# model.print_information()
# model.parameters.read.datacheck = 1
if self.log_cplex:
model.parameters.mip.display = 2
s = model.solve(log_output=True)
else:
model.parameters.mip.display = 0
model.solve()
# model.print_solution()
cost = cost.solution_value
f = lambda x: x if isinstance(x, numbers.Number) else x.solution_value
U_star = util.obj_vectorize(f, U)
X_star = util.obj_vectorize(f, X)
return util.AttrDict(cost=cost,
U_star=U_star,
X_star=X_star,
goal=goal)
def solve_MILP_cplex(program, verbose=True):
"""solve MILP using cplex
## Inputs
- program: dict of milp program generated using initialize_program()
- verbose: bool of whether to be verbose
"""
import docplex.mp.model
start = time.time()
model = docplex.mp.model.Model(name='model')
vtypes = {
bool: model.binary_var,
int: model.integer_var,
float: model.continuous_var,
}
# add variables
variables = {}
for variable_name, variable in program['variables'].items():
variable_type = variable['type']
kwargs = {}
if variable['lower_bound'] is not None:
kwargs['lb'] = variable['lower_bound']
if variable['upper_bound'] is not None:
kwargs['ub'] = variable['upper_bound']
variables[variable_name] = vtypes[variable_type](name=variable_name,
**kwargs)
# add constraints
for A, b, operator in [
[program['constraints']['A_lt'], program['constraints']['b_lt'], '<='],
[program['constraints']['A_eq'], program['constraints']['b_eq'], '='],
]:
for coefficients, constant in zip(A, b):
expression = model.sum(
coefficient * variables[variable_name]
for variable_name, coefficient in coefficients.items())
if operator == '<=':
model.add_constraint(expression <= constant)
elif operator == '=':
model.add_constraint(expression == constant)
else:
raise Exception(operator)
# add objective function
expression = model.sum(
coefficient * variables[variable_name]
for variable_name, coefficient in program['cost_function'].items())
model.minimize(expression)
# solve
model.solve()
return {
'variables': {
variable_name: variable.solution_value
for variable_name, variable in variables.items()
},
'objective': model.objective_value,
'program': program,
'model': model,
'time': time.time() - start,
}
def do_highlevel_control(self, params, ovehicles):
"""Decide parameters."""
"""Compute road segments and goal."""
segs_polytopes, goal, seg_psi_bounds = self.compute_segs_polytopes_and_goal(
params
)
"""Apply motion planning problem"""
x_bar, u_bar, Gamma = params.x_bar, params.u_bar, params.Gamma
nx, nu = params.nx, params.nu
T = self.__control_horizon
max_a, min_a = self.__params.max_a, self.__params.min_a
max_delta = self.__params.max_delta
"""Model, control and state variables"""
model = docplex.mp.model.Model(name="proposed_problem")
min_u = np.vstack((np.full(T, min_a), np.full(T, -max_delta))).T.ravel()
max_u = np.vstack((np.full(T, max_a), np.full(T, max_delta))).T.ravel()
# Slack variables for control
u = model.continuous_var_list(nu * T, lb=min_u, ub=max_u, name="u")
u = np.array(u, dtype=object)
u_delta = u - u_bar
x = util.obj_matmul(Gamma, u_delta) + x_bar
# State variables x, y, psi, v
X = x.reshape(T, nx)
# Control variables a, delta
U = u.reshape(T, nu)
"""Apply state constraints"""
model.add_constraints(self.compute_state_constraints(params, X))
"""Apply road boundary constraints"""
if self.road_boundary_constraints:
n_segs = len(segs_polytopes)
# Slack variables from road obstacles
Omicron = model.binary_var_list(n_segs*T, name="omicron")
Omicron = np.array(Omicron, dtype=object).reshape(n_segs, T)
M_big = self.__params.M_big
# diag = self.__params.diag
psi_0 = params.initial_state.world[2]
T = self.__control_horizon
for t in range(T):
for seg_idx, (A, b) in enumerate(segs_polytopes):
lhs = util.obj_matmul(A, X[t, :2]) - np.array(
M_big * (1 - Omicron[seg_idx, t])
)
rhs = b # - diag
"""Constraints on road boundaries"""
model.add_constraints([l <= r for (l, r) in zip(lhs, rhs)])
"""Constraints on angle boundaries"""
if self.angle_boundary_constraints:
lhs = X[t, 2] - psi_0 - M_big * (1 - Omicron[seg_idx, t])
model.add_constraint(lhs <= seg_psi_bounds[seg_idx][1])
lhs = X[t, 2] - psi_0 + M_big * (1 - Omicron[seg_idx, t])
model.add_constraint(lhs >= seg_psi_bounds[seg_idx][0])
model.add_constraint(np.sum(Omicron[:, t]) >= 1)
"""Apply vehicle collision constraints"""
if params.O > 0:
vertices = self.__compute_vertices(params, ovehicles)
A_union, b_union = self.__compute_overapproximations(vertices, params, ovehicles)
L, K = self.__params.L, params.K
# Slack variables for vehicle obstacles
Delta = model.binary_var_list(L*np.sum(K)*T, name='delta')
Delta = np.array(Delta, dtype=object).reshape(np.sum(K), T, L)
M_big = self.__params.M_big
diag = self.__params.diag
for ov_idx, ovehicle in enumerate(ovehicles):
n_states = ovehicle.n_states
sum_clu = np.sum(K[:ov_idx])
for latent_idx in range(n_states):
for t in range(self.__control_horizon):
A_obs = A_union[t][latent_idx][ov_idx]
b_obs = b_union[t][latent_idx][ov_idx]
indices = sum_clu + latent_idx
lhs = util.obj_matmul(A_obs, X[t, :2]) + M_big*(1 - Delta[indices, t])
rhs = b_obs + diag
model.add_constraints([l >= r for (l,r) in zip(lhs, rhs)])
model.add_constraint(np.sum(Delta[indices, t]) >= 1)
else:
vertices = A_union = b_union = None
"""Compute and minimize objective"""
cost = self.compute_objective(X, U, goal)
model.minimize(cost)
if self.road_boundary_constraints and self.__U_warmstarting is not None:
# Warm start inputs if past iteration was run.
warm_start = model.new_solution()
for i, u in enumerate(self.__U_warmstarting[self.__step_horizon :]):
warm_start.add_var_value(f"u_{2*i}", u[0])
warm_start.add_var_value(f"u_{2*i + 1}", u[1])
# add omicron_0 as hotfix to MIP warmstart as it needs
# at least 1 integer value set.
warm_start.add_var_value("omicron_0", 1)
model.add_mip_start(
warm_start, write_level=docplex.mp.constants.WriteLevel.AllVars
)
# model.print_information()
# model.parameters.read.datacheck = 1
if self.log_cplex:
model.parameters.mip.display = 2
s = model.solve(log_output=True)
else:
model.parameters.mip.display = 0
model.solve()
# model.print_solution()
"""Extract solution"""
try:
cost = cost.solution_value
f = lambda x: x if isinstance(x, numbers.Number) else x.solution_value
U_star = util.obj_vectorize(f, U)
X_star = util.obj_vectorize(f, X)
return util.AttrDict(
cost=cost, U_star=U_star, X_star=X_star, goal=goal,
A_union=A_union, b_union=b_union, vertices=vertices
), None
except docplex.mp.utils.DOcplexException as e:
# TODO: check model for infeasible solution
# docplex.util.environment.logger:environment.py:627 Notify end solve,
# status=JobSolveStatus.INFEASIBLE_SOLUTION, solve_time=None
return util.AttrDict(
cost=None, U_star=None, X_star=None,
goal=goal, A_union=A_union, b_union=b_union, vertices=vertices
), InSimulationException("Optimizer failed to find a solution")