def make_local_params(self, frame):
"""Get the linearized bicycle model using the vehicle's
immediate orientation."""
params = util.AttrDict()
params.frame = frame
"""Dynamics parameters"""
p_0_x, p_0_y, _ = carlautil.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)
# initial_state - state at current frame in world/local coordinates
# Local coordinates has initial position and heading at 0
initial_state = util.AttrDict(
world=np.array([p_0_x, p_0_y, v_0_x, v_0_y]))
params.initial_state = initial_state
A, B = self.__params.A, self.__params.B
nx, nu = self.__params.nx, self.__params.nu
T = self.__control_horizon
# make state computation account for initial position and velocity
initial_rollout = np.concatenate([
np.linalg.matrix_power(A, t) @ initial_state.world
for t in range(T + 1)
])
params.initial_rollout = util.AttrDict(world=initial_rollout)
return params
def make_local_params(self, frame, ovehicles):
"""Get the local optimization parameters used for current MPC step."""
"""Get parameters to construct control and state variables."""
params = util.AttrDict()
params.frame = frame
p_0_x, p_0_y, _ = carlautil.to_location_ndarray(self.__ego_vehicle, flip_y=True)
_, psi_0, _ = carlautil.actor_to_rotation_ndarray(
self.__ego_vehicle, flip_y=True
)
v_0_mag = self.get_current_velocity()
x_init = np.array([p_0_x, p_0_y, psi_0, v_0_mag])
initial_state = util.AttrDict(world=x_init, local=np.array([0, 0, 0, v_0_mag]))
params.initial_state = initial_state
# try:
# u_init = self.__U_warmstarting[self.__step_horizon]
# except TypeError:
# u_init = np.array([0., 0.])
# Using previous control doesn't work
u_init = np.array([0.0, 0.0])
x_bar, u_bar, Gamma, nx, nu = self.__vehicle_model.get_optimization_ltv(
x_init, u_init
)
params.x_bar, params.u_bar, params.Gamma = x_bar, u_bar, Gamma
params.nx, params.nu = nx, nu
"""Get controls for other vehicles."""
# 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
return params
def make_local_params(self, frame, ovehicles):
"""Get Local LCSS parameters that are environment dependent."""
params = util.AttrDict()
"""General local params."""
params.frame = frame
# 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.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)])
"""Local params for (random) multiple coinciding controls."""
# N_traj - number of planned trajectories possible to compute
params.N_traj = np.prod(params.K)
max_K = np.max(params.K)
# TODO: figure out a way to specify number of subtrajectories
params.N_select = min(int(1.5 * max_K), params.N_traj)
# subtraj_indices - the indices of the sub-trajectory to optimize for
params.subtraj_indices = np.random.choice(np.arange(params.N_traj),
size=params.N_select,
replace=False)
return params
def visualize_distances():
config = parse_arguments()
client = carla.Client(config.host, config.port)
client.set_timeout(10.0)
world = client.get_world()
carla_map = world.get_map()
blueprints = world.get_blueprint_library().filter("vehicle.*")
blueprints = [x for x in blueprints if int(x.get_attribute("number_of_wheels")) == 4]
blueprints = [x for x in blueprints if not x.id.endswith('isetta')]
blueprints = [x for x in blueprints if not x.id.endswith('carlacola')]
blueprints = [x for x in blueprints if not x.id.endswith('firetruck')]
blueprints = [x for x in blueprints if not x.id.endswith('cybertruck')]
blueprints = [x for x in blueprints if not x.id.endswith('ambulance')]
blueprints = [x for x in blueprints if not x.id.endswith('sprinter')]
blueprints = [x for x in blueprints if not x.id.endswith('t2')]
# blueprint = world.get_blueprint_library().find("vehicle.audi.a2")
blueprint = random.choice(blueprints)
spawn_point = carla_map.get_spawn_points()[0]
vehicle = None
try:
vehicle = world.spawn_actor(blueprint, spawn_point)
transform = carlautil.spherical_to_camera_watcher_transform(
3, math.pi, math.pi*(1/6),
pin=spawn_point.location)
world.get_spectator().set_transform(transform)
thickness = 0.2
color = carla.Color(r=255, g=0, b=0, a=100)
life_time = 6
loc = carlautil.to_location_ndarray(spawn_point.location)
scale = 2.
z = loc[2] + 0.5
box = np.array([[-1., -1.], [1., -1.], [1., 1.], [-1., 1.]])
box = (box*scale) + loc[:2]
world.debug.draw_line(
carla.Location(box[0, 0], box[0, 1], z),
carla.Location(box[1, 0], box[1, 1], z),
thickness=thickness, color=color, life_time=life_time)
world.debug.draw_line(
carla.Location(box[1, 0], box[1, 1], z),
carla.Location(box[2, 0], box[2, 1], z),
thickness=thickness, color=color, life_time=life_time)
world.debug.draw_line(
carla.Location(box[2, 0], box[2, 1], z),
carla.Location(box[3, 0], box[3, 1], z),
thickness=thickness, color=color, life_time=life_time)
world.debug.draw_line(
carla.Location(box[3, 0], box[3, 1], z),
carla.Location(box[0, 0], box[0, 1], z),
thickness=thickness, color=color, life_time=life_time)
time.sleep(life_time)
world.wait_for_tick()
finally:
if vehicle:
vehicle.destroy()
def to_point(wp, flip_x=False, flip_y=False):
"""Convert waypoint to a 2D point.
Parameters
==========
wp : carla.Waypoint
Returns
=======
list of float
"""
x, y, _ = carlautil.to_location_ndarray(wp, flip_x=flip_x, flip_y=flip_y)
return [x, y]
def extract_junction_with_portals(self):
carla_topology = self.carla_map.get_topology()
junctions = carlautil.get_junctions_from_topology_graph(carla_topology)
_junctions = []
for junction in junctions:
jx, jy, _ = carlautil.to_location_ndarray(junction, flip_y=True)
wps = junction.get_waypoints(carla.LaneType.Driving)
_wps = []
for wp1, wp2 in wps:
# wp1 is the waypoint entering into the intersection
# wp2 is the waypoint exiting out of the intersection
x, y, _ = carlautil.to_location_ndarray(wp1, flip_y=True)
_, yaw, _ = carlautil.to_rotation_ndarray(wp1, flip_y=True)
_wp1 = (x, y, yaw, wp1.lane_width)
x, y, _ = carlautil.to_location_ndarray(wp2, flip_y=True)
_, yaw, _ = carlautil.to_rotation_ndarray(wp2, flip_y=True)
_wp2 = (x, y, yaw, wp2.lane_width)
_wps.append((_wp1, _wp2))
_junctions.append(
util.AttrDict(pos=np.array([jx, jy]),
waypoints=np.array(_wps)))
return _junctions
def __plot_segs_polytopes(self, params, segs_polytopes, goal):
fig, ax = plt.subplots(figsize=(7, 7))
x_min, y_min = np.min(self.__road_segs.positions, axis=0)
x_max, y_max = np.max(self.__road_segs.positions, axis=0)
self.__map_reader.render_map(
ax, extent=(x_min - 20, x_max + 20, y_min - 20, y_max + 20)
)
x, y, _ = carlautil.to_location_ndarray(self.__ego_vehicle, flip_y=True)
ax.scatter(x, y, c="r", zorder=10)
x, y = goal
ax.scatter(x, y, c="g", marker="*", zorder=10)
for A, b in segs_polytopes:
util.npu.plot_h_polyhedron(ax, A, b, fc="b", ec="b", alpha=0.3)
filename = f"agent{self.__ego_vehicle.id}_frame{params.frame}_boundary"
fig.savefig(os.path.join("out", f"{filename}.png"))
fig.clf()
def __compute_prediction_controls(self, frame):
pred_result = self.do_prediction(frame)
ovehicles = self.make_ovehicles(pred_result)
params = self.make_local_params(ovehicles)
ctrl_result = self.do_highlevel_control(params, ovehicles)
"""use control input next round for warm starting."""
self.U_warmstarting = ctrl_result.U_star
"""Get trajectory"""
trajectory = []
x, y, _ = carlautil.to_location_ndarray(self.__ego_vehicle,
flip_y=True)
X = np.concatenate((np.array([x, y])[None], ctrl_result.X_star[:, :2]))
n_steps = X.shape[0]
headings = []
for t in range(1, n_steps):
x, y = X[t]
y = -y # flip about x-axis again to move back to UE coordinates
yaw = np.arctan2(X[t, 1] - X[t - 1, 1], X[t, 0] - X[t - 1, 0])
headings.append(yaw)
# flip about x-axis again to move back to UE coordinates
yaw = util.reflect_radians_about_x_axis(yaw)
transform = carla.Transform(carla.Location(x=x, y=y),
carla.Rotation(yaw=yaw))
trajectory.append(transform)
if self.plot_scenario:
"""Plot scenario"""
filename = f"agent{self.__ego_vehicle.id}_frame{frame}_lcss_control"
ctrl_result.headings = headings
lon, lat, _ = carlautil.actor_to_bbox_ndarray(self.__ego_vehicle)
ego_bbox = [lon, lat]
params.update(self.__params)
plot_lcss_prediction(pred_result,
ovehicles,
params,
ctrl_result,
self.__prediction_horizon,
ego_bbox,
filename=filename)
if self.plot_simulation:
"""Save planned trajectory for final plotting"""
self.__plot_simulation_data.planned_trajectories[
frame] = np.concatenate((params.x0[None], ctrl_result.X_star))
self.__plot_simulation_data.planned_controls[
frame] = ctrl_result.U_star
return trajectory
def get_straight_line(start_wp, start_yaw, tol=2.0):
"""Get the points corresponding to the straight part of the road lane starting from start_wp.
Parameters
==========
start_wp : carla.Waypoint
The starting point of the road lane.
start_yaw : float
The angle of the road in radians.
This is passed as the lanes on the road may not be parallel.
tol : float (optional)
The tolerance to select straight part of the road in meters.
Returns
=======
np.array
Array of points of shape (N, 2).
Each (x, y) point is in meters in world coordinates.
"""
x_start, y_start, _ = carlautil.to_location_ndarray(start_wp)
FACTOR = 10
x_end, y_end = x_start + FACTOR * np.cos(
start_yaw), y_start + FACTOR * np.sin(start_yaw)
def inner(wp):
wps = wp.next_until_lane_end(PRECISION)
points = np.array(util.map_to_ndarray(to_point, wps))
distances = util.distances_from_line_2d(points, x_start, y_start,
x_end, y_end)
wps_mask = np.nonzero(distances > tol)[0]
if wps_mask.size > 0:
# waypoints veer off a straight line
idx = wps_mask[0]
return points[:idx], list()
else:
# waypoints stay one straight line. Get the next set of waypoints
return points, wps[-1].next(PRECISION)
dq = collections.deque([start_wp])
point_collection = []
while len(dq) > 0:
wp = dq.popleft()
points, next_wps = inner(wp)
dq.extend(next_wps)
point_collection.append(points)
return np.concatenate(point_collection)
def compute_boundary_constraints(self, p_x, p_y):
"""
Parameters
==========
p_x : np.array of docplex.mp.vartype.VarType
p_y : np.array of docplex.mp.vartype.VarType
"""
b_length, f_length, r_width, l_width = self.__road_seg_params
mtx = util.rotation_2d(self.__road_seg_starting[2])
shift = self.__road_seg_starting[:2]
plot_bbox_constraints = False
if plot_bbox_constraints:
fig, axes = plt.subplots(1, 2, figsize=(10, 6))
axes = axes.ravel()
ego_x, ego_y, _ = carlautil.to_location_ndarray(self.__ego_vehicle)
ego_y = -ego_y
wp_x, wp_y = self.__road_seg_starting[:2]
axes[0].plot(ego_x, ego_y, 'g*')
axes[0].plot(wp_x, wp_y, 'b*')
patch = patches.Polygon(self.__road_seg_enclosure,
ec='b',
fc='none')
axes[0].add_patch(patch)
axes[0].set_aspect('equal')
ego_x, ego_y = mtx @ (np.array([ego_x, ego_y]) - shift)
axes[1].plot(ego_x, ego_y, 'g*')
axes[1].plot([0, -b_length], [0, 0], '-bo')
axes[1].plot([0, f_length], [0, 0], '-bo')
axes[1].plot([0, 0], [0, -r_width], '-bo')
axes[1].plot([0, 0], [0, l_width], '-bo')
axes[1].set_aspect('equal')
fig.tight_layout()
plt.show()
pos = np.stack([p_x, p_y], axis=1)
pos = obj_matmul((pos - shift), mtx.T)
constraints = []
constraints.extend([-b_length <= z for z in pos[:, 0]])
constraints.extend([z <= f_length for z in pos[:, 0]])
constraints.extend([-r_width <= z for z in pos[:, 1]])
constraints.extend([z <= l_width for z in pos[:, 1]])
return constraints
def make_local_params(self, frame, ovehicles):
"""Get the local optimization parameters used for current MPC step."""
"""Get parameters to construct control and state variables."""
params = util.AttrDict()
params.frame = frame
p_0_x, p_0_y, _ = carlautil.to_location_ndarray(self.__ego_vehicle,
flip_y=True)
_, psi_0, _ = carlautil.actor_to_rotation_ndarray(self.__ego_vehicle,
flip_y=True)
v_0_mag = self.get_current_velocity()
x_init = np.array([p_0_x, p_0_y, psi_0, v_0_mag])
initial_state = util.AttrDict(world=x_init,
local=np.array([0, 0, 0, v_0_mag]))
params.initial_state = initial_state
"""Get controls for other vehicles."""
# O - number of total vehicles EV and OVs
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
return params
def __plot_boundary(self):
fig, axes = plt.subplots(1, 2, figsize=(10, 6))
axes = axes.ravel()
ego_x, ego_y, _ = carlautil.to_location_ndarray(self.__ego_vehicle)
ego_y = -ego_y
wp_x, wp_y = self.__road_seg_starting[:2]
axes[0].plot(ego_x, ego_y, 'g*')
axes[0].plot(wp_x, wp_y, 'b*')
patch = patches.Polygon(self.__road_seg_enclosure, ec='b', fc='none')
axes[0].add_patch(patch)
axes[0].set_aspect('equal')
b_length, f_length, r_width, l_width = self.__road_seg_params
mtx = util.rotation_2d(self.__road_seg_starting[2])
shift = self.__road_seg_starting[:2]
ego_x, ego_y = mtx @ (np.array([ego_x, ego_y]) - shift)
axes[1].plot(ego_x, ego_y, 'g*')
axes[1].plot([0, -b_length], [0, 0], '-bo')
axes[1].plot([0, f_length], [0, 0], '-bo')
axes[1].plot([0, 0], [0, -r_width], '-bo')
axes[1].plot([0, 0], [0, l_width], '-bo')
axes[1].set_aspect('equal')
fig.tight_layout()
plt.show()
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
# N_traj - number of planned trajectories to compute
params.N_traj = np.prod(params.K)
p_0_x, p_0_y, _ = carlautil.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])
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, frame):
"""Get the local optimization parameters used for current MPC step."""
params = util.AttrDict()
params.frame = frame
p_0_x, p_0_y, _ = carlautil.to_location_ndarray(self.__ego_vehicle,
flip_y=True)
_, psi_0, _ = carlautil.actor_to_rotation_ndarray(self.__ego_vehicle,
flip_y=True)
v_0_mag = self.get_current_velocity()
x_init = np.array([p_0_x, p_0_y, psi_0, v_0_mag])
initial_state = util.AttrDict(world=x_init,
local=np.array([0, 0, 0, v_0_mag]))
params.initial_state = initial_state
# try:
# u_init = self.__U_warmstarting[self.__step_horizon]
# except TypeError:
# u_init = np.array([0., 0.])
# Using previous control doesn't work
u_init = np.array([0.0, 0.0])
x_bar, u_bar, Gamma, nx, nu = self.__vehicle_model.get_optimization_ltv(
x_init, u_init)
params.x_bar, params.u_bar, params.Gamma = x_bar, u_bar, Gamma
params.nx, params.nu = nx, nu
return params
def make_local_params(self, frame):
"""Get the linearized bicycle model using the vehicle's
immediate orientation."""
params = util.AttrDict()
params.frame = frame
"""Dynamics parameters"""
p_0_x, p_0_y, _ = carlautil.to_location_ndarray(self.__ego_vehicle,
flip_y=True)
_, psi_0, _ = carlautil.actor_to_rotation_ndarray(self.__ego_vehicle,
flip_y=True)
v_0_mag = self.get_current_velocity()
delta_0 = self.get_current_steering()
# initial_state - state at current frame in world/local coordinates
# Local coordinates has initial position and heading at 0
initial_state = util.AttrDict(
world=np.array([p_0_x, p_0_y, psi_0, v_0_mag, delta_0]),
local=np.array([0, 0, 0, v_0_mag, delta_0]))
params.initial_state = initial_state
# transform - transform points from local coordinates back to world coordinates.
M = np.array([
[math.cos(psi_0), -math.sin(psi_0), p_0_x],
[math.sin(psi_0), math.cos(psi_0), p_0_y],
])
def transform(X):
points = np.pad(X[:, :2], [(0, 0), (0, 1)],
mode="constant",
constant_values=1)
points = util.obj_matmul(points, M.T)
psis = X[:, 2] + psi_0
return np.concatenate((points, psis[..., None], X[:, 3:]), axis=1)
params.transform = transform
# longitudinal and lateral dimensions of car are normally 3.70 m, 1.79 m resp.
bbox_lon = self.__params.bbox_lon
# TODO: use center of gravity from API instead
A = get_state_matrix(0,
0,
0,
v_0_mag,
delta_0,
l_r=0.5 * bbox_lon,
L=bbox_lon)
B = get_input_matrix()
C = get_output_matrix()
D = get_feedforward_matrix()
sys = control.matlab.c2d(control.matlab.ss(A, B, C, D),
self.__timestep)
A = np.array(sys.A)
B = np.array(sys.B)
# nx, nu - size of state variable and control input respectively.
nx, nu = sys.B.shape
params.nx, params.nu = nx, nu
T = self.__control_horizon
# Closed form solution to states given inputs
# 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.Gamma = Gamma
# make state computation account for initial position and velocity
initial_local_rollout = np.concatenate([
np.linalg.matrix_power(A, t) @ initial_state.local
for t in range(T + 1)
])
params.initial_rollout = util.AttrDict(local=initial_local_rollout)
return params
def make_local_params(self, frame, ovehicles):
"""Get the local optimization parameters used for current MPC step."""
"""Get parameters to construct control and state variables."""
params = util.AttrDict()
params.frame = frame
p_0_x, p_0_y, _ = carlautil.to_location_ndarray(self.__ego_vehicle,
flip_y=True)
_, psi_0, _ = carlautil.actor_to_rotation_ndarray(self.__ego_vehicle,
flip_y=True)
v_0_mag = self.get_current_velocity()
x_init = np.array([p_0_x, p_0_y, psi_0, v_0_mag])
initial_state = util.AttrDict(world=x_init,
local=np.array([0, 0, 0, v_0_mag]))
params.initial_state = initial_state
# try:
# u_init = self.__U_warmstarting[self.__step_horizon]
# except TypeError:
# u_init = np.array([0., 0.])
# Using previous control doesn't work
u_init = np.array([0.0, 0.0])
x_bar, u_bar, Gamma, nx, nu = self.__vehicle_model.get_optimization_ltv(
x_init, u_init)
params.x_bar, params.u_bar, params.Gamma = x_bar, u_bar, Gamma
params.nx, params.nu = nx, nu
"""Get parameters for other vehicles."""
# 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
"""Get parameters for (random) multiple coinciding control."""
# N_traj : int
# Number of planned trajectories possible to compute
params.N_traj = np.prod(params.K)
if self.__random_mcc:
"""How does random multiple coinciding control work?
each item in the product set S_1 X S_2 X S_3 X S_4
represents the particular choices each vehicles make
get the subset of the product set S_1 X S_2 X S_3 X S_4
such that for each i = 1..4, for each j in S_i there is
a tuple in the subset with j in the i-th place.
"""
vehicle_n_states = [ovehicle.n_states for ovehicle in ovehicles]
n_states_max = max(vehicle_n_states)
vehicle_state_ids = [
util.range_to_list(n_states) for n_states in vehicle_n_states
]
def preprocess_state_ids(state_ids):
state_ids = state_ids + random.choices(
state_ids, k=n_states_max - len(state_ids))
random.shuffle(state_ids)
return state_ids
vehicle_state_ids = util.map_to_list(preprocess_state_ids,
vehicle_state_ids)
# sublist_joint_decisions : list of (list of int)
# Subset of S_1 X S_2 X S_3 X S_4 of joint decisions
params.sublist_joint_decisions = np.array(
vehicle_state_ids).T.tolist()
# N_select : int
# Number of planned trajectories to compute
params.N_select = len(params.sublist_joint_decisions)
else:
# sublist_joint_decisions : list of (list of int)
# Entire set of S_1 X S_2 X S_3 X S_4 of joint decision outcomes
params.sublist_joint_decisions = util.product_list_of_list([
util.range_to_list(ovehicle.n_states) for ovehicle in ovehicles
])
# N_select : int
# Number of planned trajectories to compute, equal to N_traj.
params.N_select = len(params.sublist_joint_decisions)
return params
def get_road_segment_enclosure(start_wp, tol=2.0):
"""Get rectangle that tightly inner approximates of the road segment
containing the starting waypoint.
Parameters
==========
start_wp : carla.Waypoint
A starting waypoint of the road.
tol : float (optional)
The tolerance to select straight part of the road in meters.
Returns
=======
np.array
The position and the heading angle of the starting waypoint
of the road of form [x, y, angle] in (meters, meters, radians).
np.array
The 2D bounding box enclosure in world coordinates of shape (4, 2)
enclosing the road segment.
np.array
The parameters of the enclosure of form
(b_length, f_length, r_width, l_width)
If the enclosure is in the reference frame such that the starting
waypoint points along +x-axis, then the enclosure has these length
and widths:
____________________________________
| l_width |
| | |
| | |
| b_length -- (x, y)-> -- f_length |
| | |
| | |
| r_width |
------------------------------------
"""
_LENGTH = -2
_, start_yaw, _ = carlautil.to_rotation_ndarray(start_wp)
adj_wps = get_adjacent_waypoints(start_wp)
# mtx : np.array
# Rotation matrix from world coordinates, both frames in UE orientation
mtx = util.rotation_2d(-start_yaw)
# rev_mtx : np.array
# Rotation matrix to world coordinates, both frames in UE orientation
rev_mtx = util.rotation_2d(start_yaw)
s_x, s_y, _ = carlautil.to_location_ndarray(start_wp)
# Get points of lanes
f = lambda wp: get_straight_line(wp, start_yaw, tol=tol)
pc = util.map_to_list(f, adj_wps)
# Get length of bb for lanes
def g(points):
points = points - np.array([s_x, s_y])
points = (rev_mtx @ points.T)[0]
return np.abs(np.max(points) - np.min(points))
lane_lengths = util.map_to_ndarray(g, pc)
length = np.min(lane_lengths)
# Get width of bb for lanes
lwp, rwp = adj_wps[0], adj_wps[-1]
l_x, l_y, _ = carlautil.to_location_ndarray(lwp)
r_x, r_y, _ = carlautil.to_location_ndarray(rwp)
points = np.array([[l_x, l_y], [s_x, s_y], [r_x, r_y]])
points = points @ rev_mtx.T
l_width = np.abs(points[0, 1] - points[1, 1]) + lwp.lane_width / 2.
r_width = np.abs(points[1, 1] - points[2, 1]) + rwp.lane_width / 2.
# construct bounding box of road segment
x, y, _ = carlautil.to_location_ndarray(start_wp)
vec = np.array([[0, 0], [_LENGTH, 0]]) @ mtx.T
dx0, dy0 = vec[1, 0], vec[1, 1]
vec = np.array([[0, 0], [length, 0]]) @ mtx.T
dx1, dy1 = vec[1, 0], vec[1, 1]
vec = np.array([[0, 0], [0, -l_width]]) @ mtx.T
dx2, dy2 = vec[1, 0], vec[1, 1]
vec = np.array([[0, 0], [0, r_width]]) @ mtx.T
dx3, dy3 = vec[1, 0], vec[1, 1]
bbox = np.array([[x + dx0 + dx3, y + dy0 + dy3],
[x + dx1 + dx3, y + dy1 + dy3],
[x + dx1 + dx2, y + dy1 + dy2],
[x + dx0 + dx2, y + dy0 + dy2]])
start_wp_spec = np.array([s_x, s_y, start_yaw])
bbox_spec = np.array([_LENGTH, length, r_width, l_width])
return start_wp_spec, bbox, bbox_spec
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 __capture_agents_within_radius(self, frame_id):
labels = self.__map_reader.get_actor_labels(self.__ego_vehicle)
if self.__should_exclude_dataset_sample(labels):
self.__remove_scene_builder()
player_location = carlautil.to_location_ndarray(self.__ego_vehicle)
player_z = player_location[2]
if len(self.__other_vehicles):
other_ids = list(self.__other_vehicles.keys())
other_vehicles = self.__other_vehicles.values()
others_data = carlautil.actors_to_Lxyz_Vxyz_Axyz_Rpyr_ndarray(
other_vehicles).T
other_locations = others_data[:3].T
distances = np.linalg.norm(other_locations - player_location,
axis=1)
df = pd.DataFrame({
"frame_id":
np.full((len(other_ids), ), frame_id),
"type":
[self.__scene_config.node_type.VEHICLE] * len(other_ids),
"node_id":
util.map_to_list(str, other_ids),
"robot": [False] * len(other_ids),
"distances":
distances,
"x":
others_data[0],
"y":
others_data[1],
"z":
others_data[2],
"v_x":
others_data[3],
"v_y":
others_data[4],
"v_z":
others_data[5],
"a_x":
others_data[6],
"a_y":
others_data[7],
"a_z":
others_data[8],
"length":
others_data[9],
"width":
others_data[10],
"height":
others_data[11],
"heading":
others_data[13],
})
df = df[df["distances"] < self.__radius]
df = df[df["z"].between(
player_z - self.Z_LOWERBOUND,
player_z + self.Z_UPPERBOUND,
inclusive="neither",
)]
del df["distances"]
else:
df = pd.DataFrame(columns=[
"frame_id",
"type",
"node_id",
"robot",
"x",
"y",
"z",
"length",
"width",
"height",
"heading",
])
ego_data = carlautil.actor_to_Lxyz_Vxyz_Axyz_Rpyr_ndarray(
self.__ego_vehicle)
data_point = pd.DataFrame.from_records([{
"frame_id":
frame_id,
"type":
self.__scene_config.node_type.VEHICLE,
"node_id":
"ego",
"robot":
True,
"x":
ego_data[0],
"y":
ego_data[1],
"z":
ego_data[2],
"v_x":
ego_data[3],
"v_y":
ego_data[4],
"v_z":
ego_data[5],
"a_x":
ego_data[6],
"a_y":
ego_data[7],
"a_z":
ego_data[8],
"length":
ego_data[9],
"width":
ego_data[10],
"height":
ego_data[11],
"heading":
ego_data[13],
}])
df = pd.concat((df, data_point), ignore_index=True)
# df = df.append(data_point, ignore_index=True)
if self.__trajectory_data is None:
self.__trajectory_data = df
else:
self.__trajectory_data = pd.concat((self.__trajectory_data, df),
ignore_index=True)
