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)