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'])