def _create_vehicles(self):
"""
Create some new random vehicles of a given type, and add them on the road.
"""
self.vehicle = Vehicle(self.road, [0, 0],
2 * np.pi * self.np_random.rand(), 0)
self.road.vehicles.append(self.vehicle)
lane = self.np_random.choice(self.road.network.lanes_list())
self.goal = Obstacle(self.road,
lane.position(lane.length / 2, 0),
heading=lane.heading)
self.goal.COLLISIONS_ENABLED = False
self.road.vehicles.insert(0, self.goal)
def main():
env = gym.make('highway-v0')
road = Road.create_random_road(lanes_count=2,
lane_width=4.0,
vehicles_count=0,
vehicles_type=IDMVehicle)
vehicle = MDPVehicle.create_random(road)
road.vehicles.append(vehicle)
env.road = road
env.vehicle = vehicle
agent = MCTSAgent(env, iterations=100, temperature=20 *
5) # compare step by subtree and step by prior
sim = Simulation(env, agent)
t = 0
while not sim.done:
sim.step()
t += 1
if t == 10:
print('Added obstacle')
env.road.vehicles.append(
Obstacle(
road,
[env.vehicle.position[0] + 50., env.vehicle.position[1]]))
sim.close()
def _create_vehicles(self, parked_probability=0.75):
"""
Create some new random vehicles of a given type, and add them on the road.
"""
self.vehicle = Vehicle(self.road, self.vehicle_starting, 2 * np.pi * self.np_random.rand(), 0)
self.road.vehicles.append(self.vehicle)
goal_position = [self.np_random.choice([-2 * self.spots - 10, 2 * self.spots + 10]), 0]
self.goal = Obstacle(self.road, goal_position, heading=0)
self.goal.COLLISIONS_ENABLED = False
self.road.vehicles.insert(0, self.goal)
vehicles_type = utils.class_from_path(self.config["other_vehicles_type"])
for i in range(self.config["vehicles_count"]):
is_parked = self.np_random.rand() <= parked_probability
if not is_parked:
# Just an effort to spread the vehicles out
idx = self.np_random.randint(0, self.num_middle_lanes)
longitudinal = (i * 5) - (self.x_range / 8) * self.np_random.randint(-1, 1)
self.road.vehicles.append(
vehicles_type.make_on_lane(self.road, ("d", "e", idx), longitudinal, velocity=2))
else:
lane = ("a", "b", i) if self.np_random.rand() >= 0.5 else ("b", "c", i)
self.road.vehicles.append(Vehicle.make_on_lane(self.road, lane, 4, velocity=0))
for v in self.road.vehicles: # Prevent early collisions
if v is not self.vehicle and np.linalg.norm(v.position - self.vehicle.position) < 20:
self.road.vehicles.remove(v)
def _make_road(self):
"""
Make a road composed of a straight highway and a merging lane.
:return: the road
"""
net = RoadNetwork()
# Highway 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], [c, c]]
line_type_merge = [[c, s], [c, 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]))
'''
i = 1
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=self.np_random)
road.vehicles.append(Obstacle(road, lbc.position(ends[2], 0)))
self.road = road
def generate_data(count):
# Vehicle.COLLISIONS_ENABLED = False
vehicle_type = LinearVehicle
road = Road.create_random_road(lanes_count=2,
lane_width=4.0,
vehicles_count=5,
vehicles_type=vehicle_type)
sim = Simulation(road, ego_vehicle_type=vehicle_type, displayed=True)
sim.RECORD_VIDEO = False
road.add_random_vehicles(5, vehicles_type=vehicle_type)
road.vehicles.append(Obstacle(road, np.array([50., 0])))
road.vehicles.append(Obstacle(road, np.array([130., 4.])))
for v in road.vehicles:
v.target_velocity = LinearVehicle.VELOCITY_WANTED
# v.enable_lane_change = False
for _ in range(count):
sim.handle_events()
sim.act()
sim.road.dump()
sim.step()
sim.display()
sim.quit()
return [v.get_log() for v in road.vehicles if not isinstance(v, Obstacle)]
def make_straight(self):
lm10 = StraightLane(np.array([0, 0]),
0,
4.0, [LineType.CONTINUOUS_LINE, LineType.STRIPED],
bounds=[0, 500])
l1 = LanesConcatenation([lm10])
lm20 = StraightLane(l1.position(0, 4),
0,
4.0, [LineType.STRIPED, LineType.STRIPED],
bounds=[0, 500])
l2 = LanesConcatenation([lm20])
# lm30 = StraightLane(l2.position(0,4), 0, 4.0, [LineType.STRIPED, LineType.STRIPED],bounds=[0,100])
# lm31 = StraightLane(lm30.position(0,0), 0, 4.0, [LineType.STRIPED, LineType.STRIPED],bounds=[0,500])
# l3 = LanesConcatenation([lm30,lm31])
lm30 = StraightLane(l2.position(0, 4),
0,
4.0, [LineType.STRIPED, LineType.STRIPED],
bounds=[0, 500])
l3 = LanesConcatenation([lm30])
amplitude = 4.5
lm40 = StraightLane(l3.position(0, 4),
0,
4.0, [LineType.STRIPED, LineType.CONTINUOUS_LINE],
bounds=[200, 400])
lm41 = SineLane(lm40.position(400, amplitude),
0,
4.0,
-amplitude,
2 * np.pi / (2 * 50),
np.pi / 2, [LineType.CONTINUOUS, LineType.CONTINUOUS],
bounds=[0, 50],
forbidden=True)
lm42 = StraightLane(
lm41.position(50, 0),
0,
4.0, [LineType.CONTINUOUS_LINE, LineType.CONTINUOUS_LINE],
bounds=[0, 50],
forbidden=True)
l4 = LanesConcatenation([lm40, lm41, lm42])
road = Road([l1, l2, l3, l4])
# road = Road([ l3])
# road = Road([lm0,lm2])
# todo !!!!!!!!!!! how to do with Obstacle in lane.vehicles
obstacle = Obstacle(road, lm40.position(0, 0))
road.vehicles.append(obstacle)
road.lanes[3].vehicles.append(obstacle)
self.road = road
def make_vehicles(self):
"""
Populate a road with several vehicles on the highway and on the merging lane, as well as an ego-vehicle.
:return: the ego-vehicle
"""
# road = self.road
ego_birth = [("se", "inters", 0), ("se", "inters", 1), ("se", "inters", 2)]
ego_lane_index = ego_birth[0]
ego_lane = self.road.network.get_lane(ego_lane_index)
position = ego_lane.position(0, 0)
self.destination = self.exit[2]
ego_vehicle = MPCControlledVehicle(self.road,
position,
velocity=10,
heading=ego_lane.heading_at(position)).plan_route_to(self.destination)
if self.destination == "inter_w":
self.destination_trafficlight = self.traffic_lights[0]
elif self.destination == "inter_n":
self.destination_trafficlight = self.traffic_lights[1]
elif self.destination == "inter_e":
self.destination_trafficlight = self.traffic_lights[2]
ego_vehicle.id = 0
# ego_vehicle1.myimage = pygame.image.load("../red_alpha_resize.png")
self.road.vehicles.append(ego_vehicle)
self.vehicle = ego_vehicle
# print("vehicle lane_index:", self.vehicle.lane_index)
lanes = [("se", "inters", 0), ("se", "inters", 1), ("se", "inters", 2)]
lanes.remove(ego_lane_index)
lane_1 = ego_lane
lane_2 = self.road.network.get_lane(lanes[0])
lane_3 = self.road.network.get_lane(lanes[1])
obstacle_1 = Obstacle(self.road, lane_1.position(np.random.randint(10, ego_lane.length-5), -1), LENGTH=2.0,
WIDTH=4.0)
# obstacle_2 = Obstacle(self.road, lane_2.position(np.random.randint(10, ego_lane.length-5), 0))
# obstacle_3 = Obstacle(self.road, lane_3.position(np.random.randint(10, ego_lane.length-5), 0))
self.road.vehicles.extend([obstacle_1])
self.obstacles.extend([ obstacle_1])
def make_roads(self):
net = RoadNetwork()
n, c, s = LineType.NONE, LineType.CONTINUOUS, LineType.STRIPED
net.add_lane(
"s1", "ex",
StraightLane(np.array([0, 0]),
np.array([100, 0]),
line_types=[c, s]))
net.add_lane(
"ex", "em",
StraightLane(np.array([100, 0]),
np.array([200, 0]),
line_types=[c, s]))
net.add_lane(
"em", "x1",
StraightLane(np.array([200, 0]),
np.array([300, 0]),
line_types=[c, s]))
# lm10 = StraightLane(np.array([0, 0]), 0, 4.0, [LineType.CONTINUOUS_LINE, LineType.STRIPED],bounds=[0,300])
# l1 = LanesConcatenation([lm10])
net.add_lane(
"s1", "ex",
StraightLane(np.array([0, 4]),
np.array([100, 4]),
line_types=[s, s]))
net.add_lane(
"ex", "em",
StraightLane(np.array([100, 4]),
np.array([200, 4]),
line_types=[s, s]))
net.add_lane(
"em", "x1",
StraightLane(np.array([200, 4]),
np.array([300, 4]),
line_types=[s, s]))
# lm20 = StraightLane(l1.position(0,4), 0, 4.0, [LineType.STRIPED, LineType.STRIPED],bounds=[0,300])
# l2 = LanesConcatenation([lm20])
net.add_lane(
"s1", "ex",
StraightLane(np.array([0, 8]),
np.array([100, 8]),
line_types=[s, c]))
# lm30 = StraightLane(l2.position(0,4), 0, 4.0, [LineType.STRIPED, LineType.CONTINUOUS_LINE],bounds=[0,100])
net.add_lane(
"ex", "em",
StraightLane(np.array([100, 8]),
np.array([200, 8]),
line_types=[s, n]))
net.add_lane(
"em", "x1",
StraightLane(np.array([200, 8]),
np.array([300, 8]),
line_types=[s, c]))
# lm31 = StraightLane(lm30.position(0,0), 0, 4.0, [LineType.STRIPED, LineType.STRIPED],bounds=[0,300])
# l3 = LanesConcatenation([lm30,lm31])
amplitude = 4.5
net.add_lane(
"s2", "ee",
StraightLane(np.array([0, 8 + 3 * amplitude]),
np.array([50, 8 + 3 * amplitude]),
line_types=[c, c],
forbidden=True))
# lm40 = StraightLane(l3.position(0,2*amplitude+4), 0, 4.0, [LineType.CONTINUOUS_LINE, LineType.CONTINUOUS_LINE],bounds=[0,50])
net.add_lane(
"ee", "ex",
SineLane(np.array([50, 8 + 2 * amplitude]),
np.array([100, 8 + 2 * amplitude]),
amplitude,
2 * np.pi / (2 * 50),
np.pi / 2,
line_types=[c, c],
forbidden=True))
# lm41 = SineLane(lm40.position(50, -amplitude), 0, 4.0, amplitude, 2 * np.pi / (2*50), np.pi / 2,
# [LineType.CONTINUOUS, LineType.CONTINUOUS], bounds=[0, 50], forbidden=True)
net.add_lane(
"ex", "over",
StraightLane(np.array([100, 8 + amplitude]),
np.array([200, 8 + amplitude]),
line_types=[s, c],
forbidden=True))
# net.add_lane("over", "x1",
# SineLane(np.array([200, 8 + amplitude/2]), np.array([220, 8 + amplitude/2]), amplitude/2,
# 2 * np.pi / (2 * 50), np.pi / 2, line_types=[c, c], forbidden=True))
# lm42 = StraightLane(lm41.position(50,0), 0, 4.0, [LineType.STRIPED, LineType.CONTINUOUS_LINE],bounds=[0,150],forbidden=True)
# l4 = LanesConcatenation([lm40,lm41,lm42])
# road = Road([l1,l2,l3,l4])
# road = Road([ l3])
# road = Road([lm0,lm2])
road = Road(network=net, np_random=self.np_random)
road.vehicles.append(Obstacle(road, [200, 8 + amplitude]))
self.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
OBS_SCALE = 100
REWARD_WEIGHTS = [1 / 100, 1 / 100, 1 / 100, 1 / 100, 1 / 10, 1 / 10]
SUCCESS_GOAL_REWARD = 0.15
DEFAULT_CONFIG = {"centering_position": [0.5, 0.5]}
OBSERVATION_FEATURES = ['x', 'y', 'vx', 'vy', 'cos_h', 'sin_h']
OBSERVATION_VEHICLES = 1
NORMALIZE_OBS = False
def __init__(self):
super(ParkingEnv, self).__init__()
self.config = self.DEFAULT_CONFIG.copy()
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({
"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'])
}
reward = self.compute_reward(obs['achieved_goal'], obs['desired_goal'],
info)
terminal = self._is_terminal()
return obs, reward, terminal, info
def reset(self):
self._create_road()
self._create_vehicles()
return self._observation()
def configure(self, config):
self.config.update(config)
def _create_road(self, spots=15):
"""
Create a road composed of straight adjacent lanes.
"""
net = RoadNetwork()
width = 4.0
lt = (LineType.CONTINUOUS, LineType.CONTINUOUS)
x_offset = 0
y_offset = 10
length = 8
for k in range(spots):
x = (k - spots // 2) * (width + x_offset) - width / 2
net.add_lane(
"a", "b",
StraightLane([x, y_offset], [x, y_offset + length],
width=width,
line_types=lt))
net.add_lane(
"b", "c",
StraightLane([x, -y_offset], [x, -y_offset - length],
width=width,
line_types=lt))
self.road = Road(network=net, np_random=self.np_random)
def _create_vehicles(self):
"""
Create some new random vehicles of a given type, and add them on the road.
"""
self.vehicle = Vehicle(self.road, [0, 0],
2 * np.pi * self.np_random.rand(), 0)
self.road.vehicles.append(self.vehicle)
lane = self.np_random.choice(self.road.network.lanes_list())
self.goal = Obstacle(self.road,
lane.position(lane.length / 2, 0),
heading=lane.heading)
self.goal.COLLISIONS_ENABLED = False
self.road.vehicles.insert(0, self.goal)
def _observation(self):
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 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)
def _reward(self, action):
raise NotImplementedError
def _is_success(self, achieved_goal, desired_goal):
return self.compute_reward(achieved_goal, desired_goal,
None) > -self.SUCCESS_GOAL_REWARD
def _is_terminal(self):
"""
The episode is over if the ego vehicle crashed or the goal is reached.
"""
obs = self._observation()
return self.vehicle.crashed # or self._is_success(obs['achieved_goal'], obs['desired_goal'])
def make_sin(self):
# amplitude = 4.5
amplitude = 9.0
lm10 = StraightLane(np.array([0, 0]),
0,
5.0, [LineType.CONTINUOUS_LINE, LineType.STRIPED],
bounds=[0, 400])
lm11 = SineLane(lm10.position(400, amplitude),
0,
5.0,
-amplitude,
2 * np.pi / (2 * 50),
np.pi / 2, [LineType.CONTINUOUS, LineType.STRIPED],
bounds=[0, 250])
lm12 = StraightLane(lm11.position(250, 0),
0,
5.0, [LineType.CONTINUOUS_LINE, LineType.STRIPED],
bounds=[0, 50])
l1 = LanesConcatenation([lm10, lm11, lm12])
lm20 = StraightLane(lm10.position(0, 5),
0,
5.0, [LineType.STRIPED, LineType.STRIPED],
bounds=[0, 400])
lm21 = SineLane(lm20.position(400, amplitude),
0,
5.0,
-amplitude,
2 * np.pi / (2 * 50),
np.pi / 2, [LineType.STRIPED, LineType.STRIPED],
bounds=[0, 250])
lm22 = StraightLane(lm21.position(250, 0),
0,
5.0, [LineType.STRIPED, LineType.STRIPED],
bounds=[0, 50])
l2 = LanesConcatenation([lm20, lm21, lm22])
lm30 = StraightLane(lm20.position(0, 5),
0,
5.0, [LineType.STRIPED, LineType.STRIPED],
bounds=[0, 400])
lm31 = SineLane(lm30.position(400, amplitude),
0,
5.0,
-amplitude,
2 * np.pi / (2 * 50),
np.pi / 2, [LineType.STRIPED, LineType.STRIPED],
bounds=[0, 250])
lm32 = StraightLane(lm31.position(250, 0),
0,
5.0, [LineType.STRIPED, LineType.STRIPED],
bounds=[0, 50])
l3 = LanesConcatenation([lm30, lm31, lm32])
lm40 = StraightLane(lm30.position(0, 5),
0,
5.0, [LineType.STRIPED, LineType.CONTINUOUS_LINE],
bounds=[0, 400])
lm41 = SineLane(lm40.position(400, amplitude),
0,
5.0,
-amplitude,
2 * np.pi / (2 * 50),
np.pi / 2, [LineType.STRIPED, LineType.CONTINUOUS],
bounds=[0, 250])
lm42 = StraightLane(
lm41.position(250, 0),
0,
5.0,
[LineType.STRIPED, LineType.CONTINUOUS_LINE],
bounds=[0, 50],
)
l4 = LanesConcatenation([lm40, lm41, lm42])
road = Road([l1, l2, l3, l4])
# road = Road([ l3])
# road = Road([lm0,lm2])
# todo !!!!!!!!!!! how to do with Obstacle in lane.vehicles
obstacle = Obstacle(road, lm40.position(0, 0))
road.vehicles.append(obstacle)
road.lanes[3].vehicles.append(obstacle)
self.road = road