def at_bounding_box_to_label(self, actor):
actor_location = carlautil.actor_to_location_ndarray(actor)
dist = np.linalg.norm(self.CENTER_COORDS - actor_location)
if dist < self.REGION_RADIUS:
return BoundingRegionLabel.BOUNDED
else:
return BoundingRegionLabel.NONE
def __compute_prediction_controls(self, frame):
pred_result = self.do_prediction(frame)
ovehicles = self.make_ovehicles(pred_result)
params = self.make_highlevel_params(ovehicles)
ctrl_result = self.do_highlevel_control(params, ovehicles)
"""Get trajectory"""
trajectory = []
x, y, _ = carlautil.actor_to_location_ndarray(self.__ego_vehicle)
X = np.concatenate((np.array([x, y])[None], ctrl_result.X_star))
n_steps = X.shape[0]
headings = []
for t in range(1, n_steps):
x, y = X[t]
y = -y
yaw = np.arctan2(X[t, 1] - X[t - 1, 1], X[t, 0] - X[t - 1, 0])
headings.append(yaw)
yaw = util.reflect_radians_about_x_axis(yaw)
transform = carla.Transform(carla.Location(x=x, y=y),
carla.Rotation(yaw=yaw))
trajectory.append(transform)
"""Plot scenario"""
ctrl_result.headings = headings
lon, lat, _ = carlautil.actor_to_bbox_ndarray(self.__ego_vehicle)
ego_bbox = [lon, lat]
plot_lcss_prediction(pred_result, ovehicles, params, ctrl_result,
self.__prediction_horizon, ego_bbox)
return trajectory
def __plot_overapproximations(self, params, ovehicles, vertices, A_union,
b_union):
fig, axes = plt.subplots(1, 2, figsize=(15, 7))
ax = axes[1]
X = vertices[-1][0][0][:, 0:2].T
ax.scatter(X[0], X[1], color='r', s=2)
X = vertices[-1][0][0][:, 2:4].T
ax.scatter(X[0], X[1], color='b', s=2)
X = vertices[-1][0][0][:, 4:6].T
ax.scatter(X[0], X[1], color='g', s=2)
X = vertices[-1][0][0][:, 6:8].T
ax.scatter(X[0], X[1], color='m', s=2)
A = A_union[-1][0][0]
b = b_union[-1][0][0]
try:
util.npu.plot_h_polyhedron(ax, A, b, fc='none', ec='k')
except scipy.spatial.qhull.QhullError as e:
print(f"Failed to plot polyhedron of OV")
x, y, _ = carlautil.actor_to_location_ndarray(self.__ego_vehicle,
flip_y=True)
ax = axes[0]
ax.scatter(x, y, marker='*', c='k', s=100)
ovehicle_colors = get_ovehicle_color_set(
[ov.n_states for ov in ovehicles])
for ov_idx, ovehicle in enumerate(ovehicles):
for latent_idx in range(ovehicle.n_states):
logging.info(
f"Plotting OV {ov_idx} latent value {latent_idx}.")
color = ovehicle_colors[ov_idx][latent_idx]
for t in range(self.__prediction_horizon):
X = vertices[t][latent_idx][ov_idx][:, 0:2].T
ax.scatter(X[0], X[1], color=color, s=2)
X = vertices[t][latent_idx][ov_idx][:, 2:4].T
ax.scatter(X[0], X[1], color=color, s=2)
X = vertices[t][latent_idx][ov_idx][:, 4:6].T
ax.scatter(X[0], X[1], color=color, s=2)
X = vertices[t][latent_idx][ov_idx][:, 6:8].T
ax.scatter(X[0], X[1], color=color, s=2)
A = A_union[t][latent_idx][ov_idx]
b = b_union[t][latent_idx][ov_idx]
try:
util.npu.plot_h_polyhedron(ax,
A,
b,
fc='none',
ec=color) #, alpha=0.3)
except scipy.spatial.qhull.QhullError as e:
print(
f"Failed to plot polyhedron of OV {ov_idx} latent value {latent_idx} timestep t={t}"
)
for ax in axes:
ax.set_aspect('equal')
filename = f"agent{self.__ego_vehicle.id}_frame{params.frame}_overapprox"
fig.savefig(os.path.join('out', f"{filename}.png"))
fig.clf()
def at_slope_to_label(self, actor):
"""Check wheter actor (i.e. vehicle) is near a slope,
returning a ScenarioSlopeLabel.
TODO: make wp_locations array size smaller after debugging.
Don't need to check non-sloped waypoints.
"""
actor_location = carlautil.actor_to_location_ndarray(actor)
actor_xy = actor_location[:2]
actor_z = actor_location[-1]
"""Want to ignore waypoints above and below (i.e. in the case actor is on a bridge)."""
upperbound_z = actor.bounding_box.extent.z * 2
lowerbound_z = -1
xy_distances_to_wps = np.linalg.norm(self.wp_locations[:, :2] -
actor_xy,
axis=1)
z_displacement_to_wps = self.wp_locations[:, -1] - actor_z
"""get waypoints close to vehicle filter"""
wps_filter = np.logical_and(
xy_distances_to_wps < self.SLOPE_FIND_RADIUS,
np.logical_and(z_displacement_to_wps < upperbound_z,
z_displacement_to_wps > lowerbound_z))
#####
"""obtain extra slope information"""
wp_pitches = self.wp_pitches[wps_filter]
if wp_pitches.size == 0:
max_wp_pitch = 0.0
else:
wp_pitches = np.min(np.vstack((
wp_pitches,
np.abs(wp_pitches - 360.),
)),
axis=0)
max_wp_pitch = np.max(wp_pitches)
#####
if self._debug:
nearby_slopes = np.logical_and(self.wp_is_sloped == True,
wps_filter)
for wp_location in self.wp_locations[nearby_slopes]:
loc = carlautil.ndarray_to_location(wp_location)
carlautil.debug_point(self.carla_world, loc)
if np.any(self.wp_is_sloped[wps_filter]):
return ScenarioSlopeLabel.SLOPES, max_wp_pitch
else:
return ScenarioSlopeLabel.NONE, max_wp_pitch
def at_intersection_to_label(self, actor):
"""Retrieve the label corresponding to the actor's location in the
map based on proximity to intersections.
Parameters
----------
actor : carla.Actor
Returns
"""
actor_location = carlautil.actor_to_location_ndarray(actor)
distances_to_uncontrolled = np.linalg.norm(
self.uncontrolled_junction_locations - actor_location, axis=1)
if np.any(distances_to_uncontrolled < self.TLIGHT_DETECT_RADIUS):
return ScenarioIntersectionLabel.UNCONTROLLED
distances_to_controlled = np.linalg.norm(
self.controlled_junction_locations - actor_location, axis=1)
if np.any(distances_to_controlled < self.TLIGHT_DETECT_RADIUS):
return ScenarioIntersectionLabel.CONTROLLED
return ScenarioIntersectionLabel.NONE
def make_local_params(self, ovehicles):
"""Get Local LCSS parameters that are environment dependent."""
params = util.AttrDict()
params.O = len(ovehicles) # number of obstacles
params.K = np.zeros(params.O, dtype=int)
for idx, vehicle in enumerate(ovehicles):
params.K[idx] = vehicle.n_states
p_0_x, p_0_y, _ = carlautil.actor_to_location_ndarray(
self.__ego_vehicle)
p_0_y = -p_0_y # need to flip about x-axis
v_0_x, v_0_y, _ = carlautil.actor_to_velocity_ndarray(
self.__ego_vehicle)
v_0_y = -v_0_y # need to flip about x-axis
# x0 : np.array
# Initial state
x0 = np.array([p_0_x, p_0_y, v_0_x, v_0_y])
A, T = self.__params.A, self.__params.T
# States_free_init has shape (nx*(T+1))
params.States_free_init = np.concatenate(
[np.linalg.matrix_power(A, t) @ x0 for t in range(T + 1)])
return params
def make_local_params(self, ovehicles):
"""Get Local LCSS parameters that are environment dependent."""
params = util.AttrDict()
# O - number of obstacles
params.O = len(ovehicles)
# K - for each o=1,...,O K[o] is the number of outer approximations for vehicle o
params.K = np.zeros(params.O, dtype=int)
for idx, vehicle in enumerate(ovehicles):
params.K[idx] = vehicle.n_states
p_0_x, p_0_y, _ = carlautil.actor_to_location_ndarray(
self.__ego_vehicle, flip_y=True)
v_0_x, v_0_y, _ = carlautil.actor_to_velocity_ndarray(
self.__ego_vehicle, flip_y=True)
# x0 : np.array
# Initial state
x0 = np.array([p_0_x, p_0_y, v_0_x, v_0_y])
params.x0 = x0
A, T = self.__params.A, self.__params.T
# States_free_init has shape (nx*(T+1))
params.States_free_init = np.concatenate(
[np.linalg.matrix_power(A, t) @ x0 for t in range(T + 1)])
return params
def __plot_vertices(self, ovehicles, vertices):
fig, axes = plt.subplots(1, 2, figsize=(15, 7))
ax = axes[1]
X = vertices[-1][0][0][:, 0:2].T
ax.scatter(X[0], X[1], c='r', s=2)
X = vertices[-1][0][0][:, 2:4].T
ax.scatter(X[0], X[1], c='b', s=2)
X = vertices[-1][0][0][:, 4:6].T
ax.scatter(X[0], X[1], c='g', s=2)
X = vertices[-1][0][0][:, 6:8].T
ax.scatter(X[0], X[1], c='m', s=2)
x, y, _ = carlautil.actor_to_location_ndarray(self.__ego_vehicle,
flip_y=True)
ax = axes[0]
ax.scatter(x, y, marker='*', c='k', s=100)
ovehicle_colors = get_ovehicle_color_set(
[ov.n_states for ov in ovehicles])
for ov_idx, ovehicle in enumerate(ovehicles):
for latent_idx in range(ovehicle.n_states):
logging.info(
f"Plotting OV {ov_idx} latent value {latent_idx}.")
color = ovehicle_colors[ov_idx][latent_idx]
for t in range(self.__params.T):
X = vertices[t][latent_idx][ov_idx][:, 0:2].T
ax.scatter(X[0], X[1], color=color, s=2)
X = vertices[t][latent_idx][ov_idx][:, 2:4].T
ax.scatter(X[0], X[1], color=color, s=2)
X = vertices[t][latent_idx][ov_idx][:, 4:6].T
ax.scatter(X[0], X[1], color=color, s=2)
X = vertices[t][latent_idx][ov_idx][:, 6:8].T
ax.scatter(X[0], X[1], color=color, s=2)
for ax in axes:
ax.set_aspect('equal')
plt.show()
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 make_highlevel_params(self, ovehicles):
p_0_x, p_0_y, _ = carlautil.actor_to_location_ndarray(
self.__ego_vehicle)
p_0_y = -p_0_y # need to flip about x-axis
v_0_x, v_0_y, _ = carlautil.actor_to_velocity_ndarray(
self.__ego_vehicle)
v_0_y = -v_0_y # need to flip about x-axis
"""Get LCSS parameters"""
# TODO: refactor this long chain
params = util.AttrDict()
params.Ts = 0.5
# A, sys.A both have shape (4, 4)
A = np.diag([1, 1], k=2)
# B, sys.B both have shape (4, 2)
B = np.concatenate((
np.diag([0, 0]),
np.diag([1, 1]),
))
# C has shape (2, 4)
C = np.concatenate((
np.diag([1, 1]),
np.diag([0, 0]),
), axis=1)
# D has shape (2, 2)
D = np.diag([0, 0])
sys = control.matlab.c2d(control.matlab.ss(A, B, C, D), params.Ts)
params.A = np.array(sys.A)
params.B = np.array(sys.B)
# number of state variables x, number of input variables u
# nx = 4, nu = 2
params.nx, params.nu = sys.B.shape
# TODO: remove car, truck magic numbers
# only truck.d is used
params.car = util.AttrDict()
params.car.d = np.array([4.5, 2.5])
# truck should be renamed to car, and only truck.d is used
params.truck = util.AttrDict()
params.truck.d = [4.5, 2.5]
# params.x0 : np.array
# Initial state
params.x0 = np.array([p_0_x, p_0_y, v_0_x, v_0_y])
params.diag = np.sqrt(params.car.d[1]**2 + params.car.d[0]**2) / 2.
# TODO: remove constraint of v, u magic numbers
# Vehicle dynamics restrictions
params.vmin = np.array([0 / 3.6, -20 / 3.6]) # lower bounds
params.vmax = np.array([80 / 3.6, 20 / 3.6]) # upper bounds
params.umin = np.array([-10, -5])
params.umax = np.array([3, 5])
# Prediction parameters
params.T = self.__prediction_horizon
params.O = len(ovehicles) # number of obstacles
params.L = 4 # number of faces of obstacle sets
params.K = np.zeros(params.O, dtype=int)
for idx, vehicle in enumerate(ovehicles):
params.K[idx] = vehicle.n_states
"""
3rd and 4th states have COUPLED CONSTRAINTS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% x4 - c1*x3 <= - c3
% x4 - c1*x3 >= - c2
% x4 + c1*x3 <= c2
% x4 + c1*x3 >= c3
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
"""
vmin, vmax = params.vmin, params.vmax
# they work only assuming vmax(2) = -vmin(2)
params.c1 = vmax[1] / (0.5 * (vmax[0] - vmin[0]))
params.c2 = params.c1 * vmax[0]
params.c3 = params.c1 * vmin[0]
"""
Inputs have COUPLED CONSTRAINTS
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% u2 - t1*u1 <= - t3
% u2 - t1*u1 >= - t2
% u2 + t1*u1 <= t2
% u2 + t1*u1 >= t3
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
"""
umin, umax = params.umin, params.umax
params.t1 = umax[1] / (0.5 * (umax[0] - umin[0]))
params.t2 = params.t1 * umax[0]
params.t3 = params.t1 * umin[0]
A, B, T, nx, nu = params.A, params.B, params.T, params.nx, params.nu
# C1 has shape (nx, T*nx)
C1 = np.zeros((
nx,
T * nx,
))
# C2 has shape (nx*(T - 1), nx*(T-1)) as A has shape (nx, nx)
C2 = np.kron(np.eye(T - 1), A)
# C3 has shape (nx*(T - 1), nx)
C3 = np.zeros((
(T - 1) * nx,
nx,
))
# C, Abar have shape (nx*T, nx*T)
C = np.concatenate((
C1,
np.concatenate((
C2,
C3,
), axis=1),
), axis=0)
Abar = np.eye(T * nx) - C
# Bbar has shape (nx*T, nu*T) as B has shape (nx, nu)
Bbar = np.kron(np.eye(T), B)
# Gamma has shape (nx*(T + 1), nu*T) as Abar\Bbar has shape (nx*T, nu*T)
Gamma = np.concatenate((
np.zeros((
nx,
T * nu,
)),
np.linalg.solve(Abar, Bbar),
))
params.Abar = Abar
params.Bbar = Bbar
params.Gamma = Gamma
A, T, x0 = params.A, params.T, params.x0
# States_free_init has shape (nx*(T+1))
params.States_free_init = np.concatenate(
[np.linalg.matrix_power(A, t) @ x0 for t in range(T + 1)])
return params
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)