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 test_step():
v = Vehicle(road=None, position=[0, 0], velocity=20, heading=0)
for _ in range(2*FPS):
v.step(dt=1/FPS)
assert v.position[0] == pytest.approx(40)
assert v.position[1] == pytest.approx(0)
assert v.velocity == pytest.approx(20)
assert v.heading == pytest.approx(0)
def test_act():
v = Vehicle(road=None, position=[0, 0], velocity=20, heading=0)
v.act({'acceleration': 1, 'steering': 0})
for _ in range(1 * FPS):
v.step(dt=1/FPS)
assert v.velocity == pytest.approx(21)
v.act({'acceleration': 0, 'steering': 0.5})
for _ in range(1 * FPS):
v.step(dt=1/FPS)
assert v.velocity == pytest.approx(21)
assert v.position[1] > 0
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 test():
from highway_env.vehicle.dynamics import Vehicle
r = None
v = Vehicle(r, [0, 0], 0, 20)
v.dump()
v.dump()
v.dump()
v.dump()
print(v.get_log())
def _create_vehicles(self):
"""
Create some new random vehicles of a given type, and add them on the road.
"""
self.vehicle = Vehicle.create_random(
self.road, 25, spacing=self.config["initial_spacing"])
self.road.vehicles.append(self.vehicle)
vehicles_type = utils.class_from_path(
self.config["other_vehicles_type"]
) # IDM from the config: can change
for _ in range(self.config["vehicles_count"]):
self.road.vehicles.append(vehicles_type.create_random(self.road))
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)
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 test_brake():
v = Vehicle(road=None, position=[0, 0], velocity=20, heading=0)
for _ in range(10 * FPS):
v.act({'acceleration': min(max(-1*v.velocity, -6), 6), 'steering': 0})
v.step(dt=1/FPS)
assert v.velocity == pytest.approx(0, abs=0.01)