def _build_multilane(scene):
"""
Create a road composed of straight adjacent lanes.
"""
scene.road = Road(network=RoadNetwork.straight_road_network(
scene.config["lanes_count"]),
np_random=scene.np_random)
def _create_road(self):
"""
Create a road composed of straight adjacent lanes.
"""
self.road = Road(network=RoadNetwork.straight_road_network(
self.config["lanes_count"]),
np_random=self.np_random)
def _build_parking(self):
"""
Create a road composed of straight adjacent lanes.
We will have 4 parking configurations based on (parking angle):
https://www.webpages.uidaho.edu/niatt_labmanual/chapters/parkinglotdesign/theoryandconcepts/parkingstalllayoutconsiderations.htm
parking angle = 90, 75, 60, 45
"""
net = RoadNetwork()
lt = (LineType.CONTINUOUS, LineType.CONTINUOUS)
spots_offset = 0.0
parking_angles = np.deg2rad([90, 75, 60, 45, 0])
aisle_width = 10.0
length = 8.0
width = 4.0
# Let's start by randomly choosing the parking angle
#angle = parking_angles[self.np_random.randint(len(parking_angles))]
angle = 0 #np.pi/3
# Let's now build the parking lot
for k in range(self.parking_spots):
x1 = (k - self.parking_spots // 2) * (width +
spots_offset) - width / 2
y1 = aisle_width / 2
x2 = x1
y2 = y1 + length
x3 = x1
y3 = -y1
x4 = x3
y4 = -y2
x1, y1 = self.rot((x1, y1), angle)
x2, y2 = self.rot((x2, y2), angle)
x3, y3 = self.rot((x3, y3), angle)
x4, y4 = self.rot((x4, y4), angle)
net.add_lane(
"a", "b",
StraightLane([x1, y1], [x2, y2], width=width, line_types=lt))
net.add_lane(
"b", "c",
StraightLane([x3, y3], [x4, y4], width=width, line_types=lt))
self.road = Road(network=net, np_random=self.np_random)
def test_front():
r = Road(RoadNetwork.straight_road_network(1))
v1 = Vehicle(road=r, position=[0, 0], velocity=20)
v2 = Vehicle(road=r, position=[10, 0], velocity=10)
r.vehicles.extend([v1, v2])
assert v1.lane_distance_to(v2) == pytest.approx(10)
assert v2.lane_distance_to(v1) == pytest.approx(-10)
def test_velocity_control():
road = Road(RoadNetwork.straight_road_network(1))
v = ControlledVehicle(road=road, position=road.network.get_lane((0, 1, 0)).position(0, 0), velocity=20, heading=0)
v.act('FASTER')
for _ in range(int(3 * v.TAU_A * FPS)):
v.act()
v.step(dt=1/FPS)
assert v.velocity == pytest.approx(20+v.DELTA_VELOCITY, abs=0.5)
assert v.position[1] == pytest.approx(0)
assert v.lane_index[2] == 0
def test_lane_change():
road = Road(RoadNetwork.straight_road_network(2))
v = ControlledVehicle(road=road, position=road.network.get_lane((0, 1, 0)).position(0, 0), velocity=20, heading=0)
v.act('LANE_RIGHT')
for _ in range(3 * FPS):
v.act()
v.step(dt=1/FPS)
assert v.velocity == pytest.approx(20)
assert v.position[1] == pytest.approx(StraightLane.DEFAULT_WIDTH, abs=StraightLane.DEFAULT_WIDTH/4)
assert v.lane_index[2] == 1
def test_network():
# Diamond
net = RoadNetwork()
net.add_lane(0, 1, StraightLane([0, 0], [10, 0]))
net.add_lane(1, 2, StraightLane([10, 0], [5, 5]))
net.add_lane(2, 0, StraightLane([5, 5], [0, 0]))
net.add_lane(1, 3, StraightLane([10, 0], [5, -5]))
net.add_lane(3, 0, StraightLane([5, -5], [0, 0]))
print(net.graph)
# Road
road = Road(network=net)
v = ControlledVehicle(road, [5, 0], heading=0, target_velocity=2)
road.vehicles.append(v)
assert v.lane_index == (0, 1, 0)
# Lane changes
dt = 1/15
lane_index = v.target_lane_index
lane_changes = 0
for _ in range(int(20/dt)):
road.act()
road.step(dt)
if lane_index != v.target_lane_index:
lane_index = v.target_lane_index
lane_changes += 1
assert lane_changes >= 3
def _build_merge(scene):
"""
Make a road composed of a straight urban_AD and a merging lane.
:return: the road
"""
net = RoadNetwork()
# urban_AD lanes
ends = [150, 80, 80, 150] # Before, converging, merge, after
c, s, n = LineType.CONTINUOUS_LINE, LineType.STRIPED, LineType.NONE
y = [0, StraightLane.DEFAULT_WIDTH]
line_type = [[c, s], [n, c]]
line_type_merge = [[c, s], [n, s]]
for i in range(2):
net.add_lane(
"a", "b",
StraightLane([0, y[i]], [sum(ends[:2]), y[i]],
line_types=line_type[i]))
net.add_lane(
"b", "c",
StraightLane([sum(ends[:2]), y[i]], [sum(ends[:3]), y[i]],
line_types=line_type_merge[i]))
net.add_lane(
"c", "d",
StraightLane([sum(ends[:3]), y[i]], [sum(ends), y[i]],
line_types=line_type[i]))
# Merging lane
amplitude = 3.25
ljk = StraightLane([0, 6.5 + 4 + 4], [ends[0], 6.5 + 4 + 4],
line_types=[c, c],
forbidden=True)
lkb = SineLane(ljk.position(ends[0], -amplitude),
ljk.position(sum(ends[:2]), -amplitude),
amplitude,
2 * np.pi / (2 * ends[1]),
np.pi / 2,
line_types=[c, c],
forbidden=True)
lbc = StraightLane(lkb.position(ends[1], 0),
lkb.position(ends[1], 0) + [ends[2], 0],
line_types=[n, c],
forbidden=True)
net.add_lane("j", "k", ljk)
net.add_lane("k", "b", lkb)
net.add_lane("b", "c", lbc)
road = Road(network=net, np_random=scene.np_random)
road.vehicles.append(Obstacle(road, lbc.position(ends[2], 0)))
scene.road = road
def _build_roundabout(scene):
# Circle lanes: (s)outh/(e)ast/(n)orth/(w)est (e)ntry/e(x)it.
center = [0, 0] # [m]
radius = 30 # [m]
alpha = 20 # [deg]
net = RoadNetwork()
radii = [radius, radius + 4]
n, c, s = LineType.NONE, LineType.CONTINUOUS, LineType.STRIPED
line = [[c, s], [n, c]]
for lane in [0, 1]:
net.add_lane(
"se", "ex",
CircularLane(center,
radii[lane],
rad(90 - alpha),
rad(alpha),
line_types=line[lane]))
net.add_lane(
"ex", "ee",
CircularLane(center,
radii[lane],
rad(alpha),
rad(-alpha),
line_types=line[lane]))
net.add_lane(
"ee", "nx",
CircularLane(center,
radii[lane],
rad(-alpha),
rad(-90 + alpha),
line_types=line[lane]))
net.add_lane(
"nx", "ne",
CircularLane(center,
radii[lane],
rad(-90 + alpha),
rad(-90 - alpha),
line_types=line[lane]))
net.add_lane(
"ne", "wx",
CircularLane(center,
radii[lane],
rad(-90 - alpha),
rad(-180 + alpha),
line_types=line[lane]))
net.add_lane(
"wx", "we",
CircularLane(center,
radii[lane],
rad(-180 + alpha),
rad(-180 - alpha),
line_types=line[lane]))
net.add_lane(
"we", "sx",
CircularLane(center,
radii[lane],
rad(180 - alpha),
rad(90 + alpha),
line_types=line[lane]))
net.add_lane(
"sx", "se",
CircularLane(center,
radii[lane],
rad(90 + alpha),
rad(90 - alpha),
line_types=line[lane]))
# Access lanes: (r)oad/(s)ine
access = 200 # [m]
dev = 120 # [m]
a = 5 # [m]
delta_st = 0.20 * dev # [m]
delta_en = dev - delta_st
w = 2 * np.pi / dev
net.add_lane("ser", "ses",
StraightLane([2, access], [2, dev / 2], line_types=[s, c]))
net.add_lane(
"ses", "se",
SineLane([2 + a, dev / 2], [2 + a, dev / 2 - delta_st],
a,
w,
-np.pi / 2,
line_types=[c, c]))
net.add_lane(
"sx", "sxs",
SineLane([-2 - a, -dev / 2 + delta_en], [-2 - a, dev / 2],
a,
w,
-np.pi / 2 + w * delta_en,
line_types=[c, c]))
net.add_lane("sxs", "sxr",
StraightLane([-2, dev / 2], [-2, access], line_types=[n, c]))
net.add_lane("eer", "ees",
StraightLane([access, -2], [dev / 2, -2], line_types=[s, c]))
net.add_lane(
"ees", "ee",
SineLane([dev / 2, -2 - a], [dev / 2 - delta_st, -2 - a],
a,
w,
-np.pi / 2,
line_types=[c, c]))
net.add_lane(
"ex", "exs",
SineLane([-dev / 2 + delta_en, 2 + a], [dev / 2, 2 + a],
a,
w,
-np.pi / 2 + w * delta_en,
line_types=[c, c]))
net.add_lane("exs", "exr",
StraightLane([dev / 2, 2], [access, 2], line_types=[n, c]))
net.add_lane(
"ner", "nes",
StraightLane([-2, -access], [-2, -dev / 2], line_types=[s, c]))
net.add_lane(
"nes", "ne",
SineLane([-2 - a, -dev / 2], [-2 - a, -dev / 2 + delta_st],
a,
w,
-np.pi / 2,
line_types=[c, c]))
net.add_lane(
"nx", "nxs",
SineLane([2 + a, dev / 2 - delta_en], [2 + a, -dev / 2],
a,
w,
-np.pi / 2 + w * delta_en,
line_types=[c, c]))
net.add_lane("nxs", "nxr",
StraightLane([2, -dev / 2], [2, -access], line_types=[n, c]))
road = Road(network=net, np_random=scene.np_random)
scene.road = road
class ParkingEnv(AbstractEnv, GoalEnv):
"""
A continuous control environment.
It implements a reach-type task, where the agent observes their position and velocity and must
control their acceleration and steering so as to reach a given goal.
Credits to Munir Jojo-Verge for the idea and initial implementation.
"""
STEERING_RANGE = np.pi / 4
ACCELERATION_RANGE = 5.0
COLLISION_REWARD = -1.0
MOVING_REWARD = +0.2
OVER_OTHER_PARKING_SPOT_REWARD = -0.2
REACHING_GOAL_REWARD = +1.0
PARKING_MAX_VELOCITY = 7.0 # m/s
OBS_SCALE = 100
REWARD_SCALE = np.absolute(COLLISION_REWARD)
REWARD_WEIGHTS = [5 / 100, 5 / 100, 1 / 100, 1 / 100, 5 / 10, 5 / 10]
SUCCESS_THRESHOLD = 0.27
DEFAULT_CONFIG = {
"other_vehicles_type": "urban_AD_env.vehicle.behavior.IDMVehicle",
"centering_position": [0.5, 0.5],
"parking_spots": 15, #'random', # Parking Spots Per side
"vehicles_count": 0, #'random', # Total number of cars
"screen_width": 600 * 2,
"screen_height": 600 * 2
}
OBSERVATION_FEATURES = ['x', 'y', 'vx', 'vy', 'cos_h', 'sin_h']
OBSERVATION_NEAR_EGO = 0 # How many vehicles "near" EGO you want to include in the "observation"/"state space"
OBSERVATION_NEAR_GOAL = 0 # How many vehicles "near" GOAL you want to include in the "observation"/"state space" (Testing a better awarnes around the goal)
NORMALIZE_OBS = False
def __init__(self):
super(ParkingEnv, self).__init__()
self.config = self.DEFAULT_CONFIG.copy()
if self.config["parking_spots"] == 'random':
self.parking_spots = self.np_random.randint(1, 21)
else:
self.parking_spots = self.config["parking_spots"]
if self.config["vehicles_count"] == 'random':
self.vehicles_count = self.np_random.randint(
self.parking_spots) * 2
else:
self.vehicles_count = self.config["vehicles_count"]
assert (self.vehicles_count < self.parking_spots * 2)
obs = self.reset()
self.observation_space = spaces.Dict(
dict(
desired_goal=spaces.Box(-np.inf,
np.inf,
shape=obs["desired_goal"].shape,
dtype=np.float32),
achieved_goal=spaces.Box(-np.inf,
np.inf,
shape=obs["achieved_goal"].shape,
dtype=np.float32),
observation=spaces.Box(-np.inf,
np.inf,
shape=obs["observation"].shape,
dtype=np.float32),
))
self.action_space = spaces.Box(-1., 1., shape=(2, ), dtype=np.float32)
self.REWARD_WEIGHTS = np.array(self.REWARD_WEIGHTS)
EnvViewer.SCREEN_HEIGHT = EnvViewer.SCREEN_WIDTH // 2
def step(self, action):
# Forward action to the vehicle
# self.vehicle.act({"steering": action[0] * self.STEERING_RANGE,
# "acceleration": action[1] * self.ACCELERATION_RANGE})
self.vehicle.act({
"acceleration": action[0] * self.ACCELERATION_RANGE,
"steering": action[1] * self.STEERING_RANGE
})
self._simulate()
obs = self._observation()
info = {
"is_success":
self._is_success(obs['achieved_goal'], obs['desired_goal']),
"is_collision":
int(self.vehicle.crashed),
"is_over_others_parking_spot":
int(self.is_over_others_parking_spot(self.vehicle.position)),
"velocity_idx":
self.vehicle.velocity / self.PARKING_MAX_VELOCITY
}
reward = self.compute_reward(obs['achieved_goal'], obs['desired_goal'],
info)
terminal = self._is_terminal()
return obs, reward, terminal, info
def is_over_others_parking_spot(self, position):
for _from, to_dict in self.road.network.graph.items():
for _to, lanes in to_dict.items():
for _id, lane in enumerate(lanes):
if lane != self.goal.lane:
over_others_parking_spots = lane.on_lane(position)
if (over_others_parking_spots):
return True
return False
def reset(self):
self._build_parking()
self._populate_parking()
return self._observation()
def configure(self, config):
self.config.update(config)
def rot(self, point, angle):
assert len(point) == 2
x = point[0]
y = point[1]
cos_ang = np.cos(angle)
sin_ang = np.sin(angle)
x_rot = x * cos_ang - y * sin_ang
y_rot = x * sin_ang + y * cos_ang
return x_rot, y_rot
def _build_parking(self):
"""
Create a road composed of straight adjacent lanes.
We will have 4 parking configurations based on (parking angle):
https://www.webpages.uidaho.edu/niatt_labmanual/chapters/parkinglotdesign/theoryandconcepts/parkingstalllayoutconsiderations.htm
parking angle = 90, 75, 60, 45
"""
net = RoadNetwork()
lt = (LineType.CONTINUOUS, LineType.CONTINUOUS)
spots_offset = 0.0
parking_angles = np.deg2rad([90, 75, 60, 45, 0])
aisle_width = 10.0
length = 8.0
width = 4.0
# Let's start by randomly choosing the parking angle
#angle = parking_angles[self.np_random.randint(len(parking_angles))]
angle = 0 #np.pi/3
# Let's now build the parking lot
for k in range(self.parking_spots):
x1 = (k - self.parking_spots // 2) * (width +
spots_offset) - width / 2
y1 = aisle_width / 2
x2 = x1
y2 = y1 + length
x3 = x1
y3 = -y1
x4 = x3
y4 = -y2
x1, y1 = self.rot((x1, y1), angle)
x2, y2 = self.rot((x2, y2), angle)
x3, y3 = self.rot((x3, y3), angle)
x4, y4 = self.rot((x4, y4), angle)
net.add_lane(
"a", "b",
StraightLane([x1, y1], [x2, y2], width=width, line_types=lt))
net.add_lane(
"b", "c",
StraightLane([x3, y3], [x4, y4], width=width, line_types=lt))
self.road = Road(network=net, np_random=self.np_random)
def _populate_parking(self):
"""
Create some new random vehicles of a given type, and add them on the road.
"""
##### ADDING EGO #####
self.vehicle = MDPVehicle(self.road, [0, 0],
2 * np.pi * self.np_random.rand(),
velocity=0)
self.vehicle.MAX_VELOCITY = self.PARKING_MAX_VELOCITY
self.road.vehicles.append(self.vehicle)
##### ADDING GOAL #####
parking_spots_used = []
lane = self.np_random.choice(self.road.network.lanes_list())
parking_spots_used.append(lane)
goal_heading = lane.heading #+ self.np_random.randint(2) * np.pi
self.goal = Obstacle(self.road,
lane.position(lane.length / 2, 0),
heading=goal_heading)
self.goal.COLLISIONS_ENABLED = False
self.road.vehicles.insert(0, self.goal)
##### ADDING OTHER VEHICLES #####
# vehicles_type = utils.class_from_path(scene.config["other_vehicles_type"])
for _ in range(self.vehicles_count):
while lane in parking_spots_used: # this loop should never be infinite since we assert that there should be more parking spots/lanes than vehicles
lane = self.np_random.choice(self.road.network.lanes_list()
) # to-do: chceck for empty spots
parking_spots_used.append(lane)
vehicle_heading = lane.heading #+ self.np_random.randint(2) * np.pi
self.road.vehicles.append(
Vehicle(self.road,
lane.position(lane.length / 2, 0),
heading=vehicle_heading,
velocity=0))
def _observation(self):
##### ADDING EGO #####
obs = pandas.DataFrame.from_records([self.vehicle.to_dict()
])[self.OBSERVATION_FEATURES]
ego_obs = np.ravel(obs.copy())
##### ADDING NEARBY (TO EGO) TRAFFIC #####
close_vehicles = self.road.closest_vehicles_to(
self.vehicle, self.OBSERVATION_NEAR_EGO)
if close_vehicles:
obs = obs.append(pandas.DataFrame.from_records([
v.to_dict(self.vehicle) for v in close_vehicles
])[self.OBSERVATION_FEATURES],
ignore_index=True)
# Fill missing rows
needed = self.OBSERVATION_NEAR_EGO + 1
missing = needed - obs.shape[0]
if obs.shape[0] < (needed):
rows = -np.ones((missing, len(self.OBSERVATION_FEATURES)))
obs = obs.append(pandas.DataFrame(
data=rows, columns=self.OBSERVATION_FEATURES),
ignore_index=True)
##### ADDING NEARBY (TO GOAL) TRAFFIC #####
close_vehicles = self.road.closest_vehicles_to(
self.goal, self.OBSERVATION_NEAR_GOAL)
if close_vehicles:
obs = obs.append(pandas.DataFrame.from_records([
v.to_dict(self.vehicle) for v in close_vehicles
])[self.OBSERVATION_FEATURES],
ignore_index=True)
# Fill missing rows
needed = self.OBSERVATION_NEAR_EGO + self.OBSERVATION_NEAR_GOAL + 1
missing = needed - obs.shape[0]
if obs.shape[0] < (needed):
rows = -np.ones((missing, len(self.OBSERVATION_FEATURES)))
obs = obs.append(pandas.DataFrame(
data=rows, columns=self.OBSERVATION_FEATURES),
ignore_index=True)
# Reorder
obs = obs[self.OBSERVATION_FEATURES]
# Flatten
obs = np.ravel(obs)
# Goal
goal = np.ravel(
pandas.DataFrame.from_records([self.goal.to_dict()
])[self.OBSERVATION_FEATURES])
# Arrange it as required by Openai GoalEnv
obs = {
"observation": obs / self.OBS_SCALE,
"achieved_goal": ego_obs / self.OBS_SCALE,
"desired_goal": goal / self.OBS_SCALE
}
return obs
# obs = np.ravel(pandas.DataFrame.from_records([self.vehicle.to_dict()])[self.OBSERVATION_FEATURES])
# goal = np.ravel(pandas.DataFrame.from_records([self.goal.to_dict()])[self.OBSERVATION_FEATURES])
# obs = {
# "observation": obs / self.OBS_SCALE,
# "achieved_goal": obs / self.OBS_SCALE,
# "desired_goal": goal / self.OBS_SCALE
# }
# return obs
def distance_2_goal_reward(self, achieved_goal, desired_goal, p=0.5):
return -np.power(
np.dot(self.OBS_SCALE * np.abs(achieved_goal - desired_goal),
self.REWARD_WEIGHTS), p)
def compute_reward(self, achieved_goal, desired_goal, info, p=0.5):
"""
Proximity to the goal is rewarded
We use a weighted p-norm
:param achieved_goal: the goal that was achieved
:param desired_goal: the goal that was desired
:param info: any supplementary information
:param p: the Lp^p norm used in the reward. Use p<1 to have high kurtosis for rewards in [0, 1]
:return: the corresponding reward
"""
# return - np.power(np.dot(self.OBS_SCALE * np.abs(achieved_goal - desired_goal), self.REWARD_WEIGHTS), p)
# DISTANCE TO GOAL
distance_to_goal_reward = self.distance_2_goal_reward(
achieved_goal, desired_goal, p)
# OVER OTHER PARKING SPOTS REWARD
over_other_parking_spots_reward = self.OVER_OTHER_PARKING_SPOT_REWARD * np.squeeze(
info["is_over_others_parking_spot"])
# COLLISION REWARD
collision_reward = self.COLLISION_REWARD * np.squeeze(
info["is_collision"])
# MOVING REWARD
# moving_reward = self.MOVING_REWARD * np.squeeze(info["velocity_idx"])
# REACHING THE GOAL REWARD
# reaching_goal_reward = self.REACHING_GOAL_REWARD * np.squeeze(info["is_success"])
reward = (distance_to_goal_reward + \
over_other_parking_spots_reward + \
# reverse_reward + \
# against_traffic_reward + \
# moving_reward +\
# reaching_goal_reward + \
collision_reward)
reward /= self.REWARD_SCALE
#print(reward)
return reward
def _reward(self, action):
raise NotImplementedError
def _is_success(self, achieved_goal, desired_goal):
# DISTANCE TO GOAL
distance_to_goal_reward = self.distance_2_goal_reward(
achieved_goal, desired_goal)
#print(distance_to_goal_reward)
self.vehicle.is_success = (distance_to_goal_reward >
-self.SUCCESS_THRESHOLD)
return self.vehicle.is_success
# Let's try something new: Dicouple everything
# Let me start defining the thresholds in SI units ( m, m/s, degrees)
# x_error_thr = 0.1
# y_error_thr = 0.1
# vx_error_thr = 0.1 #
# vy_error_thr = 0.1 #0.27
# heading_error_thr = np.deg2rad(5)
# cos_h_error_thr = np.cos(heading_error_thr)
# sin_h_error_thr = np.sin(heading_error_thr)
# thresholds = [x_error_thr, y_error_thr, vx_error_thr, vy_error_thr, cos_h_error_thr, sin_h_error_thr]
# errors = self.OBS_SCALE * np.abs(desired_goal - achieved_goal)
# success = np.less_equal(errors,thresholds)
# self.vehicle.is_success = np.all(success)
# return self.vehicle.is_success
def _is_terminal(self):
"""
The episode is over if the ego vehicle crashed or the goal is reached.
"""
# The episode cannot terminate unless all time steps are done. The reason for this is that HER + DDPG uses constant
# length episodes. If you plan to use other algorithms, please uncomment this line
#if info["is_collision"] or info["is_success"]:
if self.vehicle.crashed: # or self.vehicle.is_success:
self.reset()
return False # self.vehicle.crashed or self._is_success(obs['achieved_goal'], obs['desired_goal'])