From 39cc1a6560f390fce417a650f99bc5a0cac0fedc Mon Sep 17 00:00:00 2001 From: snow-fox Date: Sat, 18 Nov 2023 13:05:09 +0000 Subject: [PATCH 1/9] support all python versions --- .github/workflows/build.yml | 8 ++++++-- setup.cfg | 4 +--- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 29c1d44ea..51745f86a 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -14,12 +14,16 @@ jobs: runs-on: ubuntu-latest + strategy: + matrix: + python-version: ["3.8", "3.9", "3.10", "3.11"] + steps: - uses: actions/checkout@v2 - - name: Set up Python 3.8 + - name: Set up Python uses: actions/setup-python@v1 with: - python-version: 3.8 + python-version: ${{ matrix.python-version }} - name: Install dependencies run: | python -m pip install --upgrade pip diff --git a/setup.cfg b/setup.cfg index 71e73335b..876252256 100644 --- a/setup.cfg +++ b/setup.cfg @@ -10,9 +10,7 @@ url=https://github.com/eleurent/highway-env license=MIT classifiers= Development Status :: 5 - Production/Stable - Programming Language :: Python - Programming Language :: Python :: 3 :: Only - Programming Language :: Python :: 3.8 + Programming Language :: Python :: 3 License :: OSI Approved :: MIT License From 5907652bbdf22a03865714300c676f08da97fab8 Mon Sep 17 00:00:00 2001 From: snow-fox Date: Sat, 18 Nov 2023 13:16:12 +0000 Subject: [PATCH 2/9] update gitignore --- .gitignore | 146 ++++++++++++++++++++++++++++++++++++++++++++++------- 1 file changed, 129 insertions(+), 17 deletions(-) diff --git a/.gitignore b/.gitignore index 366cfceaf..73358ad82 100644 --- a/.gitignore +++ b/.gitignore @@ -1,27 +1,139 @@ -# Compiled sources -*.pyc +# Byte-compiled / optimized / DLL files +__pycache__/ +*.py[cod] +*$py.class + +# C extensions +*.so + +# Distribution / packaging +.Python build/ +develop-eggs/ dist/ +downloads/ +eggs/ +.eggs/ +lib/ +lib64/ +parts/ +sdist/ +var/ +wheels/ +share/python-wheels/ +*.egg-info/ +.installed.cfg +*.egg +MANIFEST + +# PyInstaller +# Usually these files are written by a python script from a template +# before PyInstaller builds the exe, so as to inject date/other infos into it. +*.manifest +*.spec + +# Installer logs +pip-log.txt +pip-delete-this-directory.txt + +# Unit test / coverage reports +htmlcov/ +.tox/ +.nox/ +.coverage +.coverage.* +.cache +nosetests.xml +coverage.xml +*.cover +*.py,cover +.hypothesis/ +.pytest_cache/ +cover/ -# Setup -**.egg* +# Translations +*.mo +*.pot -# Jupyter notebooks -**.ipynb_checkpoints* +# Django stuff: +*.log +local_settings.py +db.sqlite3 +db.sqlite3-journal + +# Flask stuff: +instance/ +.webassets-cache + +# Scrapy stuff: +.scrapy # Sphinx documentation -_build +docs/_build/ -# Editor files -.idea +# PyBuilder +.pybuilder/ +target/ -# Test files -.pytest_cache -.cache +# Jupyter Notebook +.ipynb_checkpoints + +# IPython +profile_default/ +ipython_config.py + +# pyenv +# For a library or package, you might want to ignore these files since the code is +# intended to run in multiple environments; otherwise, check them in: +# .python-version + +# pipenv +# According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. +# However, in case of collaboration, if having platform-specific dependencies or dependencies +# having no cross-platform support, pipenv may install dependencies that don't work, or not +# install all needed dependencies. +#Pipfile.lock + +# PEP 582; used by e.g. github.com/David-OConnor/pyflow +__pypackages__/ + +# Celery stuff +celerybeat-schedule +celerybeat.pid + +# SageMath parsed files +*.sage.py + +# Environments +.env +.venv +env/ +venv/ +ENV/ +env.bak/ +venv.bak/ + +# Spyder project settings +.spyderproject +.spyproject + +# Rope project settings +.ropeproject + +# mkdocs documentation +/site + +# mypy +.mypy_cache/ +.dmypy.json +dmypy.json + +# Pyre type checker +.pyre/ + +# pytype static type analyzer +.pytype/ -# Outputs -**/out/* +# Cython debug symbols +cython_debug/ -# Docs -docs/_build -**jupyter_execute \ No newline at end of file From 267a3cada0c114a5df51acd697c8813e91e5bc56 Mon Sep 17 00:00:00 2001 From: snow-fox Date: Sat, 18 Nov 2023 13:17:23 +0000 Subject: [PATCH 3/9] add precommit config --- .pre-commit-config.yaml | 70 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 70 insertions(+) create mode 100644 .pre-commit-config.yaml diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 000000000..817a82a39 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,70 @@ +# See https://pre-commit.com for more information +# See https://pre-commit.com/hooks.html for more hooks +repos: + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v4.4.0 + hooks: + - id: check-symlinks + - id: destroyed-symlinks + - id: trailing-whitespace + - id: end-of-file-fixer + - id: check-yaml + - id: check-toml + - id: check-ast + - id: check-added-large-files + - id: check-merge-conflict + - id: check-executables-have-shebangs + - id: check-shebang-scripts-are-executable + - id: detect-private-key + - id: debug-statements + - repo: https://github.com/codespell-project/codespell + rev: v2.2.4 + hooks: + - id: codespell + args: + - --ignore-words-list=reacher,ure,referenc,wile + - repo: https://github.com/PyCQA/flake8 + rev: 6.0.0 + hooks: + - id: flake8 + args: + - '--per-file-ignores=*/__init__.py:F401 gymnasium/envs/registration.py:E704 docs/tutorials/*.py:E402 gymnasium/experimental/wrappers/__init__.py:E402' + - --ignore=E203,W503,E741 + - --max-complexity=30 + - --max-line-length=456 + - --show-source + - --statistics + - repo: https://github.com/asottile/pyupgrade + rev: v3.3.2 + hooks: + - id: pyupgrade + args: ["--py38-plus"] + - repo: https://github.com/PyCQA/isort + rev: 5.12.0 + hooks: + - id: isort + - repo: https://github.com/python/black + rev: 23.3.0 + hooks: + - id: black + - repo: https://github.com/pycqa/pydocstyle + rev: 6.3.0 + hooks: + - id: pydocstyle + exclude: ^(gymnasium/envs/box2d)|(gymnasium/envs/classic_control)|(gymnasium/envs/mujoco)|(gymnasium/envs/toy_text)|(tests/envs)|(tests/spaces)|(tests/utils)|(tests/vector)|(docs/) + args: + - --source + - --explain + - --convention=google + additional_dependencies: ["tomli"] + - repo: local + hooks: + - id: pyright + name: pyright + entry: pyright + language: node + pass_filenames: false + types: [python] + additional_dependencies: ["pyright@1.1.308"] + args: + - --project=pyproject.toml From 4b58fee91f4d89822c26df707bbc2004ff548f15 Mon Sep 17 00:00:00 2001 From: snow-fox Date: Sat, 18 Nov 2023 13:25:33 +0000 Subject: [PATCH 4/9] update precommit --- .pre-commit-config.yaml | 54 +++-------------------------------------- 1 file changed, 4 insertions(+), 50 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 817a82a39..719cf7f87 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,70 +1,24 @@ # See https://pre-commit.com for more information # See https://pre-commit.com/hooks.html for more hooks repos: - - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.4.0 - hooks: - - id: check-symlinks - - id: destroyed-symlinks - - id: trailing-whitespace - - id: end-of-file-fixer - - id: check-yaml - - id: check-toml - - id: check-ast - - id: check-added-large-files - - id: check-merge-conflict - - id: check-executables-have-shebangs - - id: check-shebang-scripts-are-executable - - id: detect-private-key - - id: debug-statements - - repo: https://github.com/codespell-project/codespell - rev: v2.2.4 - hooks: - - id: codespell - args: - - --ignore-words-list=reacher,ure,referenc,wile - repo: https://github.com/PyCQA/flake8 rev: 6.0.0 hooks: - id: flake8 args: - - '--per-file-ignores=*/__init__.py:F401 gymnasium/envs/registration.py:E704 docs/tutorials/*.py:E402 gymnasium/experimental/wrappers/__init__.py:E402' - - --ignore=E203,W503,E741 + - '--per-file-ignores=**/__init__.py:F401,F403' + - --ignore=E203,W503,E741,E731 - --max-complexity=30 - --max-line-length=456 - --show-source - --statistics - - repo: https://github.com/asottile/pyupgrade - rev: v3.3.2 - hooks: - - id: pyupgrade - args: ["--py38-plus"] - repo: https://github.com/PyCQA/isort rev: 5.12.0 hooks: - id: isort + args: ["--profile", "black"] + - skip: ["**/__init__.py"] - repo: https://github.com/python/black rev: 23.3.0 hooks: - id: black - - repo: https://github.com/pycqa/pydocstyle - rev: 6.3.0 - hooks: - - id: pydocstyle - exclude: ^(gymnasium/envs/box2d)|(gymnasium/envs/classic_control)|(gymnasium/envs/mujoco)|(gymnasium/envs/toy_text)|(tests/envs)|(tests/spaces)|(tests/utils)|(tests/vector)|(docs/) - args: - - --source - - --explain - - --convention=google - additional_dependencies: ["tomli"] - - repo: local - hooks: - - id: pyright - name: pyright - entry: pyright - language: node - pass_filenames: false - types: [python] - additional_dependencies: ["pyright@1.1.308"] - args: - - --project=pyproject.toml From e95e6f7ba68f9cf1dfe594fedf4d6edd0e52710b Mon Sep 17 00:00:00 2001 From: snow-fox Date: Sat, 18 Nov 2023 13:42:03 +0000 Subject: [PATCH 5/9] black flake8 isort --- .pre-commit-config.yaml | 2 +- docs/conf.py | 30 +- docs/scripts/move_404.py | 2 +- highway_env/__init__.py | 71 ++- highway_env/envs/common/abstract.py | 101 ++-- highway_env/envs/common/action.py | 191 ++++---- highway_env/envs/common/finite_mdp.py | 93 ++-- highway_env/envs/common/graphics.py | 120 +++-- highway_env/envs/common/observation.py | 441 ++++++++++++------ highway_env/envs/exit_env.py | 187 +++++--- highway_env/envs/highway_env.py | 124 +++-- highway_env/envs/intersection_env.py | 394 ++++++++++------ highway_env/envs/lane_keeping_env.py | 118 +++-- highway_env/envs/merge_env.py | 111 +++-- highway_env/envs/parking_env.py | 157 +++++-- highway_env/envs/racetrack_env.py | 410 +++++++++++----- highway_env/envs/roundabout_env.py | 387 +++++++++++---- highway_env/envs/two_way_env.py | 116 +++-- highway_env/envs/u_turn_env.py | 227 +++++---- highway_env/interval.py | 168 +++++-- highway_env/road/graphics.py | 194 ++++++-- highway_env/road/lane.py | 176 ++++--- highway_env/road/regulation.py | 46 +- highway_env/road/road.py | 280 +++++++---- highway_env/road/spline.py | 3 +- highway_env/utils.py | 140 ++++-- highway_env/vehicle/behavior.py | 375 +++++++++------ highway_env/vehicle/controller.py | 204 +++++--- highway_env/vehicle/dynamics.py | 214 ++++++--- highway_env/vehicle/graphics.py | 150 ++++-- highway_env/vehicle/kinematics.py | 173 ++++--- highway_env/vehicle/objects.py | 108 +++-- highway_env/vehicle/uncertainty/estimation.py | 79 +++- highway_env/vehicle/uncertainty/prediction.py | 278 +++++++---- scripts/sb3_highway_dqn.py | 34 +- scripts/sb3_highway_dqn_cnn.py | 63 +-- scripts/sb3_highway_ppo.py | 31 +- scripts/sb3_highway_ppo_transformer.py | 252 +++++----- scripts/sb3_racetracks_ppo.py | 33 +- scripts/utils.py | 20 +- setup.py | 2 +- tests/envs/test_actions.py | 1 + tests/envs/test_env_preprocessors.py | 5 +- tests/envs/test_gym.py | 1 + tests/envs/test_time.py | 6 +- tests/graphics/test_render.py | 34 +- tests/road/test_road.py | 6 +- tests/test_utils.py | 8 +- tests/vehicle/test_behavior.py | 8 +- tests/vehicle/test_control.py | 28 +- tests/vehicle/test_dynamics.py | 18 +- tests/vehicle/test_uncertainty.py | 4 +- 52 files changed, 4235 insertions(+), 2189 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 719cf7f87..e9d84ff8a 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -17,7 +17,7 @@ repos: hooks: - id: isort args: ["--profile", "black"] - - skip: ["**/__init__.py"] + exclude: "__init__.py" - repo: https://github.com/python/black rev: 23.3.0 hooks: diff --git a/docs/conf.py b/docs/conf.py index 0ca708a77..7e3561ab1 100644 --- a/docs/conf.py +++ b/docs/conf.py @@ -20,14 +20,13 @@ import highway_env - project = "highway-env" copyright = "2023 Farama Foundation" author = "Farama Foundation" # The full version, including alpha/beta/rc tags # release = highway_env.__version__ -release = '' +release = "" # -- General configuration --------------------------------------------------- @@ -36,27 +35,32 @@ # ones. extensions = [ "sphinx.ext.napoleon", - 'sphinx.ext.coverage', + "sphinx.ext.coverage", "sphinx.ext.doctest", "sphinx.ext.autodoc", "sphinx.ext.githubpages", - 'sphinx.ext.viewcode', - 'sphinx.ext.autosectionlabel', - 'sphinxcontrib.bibtex', + "sphinx.ext.viewcode", + "sphinx.ext.autosectionlabel", + "sphinxcontrib.bibtex", "jupyter_sphinx", "myst_parser", ] -autodoc_default_flags = ['members', 'private-members', 'undoc-members', 'special-members'] -autodoc_member_order = 'bysource' +autodoc_default_flags = [ + "members", + "private-members", + "undoc-members", + "special-members", +] +autodoc_member_order = "bysource" # Add any paths that contain templates here, relative to this directory. -templates_path = ['_templates'] +templates_path = ["_templates"] # List of patterns, relative to source directory, that match files and # directories to ignore when looking for source files. # This pattern also affects html_static_path and html_extra_path . -exclude_patterns = ['_build', 'Thumbs.db', '.DS_Store'] +exclude_patterns = ["_build", "Thumbs.db", ".DS_Store"] # Napoleon settings # napoleon_use_ivar = True @@ -102,9 +106,9 @@ # -- BibTeX ------------------------------------------------------------- -bibtex_bibfiles = ['bibliography/biblio.bib'] -bibtex_encoding = 'latin' -bibtex_default_style = 'alpha' +bibtex_bibfiles = ["bibliography/biblio.bib"] +bibtex_encoding = "latin" +bibtex_default_style = "alpha" # -- Generate Changelog ------------------------------------------------- diff --git a/docs/scripts/move_404.py b/docs/scripts/move_404.py index 2b5db551f..ef6a18037 100644 --- a/docs/scripts/move_404.py +++ b/docs/scripts/move_404.py @@ -13,4 +13,4 @@ fp.seek(0) fp.truncate() - fp.write(content) \ No newline at end of file + fp.write(content) diff --git a/highway_env/__init__.py b/highway_env/__init__.py index 04edaebb3..3f1c9ff09 100644 --- a/highway_env/__init__.py +++ b/highway_env/__init__.py @@ -1,103 +1,96 @@ # Hide pygame support prompt import os -os.environ['PYGAME_HIDE_SUPPORT_PROMPT'] = '1' -from gymnasium.envs.registration import register +os.environ["PYGAME_HIDE_SUPPORT_PROMPT"] = "1" +from gymnasium.envs.registration import register def register_highway_envs(): """Import the envs module so that envs register themselves.""" # exit_env.py register( - id='exit-v0', - entry_point='highway_env.envs:ExitEnv', + id="exit-v0", + entry_point="highway_env.envs:ExitEnv", ) # highway_env.py register( - id='highway-v0', - entry_point='highway_env.envs:HighwayEnv', + id="highway-v0", + entry_point="highway_env.envs:HighwayEnv", ) register( - id='highway-fast-v0', - entry_point='highway_env.envs:HighwayEnvFast', + id="highway-fast-v0", + entry_point="highway_env.envs:HighwayEnvFast", ) # intersection_env.py register( - id='intersection-v0', - entry_point='highway_env.envs:IntersectionEnv', + id="intersection-v0", + entry_point="highway_env.envs:IntersectionEnv", ) register( - id='intersection-v1', - entry_point='highway_env.envs:ContinuousIntersectionEnv', + id="intersection-v1", + entry_point="highway_env.envs:ContinuousIntersectionEnv", ) register( - id='intersection-multi-agent-v0', - entry_point='highway_env.envs:MultiAgentIntersectionEnv', + id="intersection-multi-agent-v0", + entry_point="highway_env.envs:MultiAgentIntersectionEnv", ) register( - id='intersection-multi-agent-v1', - entry_point='highway_env.envs:TupleMultiAgentIntersectionEnv', + id="intersection-multi-agent-v1", + entry_point="highway_env.envs:TupleMultiAgentIntersectionEnv", ) # lane_keeping_env.py register( - id='lane-keeping-v0', - entry_point='highway_env.envs:LaneKeepingEnv', - max_episode_steps=200 + id="lane-keeping-v0", + entry_point="highway_env.envs:LaneKeepingEnv", + max_episode_steps=200, ) # merge_env.py register( - id='merge-v0', - entry_point='highway_env.envs:MergeEnv', + id="merge-v0", + entry_point="highway_env.envs:MergeEnv", ) # parking_env.py register( - id='parking-v0', - entry_point='highway_env.envs:ParkingEnv', + id="parking-v0", + entry_point="highway_env.envs:ParkingEnv", ) register( - id='parking-ActionRepeat-v0', - entry_point='highway_env.envs:ParkingEnvActionRepeat' + id="parking-ActionRepeat-v0", + entry_point="highway_env.envs:ParkingEnvActionRepeat", ) register( - id='parking-parked-v0', - entry_point='highway_env.envs:ParkingEnvParkedVehicles' + id="parking-parked-v0", entry_point="highway_env.envs:ParkingEnvParkedVehicles" ) # racetrack_env.py register( - id='racetrack-v0', - entry_point='highway_env.envs:RacetrackEnv', + id="racetrack-v0", + entry_point="highway_env.envs:RacetrackEnv", ) # roundabout_env.py register( - id='roundabout-v0', - entry_point='highway_env.envs:RoundaboutEnv', + id="roundabout-v0", + entry_point="highway_env.envs:RoundaboutEnv", ) # two_way_env.py register( - id='two-way-v0', - entry_point='highway_env.envs:TwoWayEnv', - max_episode_steps=15 + id="two-way-v0", entry_point="highway_env.envs:TwoWayEnv", max_episode_steps=15 ) # u_turn_env.py - register( - id='u-turn-v0', - entry_point='highway_env.envs:UTurnEnv' - ) - + register(id="u-turn-v0", entry_point="highway_env.envs:UTurnEnv") diff --git a/highway_env/envs/common/abstract.py b/highway_env/envs/common/abstract.py index f60ffba52..1c8b62e7a 100644 --- a/highway_env/envs/common/abstract.py +++ b/highway_env/envs/common/abstract.py @@ -1,19 +1,18 @@ import copy import os -from typing import List, Tuple, Optional, Callable, TypeVar, Generic, Union, Dict, Text +from typing import Dict, List, Optional, Text, Tuple, TypeVar + import gymnasium as gym +import numpy as np from gymnasium import Wrapper from gymnasium.wrappers import RecordVideo -from gymnasium.utils import seeding -import numpy as np from highway_env import utils -from highway_env.envs.common.action import action_factory, Action, DiscreteMetaAction, ActionType -from highway_env.envs.common.observation import observation_factory, ObservationType +from highway_env.envs.common.action import Action, ActionType, action_factory from highway_env.envs.common.finite_mdp import finite_mdp from highway_env.envs.common.graphics import EnvViewer -from highway_env.vehicle.behavior import IDMVehicle, LinearVehicle -from highway_env.vehicle.controller import MDPVehicle +from highway_env.envs.common.observation import ObservationType, observation_factory +from highway_env.vehicle.behavior import IDMVehicle from highway_env.vehicle.kinematics import Vehicle Observation = TypeVar("Observation") @@ -28,11 +27,12 @@ class AbstractEnv(gym.Env): speed. The action space is fixed, but the observation space and reward function must be defined in the environment implementations. """ + observation_type: ObservationType action_type: ActionType _record_video_wrapper: Optional[RecordVideo] metadata = { - 'render_modes': ['human', 'rgb_array'], + "render_modes": ["human", "rgb_array"], } PERCEPTION_DISTANCE = 5.0 * Vehicle.MAX_SPEED @@ -89,12 +89,8 @@ def default_config(cls) -> dict: :return: a configuration dict """ return { - "observation": { - "type": "Kinematics" - }, - "action": { - "type": "DiscreteMetaAction" - }, + "observation": {"type": "Kinematics"}, + "action": {"type": "DiscreteMetaAction"}, "simulation_frequency": 15, # [Hz] "policy_frequency": 1, # [Hz] "other_vehicles_type": "highway_env.vehicle.behavior.IDMVehicle", @@ -106,7 +102,7 @@ def default_config(cls) -> dict: "render_agent": True, "offscreen_rendering": os.environ.get("OFFSCREEN_RENDERING", "0") == "1", "manual_control": False, - "real_time_rendering": False + "real_time_rendering": False, } def configure(self, config: dict) -> None: @@ -114,9 +110,12 @@ def configure(self, config: dict) -> None: self.config.update(config) def update_metadata(self, video_real_time_ratio=2): - frames_freq = self.config["simulation_frequency"] \ - if self._record_video_wrapper else self.config["policy_frequency"] - self.metadata['render_fps'] = video_real_time_ratio * frames_freq + frames_freq = ( + self.config["simulation_frequency"] + if self._record_video_wrapper + else self.config["policy_frequency"] + ) + self.metadata["render_fps"] = video_real_time_ratio * frames_freq def define_spaces(self) -> None: """ @@ -183,10 +182,11 @@ def _info(self, obs: Observation, action: Optional[Action] = None) -> dict: pass return info - def reset(self, - *, - seed: Optional[int] = None, - options: Optional[dict] = None, + def reset( + self, + *, + seed: Optional[int] = None, + options: Optional[dict] = None, ) -> Tuple[Observation, dict]: """ Reset the environment to it's initial configuration @@ -206,7 +206,7 @@ def reset(self, self.define_spaces() # Second, to link the obs and actions to the vehicles once the scene is created obs = self.observation_type.observe() info = self._info(obs, action=self.action_space.sample()) - if self.render_mode == 'human': + if self.render_mode == "human": self.render() return obs, info @@ -229,7 +229,9 @@ def step(self, action: Action) -> Tuple[Observation, float, bool, bool, dict]: :return: a tuple (observation, reward, terminated, truncated, info) """ if self.road is None or self.vehicle is None: - raise NotImplementedError("The road and vehicle must be initialized in the environment implementation") + raise NotImplementedError( + "The road and vehicle must be initialized in the environment implementation" + ) self.time += 1 / self.config["policy_frequency"] self._simulate(action) @@ -239,19 +241,28 @@ def step(self, action: Action) -> Tuple[Observation, float, bool, bool, dict]: terminated = self._is_terminated() truncated = self._is_truncated() info = self._info(obs, action) - if self.render_mode == 'human': + if self.render_mode == "human": self.render() return obs, reward, terminated, truncated, info def _simulate(self, action: Optional[Action] = None) -> None: """Perform several steps of simulation with constant action.""" - frames = int(self.config["simulation_frequency"] // self.config["policy_frequency"]) + frames = int( + self.config["simulation_frequency"] // self.config["policy_frequency"] + ) for frame in range(frames): # Forward action to the vehicle - if action is not None \ - and not self.config["manual_control"] \ - and self.steps % int(self.config["simulation_frequency"] // self.config["policy_frequency"]) == 0: + if ( + action is not None + and not self.config["manual_control"] + and self.steps + % int( + self.config["simulation_frequency"] + // self.config["policy_frequency"] + ) + == 0 + ): self.action_type.act(action) self.road.act() @@ -260,7 +271,9 @@ def _simulate(self, action: Optional[Action] = None) -> None: # Automatically render intermediate simulation steps if a viewer has been launched # Ignored if the rendering is done offscreen - if frame < frames - 1: # Last frame will be rendered through env.render() as usual + if ( + frame < frames - 1 + ): # Last frame will be rendered through env.render() as usual self._automatic_rendering() self.enable_auto_render = False @@ -288,7 +301,7 @@ def render(self) -> Optional[np.ndarray]: if not self.viewer.offscreen: self.viewer.handle_events() - if self.render_mode == 'rgb_array': + if self.render_mode == "rgb_array": image = self.viewer.get_image() return image @@ -318,13 +331,12 @@ def _automatic_rendering(self) -> None: If a RecordVideo wrapper has been set, use it to capture intermediate frames. """ if self.viewer is not None and self.enable_auto_render: - if self._record_video_wrapper and self._record_video_wrapper.video_recorder: self._record_video_wrapper.video_recorder.capture_frame() else: self.render() - def simplify(self) -> 'AbstractEnv': + def simplify(self) -> "AbstractEnv": """ Return a simplified copy of the environment where distant vehicles have been removed from the road. @@ -333,12 +345,15 @@ def simplify(self) -> 'AbstractEnv': :return: a simplified environment state """ state_copy = copy.deepcopy(self) - state_copy.road.vehicles = [state_copy.vehicle] + state_copy.road.close_vehicles_to( - state_copy.vehicle, self.PERCEPTION_DISTANCE) + state_copy.road.vehicles = [ + state_copy.vehicle + ] + state_copy.road.close_vehicles_to( + state_copy.vehicle, self.PERCEPTION_DISTANCE + ) return state_copy - def change_vehicles(self, vehicle_class_path: str) -> 'AbstractEnv': + def change_vehicles(self, vehicle_class_path: str) -> "AbstractEnv": """ Change the type of all vehicles on the road @@ -355,7 +370,7 @@ def change_vehicles(self, vehicle_class_path: str) -> 'AbstractEnv': vehicles[i] = vehicle_class.create_from(v) return env_copy - def set_preferred_lane(self, preferred_lane: int = None) -> 'AbstractEnv': + def set_preferred_lane(self, preferred_lane: int = None) -> "AbstractEnv": env_copy = copy.deepcopy(self) if preferred_lane: for v in env_copy.road.vehicles: @@ -365,14 +380,14 @@ def set_preferred_lane(self, preferred_lane: int = None) -> 'AbstractEnv': v.LANE_CHANGE_MAX_BRAKING_IMPOSED = 1000 return env_copy - def set_route_at_intersection(self, _to: str) -> 'AbstractEnv': + def set_route_at_intersection(self, _to: str) -> "AbstractEnv": env_copy = copy.deepcopy(self) for v in env_copy.road.vehicles: if isinstance(v, IDMVehicle): v.set_route_at_intersection(_to) return env_copy - def set_vehicle_field(self, args: Tuple[str, object]) -> 'AbstractEnv': + def set_vehicle_field(self, args: Tuple[str, object]) -> "AbstractEnv": field, value = args env_copy = copy.deepcopy(self) for v in env_copy.road.vehicles: @@ -380,7 +395,7 @@ def set_vehicle_field(self, args: Tuple[str, object]) -> 'AbstractEnv': setattr(v, field, value) return env_copy - def call_vehicle_method(self, args: Tuple[str, Tuple[object]]) -> 'AbstractEnv': + def call_vehicle_method(self, args: Tuple[str, Tuple[object]]) -> "AbstractEnv": method, method_args = args env_copy = copy.deepcopy(self) for i, v in enumerate(env_copy.road.vehicles): @@ -388,7 +403,7 @@ def call_vehicle_method(self, args: Tuple[str, Tuple[object]]) -> 'AbstractEnv': env_copy.road.vehicles[i] = getattr(v, method)(*method_args) return env_copy - def randomize_behavior(self) -> 'AbstractEnv': + def randomize_behavior(self) -> "AbstractEnv": env_copy = copy.deepcopy(self) for v in env_copy.road.vehicles: if isinstance(v, IDMVehicle): @@ -396,7 +411,7 @@ def randomize_behavior(self) -> 'AbstractEnv': return env_copy def to_finite_mdp(self): - return finite_mdp(self, time_quantization=1/self.config["policy_frequency"]) + return finite_mdp(self, time_quantization=1 / self.config["policy_frequency"]) def __deepcopy__(self, memo): """Perform a deep copy but without copying the environment viewer.""" @@ -404,7 +419,7 @@ def __deepcopy__(self, memo): result = cls.__new__(cls) memo[id(self)] = result for k, v in self.__dict__.items(): - if k not in ['viewer', '_record_video_wrapper']: + if k not in ["viewer", "_record_video_wrapper"]: setattr(result, k, copy.deepcopy(v, memo)) else: setattr(result, k, None) diff --git a/highway_env/envs/common/action.py b/highway_env/envs/common/action.py index 460789f75..e52d445bb 100644 --- a/highway_env/envs/common/action.py +++ b/highway_env/envs/common/action.py @@ -1,15 +1,15 @@ import functools import itertools -from typing import TYPE_CHECKING, Optional, Union, Tuple, Callable, List -from gymnasium import spaces +from typing import TYPE_CHECKING, Callable, List, Optional, Tuple, Union + import numpy as np +from gymnasium import spaces from highway_env import utils from highway_env.utils import Vector -from highway_env.vehicle.behavior import IDMVehicle +from highway_env.vehicle.controller import MDPVehicle from highway_env.vehicle.dynamics import BicycleVehicle from highway_env.vehicle.kinematics import Vehicle -from highway_env.vehicle.controller import MDPVehicle if TYPE_CHECKING: from highway_env.envs.common.abstract import AbstractEnv @@ -21,7 +21,7 @@ class ActionType(object): """A type of action specifies its definition space, and how actions are executed in the environment""" - def __init__(self, env: 'AbstractEnv', **kwargs) -> None: + def __init__(self, env: "AbstractEnv", **kwargs) -> None: self.env = env self.__controlled_vehicle = None @@ -84,16 +84,18 @@ class ContinuousAction(ActionType): STEERING_RANGE = (-np.pi / 4, np.pi / 4) """Steering angle range: [-x, x], in rad.""" - def __init__(self, - env: 'AbstractEnv', - acceleration_range: Optional[Tuple[float, float]] = None, - steering_range: Optional[Tuple[float, float]] = None, - speed_range: Optional[Tuple[float, float]] = None, - longitudinal: bool = True, - lateral: bool = True, - dynamical: bool = False, - clip: bool = True, - **kwargs) -> None: + def __init__( + self, + env: "AbstractEnv", + acceleration_range: Optional[Tuple[float, float]] = None, + steering_range: Optional[Tuple[float, float]] = None, + speed_range: Optional[Tuple[float, float]] = None, + longitudinal: bool = True, + lateral: bool = True, + dynamical: bool = False, + clip: bool = True, + **kwargs + ) -> None: """ Create a continuous action space. @@ -107,20 +109,24 @@ def __init__(self, :param clip: clip action to the defined range """ super().__init__(env) - self.acceleration_range = acceleration_range if acceleration_range else self.ACCELERATION_RANGE + self.acceleration_range = ( + acceleration_range if acceleration_range else self.ACCELERATION_RANGE + ) self.steering_range = steering_range if steering_range else self.STEERING_RANGE self.speed_range = speed_range self.lateral = lateral self.longitudinal = longitudinal if not self.lateral and not self.longitudinal: - raise ValueError("Either longitudinal and/or lateral control must be enabled") + raise ValueError( + "Either longitudinal and/or lateral control must be enabled" + ) self.dynamical = dynamical self.clip = clip self.size = 2 if self.lateral and self.longitudinal else 1 self.last_action = np.zeros(self.size) def space(self) -> spaces.Box: - return spaces.Box(-1., 1., shape=(self.size,), dtype=np.float32) + return spaces.Box(-1.0, 1.0, shape=(self.size,), dtype=np.float32) @property def vehicle_class(self) -> Callable: @@ -130,7 +136,10 @@ def get_action(self, action: np.ndarray): if self.clip: action = np.clip(action, -1, 1) if self.speed_range: - self.controlled_vehicle.MIN_SPEED, self.controlled_vehicle.MAX_SPEED = self.speed_range + ( + self.controlled_vehicle.MIN_SPEED, + self.controlled_vehicle.MAX_SPEED, + ) = self.speed_range if self.longitudinal and self.lateral: return { "acceleration": utils.lmap(action[0], [-1, 1], self.acceleration_range), @@ -144,7 +153,7 @@ def get_action(self, action: np.ndarray): elif self.lateral: return { "acceleration": 0, - "steering": utils.lmap(action[0], [-1, 1], self.steering_range) + "steering": utils.lmap(action[0], [-1, 1], self.steering_range), } def act(self, action: np.ndarray) -> None: @@ -153,18 +162,27 @@ def act(self, action: np.ndarray) -> None: class DiscreteAction(ContinuousAction): - def __init__(self, - env: 'AbstractEnv', - acceleration_range: Optional[Tuple[float, float]] = None, - steering_range: Optional[Tuple[float, float]] = None, - longitudinal: bool = True, - lateral: bool = True, - dynamical: bool = False, - clip: bool = True, - actions_per_axis: int = 3, - **kwargs) -> None: - super().__init__(env, acceleration_range=acceleration_range, steering_range=steering_range, - longitudinal=longitudinal, lateral=lateral, dynamical=dynamical, clip=clip) + def __init__( + self, + env: "AbstractEnv", + acceleration_range: Optional[Tuple[float, float]] = None, + steering_range: Optional[Tuple[float, float]] = None, + longitudinal: bool = True, + lateral: bool = True, + dynamical: bool = False, + clip: bool = True, + actions_per_axis: int = 3, + **kwargs + ) -> None: + super().__init__( + env, + acceleration_range=acceleration_range, + steering_range=steering_range, + longitudinal=longitudinal, + lateral=lateral, + dynamical=dynamical, + clip=clip, + ) self.actions_per_axis = actions_per_axis def space(self) -> spaces.Discrete: @@ -183,35 +201,23 @@ class DiscreteMetaAction(ActionType): An discrete action space of meta-actions: lane changes, and cruise control set-point. """ - ACTIONS_ALL = { - 0: 'LANE_LEFT', - 1: 'IDLE', - 2: 'LANE_RIGHT', - 3: 'FASTER', - 4: 'SLOWER' - } + ACTIONS_ALL = {0: "LANE_LEFT", 1: "IDLE", 2: "LANE_RIGHT", 3: "FASTER", 4: "SLOWER"} """A mapping of action indexes to labels.""" - ACTIONS_LONGI = { - 0: 'SLOWER', - 1: 'IDLE', - 2: 'FASTER' - } + ACTIONS_LONGI = {0: "SLOWER", 1: "IDLE", 2: "FASTER"} """A mapping of longitudinal action indexes to labels.""" - ACTIONS_LAT = { - 0: 'LANE_LEFT', - 1: 'IDLE', - 2: 'LANE_RIGHT' - } + ACTIONS_LAT = {0: "LANE_LEFT", 1: "IDLE", 2: "LANE_RIGHT"} """A mapping of lateral action indexes to labels.""" - def __init__(self, - env: 'AbstractEnv', - longitudinal: bool = True, - lateral: bool = True, - target_speeds: Optional[Vector] = None, - **kwargs) -> None: + def __init__( + self, + env: "AbstractEnv", + longitudinal: bool = True, + lateral: bool = True, + target_speeds: Optional[Vector] = None, + **kwargs + ) -> None: """ Create a discrete action space of meta-actions. @@ -223,13 +229,24 @@ def __init__(self, super().__init__(env) self.longitudinal = longitudinal self.lateral = lateral - self.target_speeds = np.array(target_speeds) if target_speeds is not None else MDPVehicle.DEFAULT_TARGET_SPEEDS - self.actions = self.ACTIONS_ALL if longitudinal and lateral \ - else self.ACTIONS_LONGI if longitudinal \ - else self.ACTIONS_LAT if lateral \ + self.target_speeds = ( + np.array(target_speeds) + if target_speeds is not None + else MDPVehicle.DEFAULT_TARGET_SPEEDS + ) + self.actions = ( + self.ACTIONS_ALL + if longitudinal and lateral + else self.ACTIONS_LONGI + if longitudinal + else self.ACTIONS_LAT + if lateral else None + ) if self.actions is None: - raise ValueError("At least longitudinal or lateral actions must be included") + raise ValueError( + "At least longitudinal or lateral actions must be included" + ) self.actions_indexes = {v: k for k, v in self.actions.items()} def space(self) -> spaces.Space: @@ -251,29 +268,38 @@ def get_available_actions(self) -> List[int]: :return: the list of available actions """ - actions = [self.actions_indexes['IDLE']] + actions = [self.actions_indexes["IDLE"]] network = self.controlled_vehicle.road.network for l_index in network.side_lanes(self.controlled_vehicle.lane_index): - if l_index[2] < self.controlled_vehicle.lane_index[2] \ - and network.get_lane(l_index).is_reachable_from(self.controlled_vehicle.position) \ - and self.lateral: - actions.append(self.actions_indexes['LANE_LEFT']) - if l_index[2] > self.controlled_vehicle.lane_index[2] \ - and network.get_lane(l_index).is_reachable_from(self.controlled_vehicle.position) \ - and self.lateral: - actions.append(self.actions_indexes['LANE_RIGHT']) - if self.controlled_vehicle.speed_index < self.controlled_vehicle.target_speeds.size - 1 and self.longitudinal: - actions.append(self.actions_indexes['FASTER']) + if ( + l_index[2] < self.controlled_vehicle.lane_index[2] + and network.get_lane(l_index).is_reachable_from( + self.controlled_vehicle.position + ) + and self.lateral + ): + actions.append(self.actions_indexes["LANE_LEFT"]) + if ( + l_index[2] > self.controlled_vehicle.lane_index[2] + and network.get_lane(l_index).is_reachable_from( + self.controlled_vehicle.position + ) + and self.lateral + ): + actions.append(self.actions_indexes["LANE_RIGHT"]) + if ( + self.controlled_vehicle.speed_index + < self.controlled_vehicle.target_speeds.size - 1 + and self.longitudinal + ): + actions.append(self.actions_indexes["FASTER"]) if self.controlled_vehicle.speed_index > 0 and self.longitudinal: - actions.append(self.actions_indexes['SLOWER']) + actions.append(self.actions_indexes["SLOWER"]) return actions class MultiAgentAction(ActionType): - def __init__(self, - env: 'AbstractEnv', - action_config: dict, - **kwargs) -> None: + def __init__(self, env: "AbstractEnv", action_config: dict, **kwargs) -> None: super().__init__(env) self.action_config = action_config self.agents_action_types = [] @@ -283,7 +309,9 @@ def __init__(self, self.agents_action_types.append(action_type) def space(self) -> spaces.Space: - return spaces.Tuple([action_type.space() for action_type in self.agents_action_types]) + return spaces.Tuple( + [action_type.space() for action_type in self.agents_action_types] + ) @property def vehicle_class(self) -> Callable: @@ -295,10 +323,15 @@ def act(self, action: Action) -> None: action_type.act(agent_action) def get_available_actions(self): - return itertools.product(*[action_type.get_available_actions() for action_type in self.agents_action_types]) + return itertools.product( + *[ + action_type.get_available_actions() + for action_type in self.agents_action_types + ] + ) -def action_factory(env: 'AbstractEnv', config: dict) -> ActionType: +def action_factory(env: "AbstractEnv", config: dict) -> ActionType: if config["type"] == "ContinuousAction": return ContinuousAction(env, **config) if config["type"] == "DiscreteAction": diff --git a/highway_env/envs/common/finite_mdp.py b/highway_env/envs/common/finite_mdp.py index 744d99273..96ebda00d 100644 --- a/highway_env/envs/common/finite_mdp.py +++ b/highway_env/envs/common/finite_mdp.py @@ -11,9 +11,9 @@ from highway_env.envs import AbstractEnv -def finite_mdp(env: 'AbstractEnv', - time_quantization: float = 1., - horizon: float = 10.) -> object: +def finite_mdp( + env: "AbstractEnv", time_quantization: float = 1.0, horizon: float = 10.0 +) -> object: """ Time-To-Collision (TTC) representation of the state. @@ -47,27 +47,43 @@ def finite_mdp(env: 'AbstractEnv', # Compute transition function transition_model_with_grid = partial(transition_model, grid=grid) - transition = np.fromfunction(transition_model_with_grid, grid.shape + (env.action_space.n,), dtype=int) + transition = np.fromfunction( + transition_model_with_grid, grid.shape + (env.action_space.n,), dtype=int + ) transition = np.reshape(transition, (np.size(grid), env.action_space.n)) # Compute reward function v, l, t = grid.shape - lanes = np.arange(l)/max(l - 1, 1) - speeds = np.arange(v)/max(v - 1, 1) - - state_reward = \ - + env.config["collision_reward"] * grid \ - + env.config["right_lane_reward"] * np.tile(lanes[np.newaxis, :, np.newaxis], (v, 1, t)) \ - + env.config["high_speed_reward"] * np.tile(speeds[:, np.newaxis, np.newaxis], (1, l, t)) - + lanes = np.arange(l) / max(l - 1, 1) + speeds = np.arange(v) / max(v - 1, 1) + + state_reward = ( + +env.config["collision_reward"] * grid + + env.config["right_lane_reward"] + * np.tile(lanes[np.newaxis, :, np.newaxis], (v, 1, t)) + + env.config["high_speed_reward"] + * np.tile(speeds[:, np.newaxis, np.newaxis], (1, l, t)) + ) + state_reward = np.ravel(state_reward) - action_reward = [env.config["lane_change_reward"], 0, env.config["lane_change_reward"], 0, 0] - reward = np.fromfunction(np.vectorize(lambda s, a: state_reward[s] + action_reward[a]), - (np.size(state_reward), np.size(action_reward)), dtype=int) + action_reward = [ + env.config["lane_change_reward"], + 0, + env.config["lane_change_reward"], + 0, + 0, + ] + reward = np.fromfunction( + np.vectorize(lambda s, a: state_reward[s] + action_reward[a]), + (np.size(state_reward), np.size(action_reward)), + dtype=int, + ) # Compute terminal states collision = grid == 1 - end_of_horizon = np.fromfunction(lambda h, i, j: j == grid.shape[2] - 1, grid.shape, dtype=int) + end_of_horizon = np.fromfunction( + lambda h, i, j: j == grid.shape[2] - 1, grid.shape, dtype=int + ) terminal = np.ravel(collision | end_of_horizon) # Creation of a new finite MDP @@ -77,13 +93,17 @@ def finite_mdp(env: 'AbstractEnv', mdp.original_shape = grid.shape return mdp except ModuleNotFoundError as e: - raise ModuleNotFoundError("The finite_mdp module is required for conversion. {}".format(e)) + raise ModuleNotFoundError( + "The finite_mdp module is required for conversion. {}".format(e) + ) -def compute_ttc_grid(env: 'AbstractEnv', - time_quantization: float, - horizon: float, - vehicle: Optional[Vehicle] = None) -> np.ndarray: +def compute_ttc_grid( + env: "AbstractEnv", + time_quantization: float, + horizon: float, + vehicle: Optional[Vehicle] = None, +) -> np.ndarray: """ Compute the grid of predicted time-to-collision to each vehicle within the lane @@ -96,7 +116,9 @@ def compute_ttc_grid(env: 'AbstractEnv', """ vehicle = vehicle or env.vehicle road_lanes = env.road.network.all_side_lanes(env.vehicle.lane_index) - grid = np.zeros((vehicle.target_speeds.size, len(road_lanes), int(horizon / time_quantization))) + grid = np.zeros( + (vehicle.target_speeds.size, len(road_lanes), int(horizon / time_quantization)) + ) for speed_index in range(grid.shape[0]): ego_speed = vehicle.index_to_speed(speed_index) for other in env.road.vehicles: @@ -106,24 +128,35 @@ def compute_ttc_grid(env: 'AbstractEnv', collision_points = [(0, 1), (-margin, 0.5), (margin, 0.5)] for m, cost in collision_points: distance = vehicle.lane_distance_to(other) + m - other_projected_speed = other.speed * np.dot(other.direction, vehicle.direction) - time_to_collision = distance / utils.not_zero(ego_speed - other_projected_speed) + other_projected_speed = other.speed * np.dot( + other.direction, vehicle.direction + ) + time_to_collision = distance / utils.not_zero( + ego_speed - other_projected_speed + ) if time_to_collision < 0: continue - if env.road.network.is_connected_road(vehicle.lane_index, other.lane_index, - route=vehicle.route, depth=3): + if env.road.network.is_connected_road( + vehicle.lane_index, other.lane_index, route=vehicle.route, depth=3 + ): # Same road, or connected road with same number of lanes - if len(env.road.network.all_side_lanes(other.lane_index)) == len(env.road.network.all_side_lanes(vehicle.lane_index)): + if len(env.road.network.all_side_lanes(other.lane_index)) == len( + env.road.network.all_side_lanes(vehicle.lane_index) + ): lane = [other.lane_index[2]] # Different road of different number of lanes: uncertainty on future lane, use all else: lane = range(grid.shape[1]) # Quantize time-to-collision to both upper and lower values - for time in [int(time_to_collision / time_quantization), - int(np.ceil(time_to_collision / time_quantization))]: + for time in [ + int(time_to_collision / time_quantization), + int(np.ceil(time_to_collision / time_quantization)), + ]: if 0 <= time < grid.shape[2]: # TODO: check lane overflow (e.g. vehicle with higher lane id than current road capacity) - grid[speed_index, lane, time] = np.maximum(grid[speed_index, lane, time], cost) + grid[speed_index, lane, time] = np.maximum( + grid[speed_index, lane, time], cost + ) return grid diff --git a/highway_env/envs/common/graphics.py b/highway_env/envs/common/graphics.py index 2408f608c..bcda69599 100644 --- a/highway_env/envs/common/graphics.py +++ b/highway_env/envs/common/graphics.py @@ -1,10 +1,15 @@ import os from typing import TYPE_CHECKING, Callable, List, Optional + import numpy as np import pygame -from highway_env.envs.common.action import ActionType, DiscreteMetaAction, ContinuousAction -from highway_env.road.graphics import WorldSurface, RoadGraphics +from highway_env.envs.common.action import ( + ActionType, + ContinuousAction, + DiscreteMetaAction, +) +from highway_env.road.graphics import RoadGraphics, WorldSurface from highway_env.vehicle.graphics import VehicleGraphics if TYPE_CHECKING: @@ -19,7 +24,7 @@ class EnvViewer(object): SAVE_IMAGES = False agent_display = None - def __init__(self, env: 'AbstractEnv', config: Optional[dict] = None) -> None: + def __init__(self, env: "AbstractEnv", config: Optional[dict] = None) -> None: self.env = env self.config = config or env.config self.offscreen = self.config["offscreen_rendering"] @@ -37,12 +42,18 @@ def __init__(self, env: 'AbstractEnv', config: Optional[dict] = None) -> None: # instruction allows the drawing to be done on surfaces without # handling a screen display, useful for e.g. cloud computing if not self.offscreen: - self.screen = pygame.display.set_mode([self.config["screen_width"], self.config["screen_height"]]) + self.screen = pygame.display.set_mode( + [self.config["screen_width"], self.config["screen_height"]] + ) if self.agent_display: self.extend_display() self.sim_surface = WorldSurface(panel_size, 0, pygame.Surface(panel_size)) - self.sim_surface.scaling = self.config.get("scaling", self.sim_surface.INITIAL_SCALING) - self.sim_surface.centering_position = self.config.get("centering_position", self.sim_surface.INITIAL_CENTERING) + self.sim_surface.scaling = self.config.get( + "scaling", self.sim_surface.INITIAL_SCALING + ) + self.sim_surface.centering_position = self.config.get( + "centering_position", self.sim_surface.INITIAL_CENTERING + ) self.clock = pygame.time.Clock() self.enabled = True @@ -64,14 +75,18 @@ def set_agent_display(self, agent_display: Callable) -> None: def extend_display(self) -> None: if not self.offscreen: if self.config["screen_width"] > self.config["screen_height"]: - self.screen = pygame.display.set_mode((self.config["screen_width"], - 2 * self.config["screen_height"])) + self.screen = pygame.display.set_mode( + (self.config["screen_width"], 2 * self.config["screen_height"]) + ) else: - self.screen = pygame.display.set_mode((2 * self.config["screen_width"], - self.config["screen_height"])) - self.agent_surface = pygame.Surface((self.config["screen_width"], self.config["screen_height"])) + self.screen = pygame.display.set_mode( + (2 * self.config["screen_width"], self.config["screen_height"]) + ) + self.agent_surface = pygame.Surface( + (self.config["screen_width"], self.config["screen_height"]) + ) - def set_agent_action_sequence(self, actions: List['Action']) -> None: + def set_agent_action_sequence(self, actions: List["Action"]) -> None: """ Set the sequence of actions chosen by the agent, so that it can be displayed @@ -82,10 +97,12 @@ def set_agent_action_sequence(self, actions: List['Action']) -> None: elif isinstance(self.env.action_type, ContinuousAction): actions = [self.env.action_type.get_action(a) for a in actions] if len(actions) > 1: - self.vehicle_trajectory = self.env.vehicle.predict_trajectory(actions, - 1 / self.env.config["policy_frequency"], - 1 / 3 / self.env.config["policy_frequency"], - 1 / self.env.config["simulation_frequency"]) + self.vehicle_trajectory = self.env.vehicle.predict_trajectory( + actions, + 1 / self.env.config["policy_frequency"], + 1 / 3 / self.env.config["policy_frequency"], + 1 / self.env.config["simulation_frequency"], + ) def handle_events(self) -> None: """Handle pygame events by forwarding them to the display and environment vehicle.""" @@ -106,29 +123,31 @@ def display(self) -> None: if self.vehicle_trajectory: VehicleGraphics.display_trajectory( - self.vehicle_trajectory, - self.sim_surface, - offscreen=self.offscreen) + self.vehicle_trajectory, self.sim_surface, offscreen=self.offscreen + ) RoadGraphics.display_road_objects( - self.env.road, - self.sim_surface, - offscreen=self.offscreen + self.env.road, self.sim_surface, offscreen=self.offscreen ) if EnvViewer.agent_display: EnvViewer.agent_display(self.agent_surface, self.sim_surface) if not self.offscreen: if self.config["screen_width"] > self.config["screen_height"]: - self.screen.blit(self.agent_surface, (0, self.config["screen_height"])) + self.screen.blit( + self.agent_surface, (0, self.config["screen_height"]) + ) else: - self.screen.blit(self.agent_surface, (self.config["screen_width"], 0)) + self.screen.blit( + self.agent_surface, (self.config["screen_width"], 0) + ) RoadGraphics.display_traffic( self.env.road, self.sim_surface, simulation_frequency=self.env.config["simulation_frequency"], - offscreen=self.offscreen) + offscreen=self.offscreen, + ) ObservationGraphics.display(self.env.observation_type, self.sim_surface) @@ -139,7 +158,10 @@ def display(self) -> None: pygame.display.flip() if self.SAVE_IMAGES and self.directory: - pygame.image.save(self.sim_surface, str(self.directory / "highway-env_{}.png".format(self.frame))) + pygame.image.save( + self.sim_surface, + str(self.directory / "highway-env_{}.png".format(self.frame)), + ) self.frame += 1 def get_image(self) -> np.ndarray: @@ -148,7 +170,11 @@ def get_image(self) -> np.ndarray: Gymnasium's channel convention is H x W x C """ - surface = self.screen if self.config["render_agent"] and not self.offscreen else self.sim_surface + surface = ( + self.screen + if self.config["render_agent"] and not self.offscreen + else self.sim_surface + ) data = pygame.surfarray.array3d(surface) # in W x H x C channel convention return np.moveaxis(data, 0, 1) @@ -168,7 +194,9 @@ def close(self) -> None: class EventHandler(object): @classmethod - def handle_event(cls, action_type: ActionType, event: pygame.event.EventType) -> None: + def handle_event( + cls, action_type: ActionType, event: pygame.event.EventType + ) -> None: """ Map the pygame keyboard events to control decisions @@ -181,7 +209,9 @@ def handle_event(cls, action_type: ActionType, event: pygame.event.EventType) -> cls.handle_continuous_action_event(action_type, event) @classmethod - def handle_discrete_action_event(cls, action_type: DiscreteMetaAction, event: pygame.event.EventType) -> None: + def handle_discrete_action_event( + cls, action_type: DiscreteMetaAction, event: pygame.event.EventType + ) -> None: if event.type == pygame.KEYDOWN: if event.key == pygame.K_RIGHT and action_type.longitudinal: action_type.act(action_type.actions_indexes["FASTER"]) @@ -193,7 +223,9 @@ def handle_discrete_action_event(cls, action_type: DiscreteMetaAction, event: py action_type.act(action_type.actions_indexes["LANE_LEFT"]) @classmethod - def handle_continuous_action_event(cls, action_type: ContinuousAction, event: pygame.event.EventType) -> None: + def handle_continuous_action_event( + cls, action_type: ContinuousAction, event: pygame.event.EventType + ) -> None: action = action_type.last_action.copy() steering_index = action_type.space().shape[0] - 1 if event.type == pygame.KEYDOWN: @@ -223,17 +255,31 @@ class ObservationGraphics(object): @classmethod def display(cls, obs, sim_surface): from highway_env.envs.common.observation import LidarObservation + if isinstance(obs, LidarObservation): cls.display_grid(obs, sim_surface) @classmethod def display_grid(cls, lidar_observation, surface): - psi = np.repeat(np.arange(-lidar_observation.angle/2, - 2 * np.pi - lidar_observation.angle/2, - 2 * np.pi / lidar_observation.grid.shape[0]), 2) + psi = np.repeat( + np.arange( + -lidar_observation.angle / 2, + 2 * np.pi - lidar_observation.angle / 2, + 2 * np.pi / lidar_observation.grid.shape[0], + ), + 2, + ) psi = np.hstack((psi[1:], [psi[0]])) - r = np.repeat(np.minimum(lidar_observation.grid[:, 0], lidar_observation.maximum_range), 2) - points = [(surface.pos2pix(lidar_observation.origin[0] + r[i] * np.cos(psi[i]), - lidar_observation.origin[1] + r[i] * np.sin(psi[i]))) - for i in range(np.size(psi))] + r = np.repeat( + np.minimum(lidar_observation.grid[:, 0], lidar_observation.maximum_range), 2 + ) + points = [ + ( + surface.pos2pix( + lidar_observation.origin[0] + r[i] * np.cos(psi[i]), + lidar_observation.origin[1] + r[i] * np.sin(psi[i]), + ) + ) + for i in range(np.size(psi)) + ] pygame.draw.lines(surface, ObservationGraphics.COLOR, True, points, 1) diff --git a/highway_env/envs/common/observation.py b/highway_env/envs/common/observation.py index 367654233..cb3c1a13e 100644 --- a/highway_env/envs/common/observation.py +++ b/highway_env/envs/common/observation.py @@ -1,16 +1,16 @@ from collections import OrderedDict from itertools import product -from typing import List, Dict, TYPE_CHECKING, Optional, Union, Tuple -from gymnasium import spaces +from typing import TYPE_CHECKING, Dict, List, Optional, Tuple + import numpy as np import pandas as pd +from gymnasium import spaces from highway_env import utils from highway_env.envs.common.finite_mdp import compute_ttc_grid from highway_env.envs.common.graphics import EnvViewer from highway_env.road.lane import AbstractLane -from highway_env.utils import distance_to_circle, Vector -from highway_env.vehicle.controller import MDPVehicle +from highway_env.utils import Vector from highway_env.vehicle.kinematics import Vehicle if TYPE_CHECKING: @@ -18,7 +18,7 @@ class ObservationType(object): - def __init__(self, env: 'AbstractEnv', **kwargs) -> None: + def __init__(self, env: "AbstractEnv", **kwargs) -> None: self.env = env self.__observer_vehicle = None @@ -62,28 +62,34 @@ class GrayscaleObservation(ObservationType): } """ - def __init__(self, env: 'AbstractEnv', - observation_shape: Tuple[int, int], - stack_size: int, - weights: List[float], - scaling: Optional[float] = None, - centering_position: Optional[List[float]] = None, - **kwargs) -> None: + def __init__( + self, + env: "AbstractEnv", + observation_shape: Tuple[int, int], + stack_size: int, + weights: List[float], + scaling: Optional[float] = None, + centering_position: Optional[List[float]] = None, + **kwargs + ) -> None: super().__init__(env) self.observation_shape = observation_shape - self.shape = (stack_size, ) + self.observation_shape + self.shape = (stack_size,) + self.observation_shape self.weights = weights self.obs = np.zeros(self.shape, dtype=np.uint8) # The viewer configuration can be different between this observation and env.render() (typically smaller) viewer_config = env.config.copy() - viewer_config.update({ - "offscreen_rendering": True, - "screen_width": self.observation_shape[0], - "screen_height": self.observation_shape[1], - "scaling": scaling or viewer_config["scaling"], - "centering_position": centering_position or viewer_config["centering_position"] - }) + viewer_config.update( + { + "offscreen_rendering": True, + "screen_width": self.observation_shape[0], + "screen_height": self.observation_shape[1], + "scaling": scaling or viewer_config["scaling"], + "centering_position": centering_position + or viewer_config["centering_position"], + } + ) self.viewer = EnvViewer(env, config=viewer_config) def space(self) -> spaces.Space: @@ -104,34 +110,42 @@ def _render_to_grayscale(self) -> np.ndarray: class TimeToCollisionObservation(ObservationType): - def __init__(self, env: 'AbstractEnv', horizon: int = 10, **kwargs: dict) -> None: + def __init__(self, env: "AbstractEnv", horizon: int = 10, **kwargs: dict) -> None: super().__init__(env) self.horizon = horizon def space(self) -> spaces.Space: try: - return spaces.Box(shape=self.observe().shape, low=0, high=1, dtype=np.float32) + return spaces.Box( + shape=self.observe().shape, low=0, high=1, dtype=np.float32 + ) except AttributeError: return spaces.Space() def observe(self) -> np.ndarray: if not self.env.road: - return np.zeros((3, 3, int(self.horizon * self.env.config["policy_frequency"]))) - grid = compute_ttc_grid(self.env, vehicle=self.observer_vehicle, - time_quantization=1/self.env.config["policy_frequency"], horizon=self.horizon) + return np.zeros( + (3, 3, int(self.horizon * self.env.config["policy_frequency"])) + ) + grid = compute_ttc_grid( + self.env, + vehicle=self.observer_vehicle, + time_quantization=1 / self.env.config["policy_frequency"], + horizon=self.horizon, + ) padding = np.ones(np.shape(grid)) padded_grid = np.concatenate([padding, grid, padding], axis=1) obs_lanes = 3 l0 = grid.shape[1] + self.observer_vehicle.lane_index[2] - obs_lanes // 2 lf = grid.shape[1] + self.observer_vehicle.lane_index[2] + obs_lanes // 2 - clamped_grid = padded_grid[:, l0:lf+1, :] + clamped_grid = padded_grid[:, l0 : lf + 1, :] repeats = np.ones(clamped_grid.shape[0]) repeats[np.array([0, -1])] += clamped_grid.shape[0] padded_grid = np.repeat(clamped_grid, repeats.astype(int), axis=0) obs_speeds = 3 v0 = grid.shape[0] + self.observer_vehicle.speed_index - obs_speeds // 2 vf = grid.shape[0] + self.observer_vehicle.speed_index + obs_speeds // 2 - clamped_grid = padded_grid[v0:vf + 1, :, :] + clamped_grid = padded_grid[v0 : vf + 1, :, :] return clamped_grid.astype(np.float32) @@ -139,20 +153,23 @@ class KinematicObservation(ObservationType): """Observe the kinematics of nearby vehicles.""" - FEATURES: List[str] = ['presence', 'x', 'y', 'vx', 'vy'] - - def __init__(self, env: 'AbstractEnv', - features: List[str] = None, - vehicles_count: int = 5, - features_range: Dict[str, List[float]] = None, - absolute: bool = False, - order: str = "sorted", - normalize: bool = True, - clip: bool = True, - see_behind: bool = False, - observe_intentions: bool = False, - include_obstacles: bool = True, - **kwargs: dict) -> None: + FEATURES: List[str] = ["presence", "x", "y", "vx", "vy"] + + def __init__( + self, + env: "AbstractEnv", + features: List[str] = None, + vehicles_count: int = 5, + features_range: Dict[str, List[float]] = None, + absolute: bool = False, + order: str = "sorted", + normalize: bool = True, + clip: bool = True, + see_behind: bool = False, + observe_intentions: bool = False, + include_obstacles: bool = True, + **kwargs: dict + ) -> None: """ :param env: The environment to observe :param features: Names of features used in the observation @@ -178,7 +195,12 @@ def __init__(self, env: 'AbstractEnv', self.include_obstacles = include_obstacles def space(self) -> spaces.Space: - return spaces.Box(shape=(self.vehicles_count, len(self.features)), low=-np.inf, high=np.inf, dtype=np.float32) + return spaces.Box( + shape=(self.vehicles_count, len(self.features)), + low=-np.inf, + high=np.inf, + dtype=np.float32, + ) def normalize_obs(self, df: pd.DataFrame) -> pd.DataFrame: """ @@ -188,12 +210,17 @@ def normalize_obs(self, df: pd.DataFrame) -> pd.DataFrame: :param Dataframe df: observation data """ if not self.features_range: - side_lanes = self.env.road.network.all_side_lanes(self.observer_vehicle.lane_index) + side_lanes = self.env.road.network.all_side_lanes( + self.observer_vehicle.lane_index + ) self.features_range = { "x": [-5.0 * Vehicle.MAX_SPEED, 5.0 * Vehicle.MAX_SPEED], - "y": [-AbstractLane.DEFAULT_WIDTH * len(side_lanes), AbstractLane.DEFAULT_WIDTH * len(side_lanes)], - "vx": [-2*Vehicle.MAX_SPEED, 2*Vehicle.MAX_SPEED], - "vy": [-2*Vehicle.MAX_SPEED, 2*Vehicle.MAX_SPEED] + "y": [ + -AbstractLane.DEFAULT_WIDTH * len(side_lanes), + AbstractLane.DEFAULT_WIDTH * len(side_lanes), + ], + "vx": [-2 * Vehicle.MAX_SPEED, 2 * Vehicle.MAX_SPEED], + "vy": [-2 * Vehicle.MAX_SPEED, 2 * Vehicle.MAX_SPEED], } for feature, f_range in self.features_range.items(): if feature in df: @@ -209,17 +236,22 @@ def observe(self) -> np.ndarray: # Add ego-vehicle df = pd.DataFrame.from_records([self.observer_vehicle.to_dict()]) # Add nearby traffic - close_vehicles = self.env.road.close_objects_to(self.observer_vehicle, - self.env.PERCEPTION_DISTANCE, - count=self.vehicles_count - 1, - see_behind=self.see_behind, - sort=self.order == "sorted", - vehicles_only=not self.include_obstacles) + close_vehicles = self.env.road.close_objects_to( + self.observer_vehicle, + self.env.PERCEPTION_DISTANCE, + count=self.vehicles_count - 1, + see_behind=self.see_behind, + sort=self.order == "sorted", + vehicles_only=not self.include_obstacles, + ) if close_vehicles: origin = self.observer_vehicle if not self.absolute else None vehicles_df = pd.DataFrame.from_records( - [v.to_dict(origin, observe_intentions=self.observe_intentions) - for v in close_vehicles[-self.vehicles_count + 1:]]) + [ + v.to_dict(origin, observe_intentions=self.observe_intentions) + for v in close_vehicles[-self.vehicles_count + 1 :] + ] + ) df = pd.concat([df, vehicles_df], ignore_index=True) df = df[self.features] @@ -230,7 +262,9 @@ def observe(self) -> np.ndarray: # Fill missing rows if df.shape[0] < self.vehicles_count: rows = np.zeros((self.vehicles_count - df.shape[0], len(self.features))) - df = pd.concat([df, pd.DataFrame(data=rows, columns=self.features)], ignore_index=True) + df = pd.concat( + [df, pd.DataFrame(data=rows, columns=self.features)], ignore_index=True + ) # Reorder df = df[self.features] obs = df.values.copy() @@ -244,21 +278,23 @@ class OccupancyGridObservation(ObservationType): """Observe an occupancy grid of nearby vehicles.""" - FEATURES: List[str] = ['presence', 'vx', 'vy', 'on_road'] - GRID_SIZE: List[List[float]] = [[-5.5*5, 5.5*5], [-5.5*5, 5.5*5]] + FEATURES: List[str] = ["presence", "vx", "vy", "on_road"] + GRID_SIZE: List[List[float]] = [[-5.5 * 5, 5.5 * 5], [-5.5 * 5, 5.5 * 5]] GRID_STEP: List[int] = [5, 5] - def __init__(self, - env: 'AbstractEnv', - features: Optional[List[str]] = None, - grid_size: Optional[Tuple[Tuple[float, float], Tuple[float, float]]] = None, - grid_step: Optional[Tuple[float, float]] = None, - features_range: Dict[str, List[float]] = None, - absolute: bool = False, - align_to_vehicle_axes: bool = False, - clip: bool = True, - as_image: bool = False, - **kwargs: dict) -> None: + def __init__( + self, + env: "AbstractEnv", + features: Optional[List[str]] = None, + grid_size: Optional[Tuple[Tuple[float, float], Tuple[float, float]]] = None, + grid_step: Optional[Tuple[float, float]] = None, + features_range: Dict[str, List[float]] = None, + absolute: bool = False, + align_to_vehicle_axes: bool = False, + clip: bool = True, + as_image: bool = False, + **kwargs: dict + ) -> None: """ :param env: The environment to observe :param features: Names of features used in the observation @@ -272,10 +308,16 @@ def __init__(self, """ super().__init__(env) self.features = features if features is not None else self.FEATURES - self.grid_size = np.array(grid_size) if grid_size is not None else np.array(self.GRID_SIZE) - self.grid_step = np.array(grid_step) if grid_step is not None else np.array(self.GRID_STEP) - grid_shape = np.asarray(np.floor((self.grid_size[:, 1] - self.grid_size[:, 0]) / self.grid_step), - dtype=np.uint8) + self.grid_size = ( + np.array(grid_size) if grid_size is not None else np.array(self.GRID_SIZE) + ) + self.grid_step = ( + np.array(grid_step) if grid_step is not None else np.array(self.GRID_STEP) + ) + grid_shape = np.asarray( + np.floor((self.grid_size[:, 1] - self.grid_size[:, 0]) / self.grid_step), + dtype=np.uint8, + ) self.grid = np.zeros((len(self.features), *grid_shape)) self.features_range = features_range self.absolute = absolute @@ -287,7 +329,9 @@ def space(self) -> spaces.Space: if self.as_image: return spaces.Box(shape=self.grid.shape, low=0, high=255, dtype=np.uint8) else: - return spaces.Box(shape=self.grid.shape, low=-np.inf, high=np.inf, dtype=np.float32) + return spaces.Box( + shape=self.grid.shape, low=-np.inf, high=np.inf, dtype=np.float32 + ) def normalize(self, df: pd.DataFrame) -> pd.DataFrame: """ @@ -298,8 +342,8 @@ def normalize(self, df: pd.DataFrame) -> pd.DataFrame: """ if not self.features_range: self.features_range = { - "vx": [-2*Vehicle.MAX_SPEED, 2*Vehicle.MAX_SPEED], - "vy": [-2*Vehicle.MAX_SPEED, 2*Vehicle.MAX_SPEED] + "vx": [-2 * Vehicle.MAX_SPEED, 2 * Vehicle.MAX_SPEED], + "vy": [-2 * Vehicle.MAX_SPEED, 2 * Vehicle.MAX_SPEED], } for feature, f_range in self.features_range.items(): if feature in df: @@ -318,7 +362,8 @@ def observe(self) -> np.ndarray: # Get nearby traffic data df = pd.DataFrame.from_records( - [v.to_dict(self.observer_vehicle) for v in self.env.road.vehicles]) + [v.to_dict(self.observer_vehicle) for v in self.env.road.vehicles] + ) # Normalize df = self.normalize(df) # Fill-in features @@ -328,11 +373,28 @@ def observe(self) -> np.ndarray: x, y = vehicle["x"], vehicle["y"] # Recover unnormalized coordinates for cell index if "x" in self.features_range: - x = utils.lmap(x, [-1, 1], [self.features_range["x"][0], self.features_range["x"][1]]) + x = utils.lmap( + x, + [-1, 1], + [ + self.features_range["x"][0], + self.features_range["x"][1], + ], + ) if "y" in self.features_range: - y = utils.lmap(y, [-1, 1], [self.features_range["y"][0], self.features_range["y"][1]]) + y = utils.lmap( + y, + [-1, 1], + [ + self.features_range["y"][0], + self.features_range["y"][1], + ], + ) cell = self.pos_to_index((x, y), relative=not self.absolute) - if 0 <= cell[1] < self.grid.shape[-2] and 0 <= cell[0] < self.grid.shape[-1]: + if ( + 0 <= cell[1] < self.grid.shape[-2] + and 0 <= cell[0] < self.grid.shape[-1] + ): self.grid[layer, cell[1], cell[0]] = vehicle[feature] elif feature == "on_road": self.fill_road_layer_by_lanes(layer) @@ -362,25 +424,33 @@ def pos_to_index(self, position: Vector, relative: bool = False) -> Tuple[int, i if not relative: position -= self.observer_vehicle.position if self.align_to_vehicle_axes: - c, s = np.cos(self.observer_vehicle.heading), np.sin(self.observer_vehicle.heading) + c, s = np.cos(self.observer_vehicle.heading), np.sin( + self.observer_vehicle.heading + ) position = np.array([[c, s], [-s, c]]) @ position - return int(np.floor((position[0] - self.grid_size[0, 0]) / self.grid_step[0])),\ - int(np.floor((position[1] - self.grid_size[1, 0]) / self.grid_step[1])) + return int( + np.floor((position[0] - self.grid_size[0, 0]) / self.grid_step[0]) + ), int(np.floor((position[1] - self.grid_size[1, 0]) / self.grid_step[1])) def index_to_pos(self, index: Tuple[int, int]) -> np.ndarray: - - position = np.array([ - (index[1] + 0.5) * self.grid_step[0] + self.grid_size[0, 0], - (index[0] + 0.5) * self.grid_step[1] + self.grid_size[1, 0] - ]) + position = np.array( + [ + (index[1] + 0.5) * self.grid_step[0] + self.grid_size[0, 0], + (index[0] + 0.5) * self.grid_step[1] + self.grid_size[1, 0], + ] + ) if self.align_to_vehicle_axes: - c, s = np.cos(-self.observer_vehicle.heading), np.sin(-self.observer_vehicle.heading) + c, s = np.cos(-self.observer_vehicle.heading), np.sin( + -self.observer_vehicle.heading + ) position = np.array([[c, s], [-s, c]]) @ position position += self.observer_vehicle.position return position - def fill_road_layer_by_lanes(self, layer_index: int, lane_perception_distance: float = 100) -> None: + def fill_road_layer_by_lanes( + self, layer_index: int, lane_perception_distance: float = 100 + ) -> None: """ A layer to encode the onroad (1) / offroad (0) information @@ -397,12 +467,17 @@ def fill_road_layer_by_lanes(self, layer_index: int, lane_perception_distance: f for _to in road.network.graph[_from].keys(): for lane in road.network.graph[_from][_to]: origin, _ = lane.local_coordinates(self.observer_vehicle.position) - waypoints = np.arange(origin - lane_perception_distance, - origin + lane_perception_distance, - lane_waypoints_spacing).clip(0, lane.length) + waypoints = np.arange( + origin - lane_perception_distance, + origin + lane_perception_distance, + lane_waypoints_spacing, + ).clip(0, lane.length) for waypoint in waypoints: cell = self.pos_to_index(lane.position(waypoint, 0)) - if 0 <= cell[1] < self.grid.shape[-2] and 0 <= cell[0] < self.grid.shape[-1]: + if ( + 0 <= cell[1] < self.grid.shape[-2] + and 0 <= cell[0] < self.grid.shape[-1] + ): self.grid[layer_index, cell[1], cell[0]] = 1 def fill_road_layer_by_cell(self, layer_index) -> None: @@ -422,65 +497,93 @@ def fill_road_layer_by_cell(self, layer_index) -> None: class KinematicsGoalObservation(KinematicObservation): - def __init__(self, env: 'AbstractEnv', scales: List[float], **kwargs: dict) -> None: + def __init__(self, env: "AbstractEnv", scales: List[float], **kwargs: dict) -> None: self.scales = np.array(scales) super().__init__(env, **kwargs) def space(self) -> spaces.Space: try: obs = self.observe() - return spaces.Dict(dict( - desired_goal=spaces.Box(-np.inf, np.inf, shape=obs["desired_goal"].shape, dtype=np.float64), - achieved_goal=spaces.Box(-np.inf, np.inf, shape=obs["achieved_goal"].shape, dtype=np.float64), - observation=spaces.Box(-np.inf, np.inf, shape=obs["observation"].shape, dtype=np.float64), - )) + return spaces.Dict( + dict( + desired_goal=spaces.Box( + -np.inf, + np.inf, + shape=obs["desired_goal"].shape, + dtype=np.float64, + ), + achieved_goal=spaces.Box( + -np.inf, + np.inf, + shape=obs["achieved_goal"].shape, + dtype=np.float64, + ), + observation=spaces.Box( + -np.inf, + np.inf, + shape=obs["observation"].shape, + dtype=np.float64, + ), + ) + ) except AttributeError: return spaces.Space() def observe(self) -> Dict[str, np.ndarray]: if not self.observer_vehicle: - return OrderedDict([ - ("observation", np.zeros((len(self.features),))), - ("achieved_goal", np.zeros((len(self.features),))), - ("desired_goal", np.zeros((len(self.features),))) - ]) - - obs = np.ravel(pd.DataFrame.from_records([self.observer_vehicle.to_dict()])[self.features]) - goal = np.ravel(pd.DataFrame.from_records([self.env.goal.to_dict()])[self.features]) - obs = OrderedDict([ - ("observation", obs / self.scales), - ("achieved_goal", obs / self.scales), - ("desired_goal", goal / self.scales) - ]) + return OrderedDict( + [ + ("observation", np.zeros((len(self.features),))), + ("achieved_goal", np.zeros((len(self.features),))), + ("desired_goal", np.zeros((len(self.features),))), + ] + ) + + obs = np.ravel( + pd.DataFrame.from_records([self.observer_vehicle.to_dict()])[self.features] + ) + goal = np.ravel( + pd.DataFrame.from_records([self.env.goal.to_dict()])[self.features] + ) + obs = OrderedDict( + [ + ("observation", obs / self.scales), + ("achieved_goal", obs / self.scales), + ("desired_goal", goal / self.scales), + ] + ) return obs class AttributesObservation(ObservationType): - def __init__(self, env: 'AbstractEnv', attributes: List[str], **kwargs: dict) -> None: + def __init__( + self, env: "AbstractEnv", attributes: List[str], **kwargs: dict + ) -> None: self.env = env self.attributes = attributes def space(self) -> spaces.Space: try: obs = self.observe() - return spaces.Dict({ - attribute: spaces.Box(-np.inf, np.inf, shape=obs[attribute].shape, dtype=np.float64) - for attribute in self.attributes - }) + return spaces.Dict( + { + attribute: spaces.Box( + -np.inf, np.inf, shape=obs[attribute].shape, dtype=np.float64 + ) + for attribute in self.attributes + } + ) except AttributeError: return spaces.Space() def observe(self) -> Dict[str, np.ndarray]: - return OrderedDict([ - (attribute, getattr(self.env, attribute)) for attribute in self.attributes - ]) + return OrderedDict( + [(attribute, getattr(self.env, attribute)) for attribute in self.attributes] + ) class MultiAgentObservation(ObservationType): - def __init__(self, - env: 'AbstractEnv', - observation_config: dict, - **kwargs) -> None: + def __init__(self, env: "AbstractEnv", observation_config: dict, **kwargs) -> None: super().__init__(env) self.observation_config = observation_config self.agents_observation_types = [] @@ -490,19 +593,23 @@ def __init__(self, self.agents_observation_types.append(obs_type) def space(self) -> spaces.Space: - return spaces.Tuple([obs_type.space() for obs_type in self.agents_observation_types]) + return spaces.Tuple( + [obs_type.space() for obs_type in self.agents_observation_types] + ) def observe(self) -> tuple: return tuple(obs_type.observe() for obs_type in self.agents_observation_types) class TupleObservation(ObservationType): - def __init__(self, - env: 'AbstractEnv', - observation_configs: List[dict], - **kwargs) -> None: + def __init__( + self, env: "AbstractEnv", observation_configs: List[dict], **kwargs + ) -> None: super().__init__(env) - self.observation_types = [observation_factory(self.env, obs_config) for obs_config in observation_configs] + self.observation_types = [ + observation_factory(self.env, obs_config) + for obs_config in observation_configs + ] def space(self) -> spaces.Space: return spaces.Tuple([obs_type.space() for obs_type in self.observation_types]) @@ -526,23 +633,37 @@ def observe(self) -> np.ndarray: df = pd.DataFrame.from_records([ego_dict])[self.features] # Add nearby traffic - close_vehicles = self.env.road.close_vehicles_to(self.observer_vehicle, - self.env.PERCEPTION_DISTANCE, - count=self.vehicles_count - 1, - see_behind=self.see_behind) + close_vehicles = self.env.road.close_vehicles_to( + self.observer_vehicle, + self.env.PERCEPTION_DISTANCE, + count=self.vehicles_count - 1, + see_behind=self.see_behind, + ) if close_vehicles: origin = self.observer_vehicle if not self.absolute else None - df = pd.concat([df, pd.DataFrame.from_records( - [v.to_dict(origin, observe_intentions=self.observe_intentions) - for v in close_vehicles[-self.vehicles_count + 1:]])[self.features]], - ignore_index=True) + df = pd.concat( + [ + df, + pd.DataFrame.from_records( + [ + v.to_dict( + origin, observe_intentions=self.observe_intentions + ) + for v in close_vehicles[-self.vehicles_count + 1 :] + ] + )[self.features], + ], + ignore_index=True, + ) # Normalize and clip if self.normalize: df = self.normalize_obs(df) # Fill missing rows if df.shape[0] < self.vehicles_count: rows = np.zeros((self.vehicles_count - df.shape[0], len(self.features))) - df = pd.concat([df, pd.DataFrame(data=rows, columns=self.features)], ignore_index=True) + df = pd.concat( + [df, pd.DataFrame(data=rows, columns=self.features)], ignore_index=True + ) # Reorder df = df[self.features] obs = df.values.copy() @@ -556,17 +677,20 @@ class LidarObservation(ObservationType): DISTANCE = 0 SPEED = 1 - def __init__(self, env, - cells: int = 16, - maximum_range: float = 60, - normalize: bool = True, - **kwargs): + def __init__( + self, + env, + cells: int = 16, + maximum_range: float = 60, + normalize: bool = True, + **kwargs + ): super().__init__(env, **kwargs) self.cells = cells self.maximum_range = maximum_range self.normalize = normalize self.angle = 2 * np.pi / self.cells - self.grid = np.ones((self.cells, 1)) * float('inf') + self.grid = np.ones((self.cells, 1)) * float("inf") self.origin = None def space(self) -> spaces.Space: @@ -574,7 +698,9 @@ def space(self) -> spaces.Space: return spaces.Box(shape=(self.cells, 2), low=-high, high=high, dtype=np.float32) def observe(self) -> np.ndarray: - obs = self.trace(self.observer_vehicle.position, self.observer_vehicle.velocity).copy() + obs = self.trace( + self.observer_vehicle.position, self.observer_vehicle.velocity + ).copy() if self.normalize: obs /= self.maximum_range return obs @@ -598,16 +724,22 @@ def trace(self, origin: np.ndarray, origin_velocity: np.ndarray) -> np.ndarray: self.grid[center_index, :] = [distance, velocity] # Angular sector covered by the obstacle - corners = utils.rect_corners(obstacle.position, obstacle.LENGTH, obstacle.WIDTH, obstacle.heading) + corners = utils.rect_corners( + obstacle.position, obstacle.LENGTH, obstacle.WIDTH, obstacle.heading + ) angles = [self.position_to_angle(corner, origin) for corner in corners] min_angle, max_angle = min(angles), max(angles) - if min_angle < -np.pi/2 < np.pi/2 < max_angle: # Object's corners are wrapping around +pi - min_angle, max_angle = max_angle, min_angle + 2*np.pi + if ( + min_angle < -np.pi / 2 < np.pi / 2 < max_angle + ): # Object's corners are wrapping around +pi + min_angle, max_angle = max_angle, min_angle + 2 * np.pi start, end = self.angle_to_index(min_angle), self.angle_to_index(max_angle) if start < end: - indexes = np.arange(start, end+1) + indexes = np.arange(start, end + 1) else: # Object's corners are wrapping around 0 - indexes = np.hstack([np.arange(start, self.cells), np.arange(0, end + 1)]) + indexes = np.hstack( + [np.arange(start, self.cells), np.arange(0, end + 1)] + ) # Actual distance computation for these sections for index in indexes: @@ -620,7 +752,10 @@ def trace(self, origin: np.ndarray, origin_velocity: np.ndarray) -> np.ndarray: return self.grid def position_to_angle(self, position: np.ndarray, origin: np.ndarray) -> float: - return np.arctan2(position[1] - origin[1], position[0] - origin[0]) + self.angle/2 + return ( + np.arctan2(position[1] - origin[1], position[0] - origin[0]) + + self.angle / 2 + ) def position_to_index(self, position: np.ndarray, origin: np.ndarray) -> int: return self.angle_to_index(self.position_to_angle(position, origin)) @@ -632,7 +767,7 @@ def index_to_direction(self, index: int) -> np.ndarray: return np.array([np.cos(index * self.angle), np.sin(index * self.angle)]) -def observation_factory(env: 'AbstractEnv', config: dict) -> ObservationType: +def observation_factory(env: "AbstractEnv", config: dict) -> ObservationType: if config["type"] == "TimeToCollision": return TimeToCollisionObservation(env, **config) elif config["type"] == "Kinematics": diff --git a/highway_env/envs/exit_env.py b/highway_env/envs/exit_env.py index 25f304a46..f42067849 100644 --- a/highway_env/envs/exit_env.py +++ b/highway_env/envs/exit_env.py @@ -1,43 +1,43 @@ +from typing import Dict, Text, Tuple + import numpy as np -from typing import Tuple, Dict, Text from highway_env import utils -from highway_env.envs import HighwayEnv, CircularLane, Vehicle +from highway_env.envs import CircularLane, HighwayEnv, Vehicle from highway_env.envs.common.action import Action from highway_env.road.road import Road, RoadNetwork from highway_env.vehicle.controller import ControlledVehicle class ExitEnv(HighwayEnv): - """ - """ + """ """ + @classmethod def default_config(cls) -> dict: config = super().default_config() - config.update({ - "observation": { - "type": "ExitObservation", - "vehicles_count": 15, - "features": ["presence", "x", "y", "vx", "vy", "cos_h", "sin_h"], - "clip": False - }, - "action": { - "type": "DiscreteMetaAction", - "target_speeds": [18, 24, 30] - }, - "lanes_count": 6, - "collision_reward": 0, - "high_speed_reward": 0.1, - "right_lane_reward": 0, - "normalize_reward": True, - "goal_reward": 1, - "vehicles_count": 20, - "vehicles_density": 1.5, - "controlled_vehicles": 1, - "duration": 18, # [s], - "simulation_frequency": 5, - "scaling": 5 - }) + config.update( + { + "observation": { + "type": "ExitObservation", + "vehicles_count": 15, + "features": ["presence", "x", "y", "vx", "vy", "cos_h", "sin_h"], + "clip": False, + }, + "action": {"type": "DiscreteMetaAction", "target_speeds": [18, 24, 30]}, + "lanes_count": 6, + "collision_reward": 0, + "high_speed_reward": 0.1, + "right_lane_reward": 0, + "normalize_reward": True, + "goal_reward": 1, + "vehicles_count": 20, + "vehicles_density": 1.5, + "controlled_vehicles": 1, + "duration": 18, # [s], + "simulation_frequency": 5, + "scaling": 5, + } + ) return config def _reset(self) -> None: @@ -49,59 +49,89 @@ def step(self, action) -> Tuple[np.ndarray, float, bool, dict]: info.update({"is_success": self._is_success()}) return obs, reward, terminal, info - def _create_road(self, road_length=1000, exit_position=400, exit_length=100) -> None: - net = RoadNetwork.straight_road_network(self.config["lanes_count"], start=0, - length=exit_position, nodes_str=("0", "1")) - net = RoadNetwork.straight_road_network(self.config["lanes_count"] + 1, start=exit_position, - length=exit_length, nodes_str=("1", "2"), net=net) - net = RoadNetwork.straight_road_network(self.config["lanes_count"], start=exit_position+exit_length, - length=road_length-exit_position-exit_length, - nodes_str=("2", "3"), net=net) + def _create_road( + self, road_length=1000, exit_position=400, exit_length=100 + ) -> None: + net = RoadNetwork.straight_road_network( + self.config["lanes_count"], + start=0, + length=exit_position, + nodes_str=("0", "1"), + ) + net = RoadNetwork.straight_road_network( + self.config["lanes_count"] + 1, + start=exit_position, + length=exit_length, + nodes_str=("1", "2"), + net=net, + ) + net = RoadNetwork.straight_road_network( + self.config["lanes_count"], + start=exit_position + exit_length, + length=road_length - exit_position - exit_length, + nodes_str=("2", "3"), + net=net, + ) for _from in net.graph: for _to in net.graph[_from]: for _id in range(len(net.graph[_from][_to])): net.get_lane((_from, _to, _id)).speed_limit = 26 - 3.4 * _id - exit_position = np.array([exit_position + exit_length, self.config["lanes_count"] * CircularLane.DEFAULT_WIDTH]) + exit_position = np.array( + [ + exit_position + exit_length, + self.config["lanes_count"] * CircularLane.DEFAULT_WIDTH, + ] + ) radius = 150 exit_center = exit_position + np.array([0, radius]) - lane = CircularLane(center=exit_center, - radius=radius, - start_phase=3*np.pi/2, - end_phase=2*np.pi, - forbidden=True) + lane = CircularLane( + center=exit_center, + radius=radius, + start_phase=3 * np.pi / 2, + end_phase=2 * np.pi, + forbidden=True, + ) net.add_lane("2", "exit", lane) - self.road = Road(network=net, - np_random=self.np_random, - record_history=self.config["show_trajectories"]) + self.road = Road( + network=net, + np_random=self.np_random, + record_history=self.config["show_trajectories"], + ) def _create_vehicles(self) -> None: """Create some new random vehicles of a given type, and add them on the road.""" self.controlled_vehicles = [] for _ in range(self.config["controlled_vehicles"]): - vehicle = Vehicle.create_random(self.road, - speed=25, - lane_from="0", - lane_to="1", - lane_id=0, - spacing=self.config["ego_spacing"]) - vehicle = self.action_type.vehicle_class(self.road, vehicle.position, vehicle.heading, vehicle.speed) + vehicle = Vehicle.create_random( + self.road, + speed=25, + lane_from="0", + lane_to="1", + lane_id=0, + spacing=self.config["ego_spacing"], + ) + vehicle = self.action_type.vehicle_class( + self.road, vehicle.position, vehicle.heading, vehicle.speed + ) self.controlled_vehicles.append(vehicle) self.road.vehicles.append(vehicle) vehicles_type = utils.class_from_path(self.config["other_vehicles_type"]) for _ in range(self.config["vehicles_count"]): lanes = np.arange(self.config["lanes_count"]) - lane_id = self.road.np_random.choice(lanes, size=1, - p=lanes / lanes.sum()).astype(int)[0] + lane_id = self.road.np_random.choice( + lanes, size=1, p=lanes / lanes.sum() + ).astype(int)[0] lane = self.road.network.get_lane(("0", "1", lane_id)) - vehicle = vehicles_type.create_random(self.road, - lane_from="0", - lane_to="1", - lane_id=lane_id, - speed=lane.speed_limit, - spacing=1 / self.config["vehicles_density"], - ).plan_route_to("3") + vehicle = vehicles_type.create_random( + self.road, + lane_from="0", + lane_to="1", + lane_id=lane_id, + speed=lane.speed_limit, + spacing=1 / self.config["vehicles_density"], + ).plan_route_to("3") vehicle.enable_lane_change = False self.road.vehicles.append(vehicle) @@ -111,27 +141,46 @@ def _reward(self, action: Action) -> float: :param action: the last action performed :return: the corresponding reward """ - reward = sum(self.config.get(name, 0) * reward for name, reward in self._rewards(action).items()) + reward = sum( + self.config.get(name, 0) * reward + for name, reward in self._rewards(action).items() + ) if self.config["normalize_reward"]: - reward = utils.lmap(reward, [self.config["collision_reward"], self.config["goal_reward"]], [0, 1]) + reward = utils.lmap( + reward, + [self.config["collision_reward"], self.config["goal_reward"]], + [0, 1], + ) reward = np.clip(reward, 0, 1) return reward def _rewards(self, action: Action) -> Dict[Text, float]: - lane_index = self.vehicle.target_lane_index if isinstance(self.vehicle, ControlledVehicle) \ + lane_index = ( + self.vehicle.target_lane_index + if isinstance(self.vehicle, ControlledVehicle) else self.vehicle.lane_index - scaled_speed = utils.lmap(self.vehicle.speed, self.config["reward_speed_range"], [0, 1]) + ) + scaled_speed = utils.lmap( + self.vehicle.speed, self.config["reward_speed_range"], [0, 1] + ) return { "collision_reward": self.vehicle.crashed, "goal_reward": self._is_success(), "high_speed_reward": np.clip(scaled_speed, 0, 1), - "right_lane_reward": lane_index[-1] + "right_lane_reward": lane_index[-1], } def _is_success(self): - lane_index = self.vehicle.target_lane_index if isinstance(self.vehicle, ControlledVehicle) \ + lane_index = ( + self.vehicle.target_lane_index + if isinstance(self.vehicle, ControlledVehicle) else self.vehicle.lane_index - goal_reached = lane_index == ("1", "2", self.config["lanes_count"]) or lane_index == ("2", "exit", 0) + ) + goal_reached = lane_index == ( + "1", + "2", + self.config["lanes_count"], + ) or lane_index == ("2", "exit", 0) return goal_reached def _is_terminated(self) -> bool: @@ -143,10 +192,8 @@ def _is_truncated(self) -> bool: return self.time >= self.config["duration"] - # class DenseLidarExitEnv(DenseExitEnv): # @classmethod # def default_config(cls) -> dict: # return dict(super().default_config(), # observation=dict(type="LidarObservation")) - diff --git a/highway_env/envs/highway_env.py b/highway_env/envs/highway_env.py index db9163631..c5e05a27b 100644 --- a/highway_env/envs/highway_env.py +++ b/highway_env/envs/highway_env.py @@ -24,30 +24,30 @@ class HighwayEnv(AbstractEnv): @classmethod def default_config(cls) -> dict: config = super().default_config() - config.update({ - "observation": { - "type": "Kinematics" - }, - "action": { - "type": "DiscreteMetaAction", - }, - "lanes_count": 4, - "vehicles_count": 50, - "controlled_vehicles": 1, - "initial_lane_id": None, - "duration": 40, # [s] - "ego_spacing": 2, - "vehicles_density": 1, - "collision_reward": -1, # The reward received when colliding with a vehicle. - "right_lane_reward": 0.1, # The reward received when driving on the right-most lanes, linearly mapped to - # zero for other lanes. - "high_speed_reward": 0.4, # The reward received when driving at full speed, linearly mapped to zero for - # lower speeds according to config["reward_speed_range"]. - "lane_change_reward": 0, # The reward received at each lane change action. - "reward_speed_range": [20, 30], - "normalize_reward": True, - "offroad_terminal": False - }) + config.update( + { + "observation": {"type": "Kinematics"}, + "action": { + "type": "DiscreteMetaAction", + }, + "lanes_count": 4, + "vehicles_count": 50, + "controlled_vehicles": 1, + "initial_lane_id": None, + "duration": 40, # [s] + "ego_spacing": 2, + "vehicles_density": 1, + "collision_reward": -1, # The reward received when colliding with a vehicle. + "right_lane_reward": 0.1, # The reward received when driving on the right-most lanes, linearly mapped to + # zero for other lanes. + "high_speed_reward": 0.4, # The reward received when driving at full speed, linearly mapped to zero for + # lower speeds according to config["reward_speed_range"]. + "lane_change_reward": 0, # The reward received at each lane change action. + "reward_speed_range": [20, 30], + "normalize_reward": True, + "offroad_terminal": False, + } + ) return config def _reset(self) -> None: @@ -56,13 +56,20 @@ def _reset(self) -> None: def _create_road(self) -> None: """Create a road composed of straight adjacent lanes.""" - self.road = Road(network=RoadNetwork.straight_road_network(self.config["lanes_count"], speed_limit=30), - np_random=self.np_random, record_history=self.config["show_trajectories"]) + self.road = Road( + network=RoadNetwork.straight_road_network( + self.config["lanes_count"], speed_limit=30 + ), + np_random=self.np_random, + record_history=self.config["show_trajectories"], + ) def _create_vehicles(self) -> None: """Create some new random vehicles of a given type, and add them on the road.""" other_vehicles_type = utils.class_from_path(self.config["other_vehicles_type"]) - other_per_controlled = near_split(self.config["vehicles_count"], num_bins=self.config["controlled_vehicles"]) + other_per_controlled = near_split( + self.config["vehicles_count"], num_bins=self.config["controlled_vehicles"] + ) self.controlled_vehicles = [] for others in other_per_controlled: @@ -70,14 +77,18 @@ def _create_vehicles(self) -> None: self.road, speed=25, lane_id=self.config["initial_lane_id"], - spacing=self.config["ego_spacing"] + spacing=self.config["ego_spacing"], + ) + vehicle = self.action_type.vehicle_class( + self.road, vehicle.position, vehicle.heading, vehicle.speed ) - vehicle = self.action_type.vehicle_class(self.road, vehicle.position, vehicle.heading, vehicle.speed) self.controlled_vehicles.append(vehicle) self.road.vehicles.append(vehicle) for _ in range(others): - vehicle = other_vehicles_type.create_random(self.road, spacing=1 / self.config["vehicles_density"]) + vehicle = other_vehicles_type.create_random( + self.road, spacing=1 / self.config["vehicles_density"] + ) vehicle.randomize_behavior() self.road.vehicles.append(vehicle) @@ -88,33 +99,47 @@ def _reward(self, action: Action) -> float: :return: the corresponding reward """ rewards = self._rewards(action) - reward = sum(self.config.get(name, 0) * reward for name, reward in rewards.items()) + reward = sum( + self.config.get(name, 0) * reward for name, reward in rewards.items() + ) if self.config["normalize_reward"]: - reward = utils.lmap(reward, - [self.config["collision_reward"], - self.config["high_speed_reward"] + self.config["right_lane_reward"]], - [0, 1]) - reward *= rewards['on_road_reward'] + reward = utils.lmap( + reward, + [ + self.config["collision_reward"], + self.config["high_speed_reward"] + self.config["right_lane_reward"], + ], + [0, 1], + ) + reward *= rewards["on_road_reward"] return reward def _rewards(self, action: Action) -> Dict[Text, float]: neighbours = self.road.network.all_side_lanes(self.vehicle.lane_index) - lane = self.vehicle.target_lane_index[2] if isinstance(self.vehicle, ControlledVehicle) \ + lane = ( + self.vehicle.target_lane_index[2] + if isinstance(self.vehicle, ControlledVehicle) else self.vehicle.lane_index[2] + ) # Use forward speed rather than speed, see https://github.com/eleurent/highway-env/issues/268 forward_speed = self.vehicle.speed * np.cos(self.vehicle.heading) - scaled_speed = utils.lmap(forward_speed, self.config["reward_speed_range"], [0, 1]) + scaled_speed = utils.lmap( + forward_speed, self.config["reward_speed_range"], [0, 1] + ) return { "collision_reward": float(self.vehicle.crashed), "right_lane_reward": lane / max(len(neighbours) - 1, 1), "high_speed_reward": np.clip(scaled_speed, 0, 1), - "on_road_reward": float(self.vehicle.on_road) + "on_road_reward": float(self.vehicle.on_road), } def _is_terminated(self) -> bool: """The episode is over if the ego vehicle crashed.""" - return (self.vehicle.crashed or - self.config["offroad_terminal"] and not self.vehicle.on_road) + return ( + self.vehicle.crashed + or self.config["offroad_terminal"] + and not self.vehicle.on_road + ) def _is_truncated(self) -> bool: """The episode is truncated if the time limit is reached.""" @@ -128,16 +153,19 @@ class HighwayEnvFast(HighwayEnv): - fewer vehicles in the scene (and fewer lanes, shorter episode duration) - only check collision of controlled vehicles with others """ + @classmethod def default_config(cls) -> dict: cfg = super().default_config() - cfg.update({ - "simulation_frequency": 5, - "lanes_count": 3, - "vehicles_count": 20, - "duration": 30, # [s] - "ego_spacing": 1.5, - }) + cfg.update( + { + "simulation_frequency": 5, + "lanes_count": 3, + "vehicles_count": 20, + "duration": 30, # [s] + "ego_spacing": 1.5, + } + ) return cfg def _create_vehicles(self) -> None: diff --git a/highway_env/envs/intersection_env.py b/highway_env/envs/intersection_env.py index 1780bbcb6..4b512cec9 100644 --- a/highway_env/envs/intersection_env.py +++ b/highway_env/envs/intersection_env.py @@ -1,109 +1,118 @@ -from typing import Dict, Tuple, Text +from typing import Dict, Text, Tuple import numpy as np from highway_env import utils from highway_env.envs.common.abstract import AbstractEnv, MultiAgentWrapper -from highway_env.road.lane import LineType, StraightLane, CircularLane, AbstractLane +from highway_env.road.lane import AbstractLane, CircularLane, LineType, StraightLane from highway_env.road.regulation import RegulatedRoad from highway_env.road.road import RoadNetwork from highway_env.vehicle.kinematics import Vehicle -from highway_env.vehicle.controller import ControlledVehicle class IntersectionEnv(AbstractEnv): - - ACTIONS: Dict[int, str] = { - 0: 'SLOWER', - 1: 'IDLE', - 2: 'FASTER' - } + ACTIONS: Dict[int, str] = {0: "SLOWER", 1: "IDLE", 2: "FASTER"} ACTIONS_INDEXES = {v: k for k, v in ACTIONS.items()} @classmethod def default_config(cls) -> dict: config = super().default_config() - config.update({ - "observation": { - "type": "Kinematics", - "vehicles_count": 15, - "features": ["presence", "x", "y", "vx", "vy", "cos_h", "sin_h"], - "features_range": { - "x": [-100, 100], - "y": [-100, 100], - "vx": [-20, 20], - "vy": [-20, 20], + config.update( + { + "observation": { + "type": "Kinematics", + "vehicles_count": 15, + "features": ["presence", "x", "y", "vx", "vy", "cos_h", "sin_h"], + "features_range": { + "x": [-100, 100], + "y": [-100, 100], + "vx": [-20, 20], + "vy": [-20, 20], + }, + "absolute": True, + "flatten": False, + "observe_intentions": False, + }, + "action": { + "type": "DiscreteMetaAction", + "longitudinal": True, + "lateral": False, + "target_speeds": [0, 4.5, 9], }, - "absolute": True, - "flatten": False, - "observe_intentions": False - }, - "action": { - "type": "DiscreteMetaAction", - "longitudinal": True, - "lateral": False, - "target_speeds": [0, 4.5, 9] - }, - "duration": 13, # [s] - "destination": "o1", - "controlled_vehicles": 1, - "initial_vehicle_count": 10, - "spawn_probability": 0.6, - "screen_width": 600, - "screen_height": 600, - "centering_position": [0.5, 0.6], - "scaling": 5.5 * 1.3, - "collision_reward": -5, - "high_speed_reward": 1, - "arrived_reward": 1, - "reward_speed_range": [7.0, 9.0], - "normalize_reward": False, - "offroad_terminal": False - }) + "duration": 13, # [s] + "destination": "o1", + "controlled_vehicles": 1, + "initial_vehicle_count": 10, + "spawn_probability": 0.6, + "screen_width": 600, + "screen_height": 600, + "centering_position": [0.5, 0.6], + "scaling": 5.5 * 1.3, + "collision_reward": -5, + "high_speed_reward": 1, + "arrived_reward": 1, + "reward_speed_range": [7.0, 9.0], + "normalize_reward": False, + "offroad_terminal": False, + } + ) return config def _reward(self, action: int) -> float: """Aggregated reward, for cooperative agents.""" - return sum(self._agent_reward(action, vehicle) for vehicle in self.controlled_vehicles - ) / len(self.controlled_vehicles) + return sum( + self._agent_reward(action, vehicle) for vehicle in self.controlled_vehicles + ) / len(self.controlled_vehicles) def _rewards(self, action: int) -> Dict[Text, float]: """Multi-objective rewards, for cooperative agents.""" - agents_rewards = [self._agent_rewards(action, vehicle) for vehicle in self.controlled_vehicles] + agents_rewards = [ + self._agent_rewards(action, vehicle) for vehicle in self.controlled_vehicles + ] return { - name: sum(agent_rewards[name] for agent_rewards in agents_rewards) / len(agents_rewards) + name: sum(agent_rewards[name] for agent_rewards in agents_rewards) + / len(agents_rewards) for name in agents_rewards[0].keys() } def _agent_reward(self, action: int, vehicle: Vehicle) -> float: """Per-agent reward signal.""" rewards = self._agent_rewards(action, vehicle) - reward = sum(self.config.get(name, 0) * reward for name, reward in rewards.items()) + reward = sum( + self.config.get(name, 0) * reward for name, reward in rewards.items() + ) reward = self.config["arrived_reward"] if rewards["arrived_reward"] else reward reward *= rewards["on_road_reward"] if self.config["normalize_reward"]: - reward = utils.lmap(reward, [self.config["collision_reward"], self.config["arrived_reward"]], [0, 1]) + reward = utils.lmap( + reward, + [self.config["collision_reward"], self.config["arrived_reward"]], + [0, 1], + ) return reward def _agent_rewards(self, action: int, vehicle: Vehicle) -> Dict[Text, float]: """Per-agent per-objective reward signal.""" - scaled_speed = utils.lmap(vehicle.speed, self.config["reward_speed_range"], [0, 1]) + scaled_speed = utils.lmap( + vehicle.speed, self.config["reward_speed_range"], [0, 1] + ) return { "collision_reward": vehicle.crashed, "high_speed_reward": np.clip(scaled_speed, 0, 1), "arrived_reward": self.has_arrived(vehicle), - "on_road_reward": vehicle.on_road + "on_road_reward": vehicle.on_road, } def _is_terminated(self) -> bool: - return any(vehicle.crashed for vehicle in self.controlled_vehicles) \ - or all(self.has_arrived(vehicle) for vehicle in self.controlled_vehicles) \ - or (self.config["offroad_terminal"] and not self.vehicle.on_road) + return ( + any(vehicle.crashed for vehicle in self.controlled_vehicles) + or all(self.has_arrived(vehicle) for vehicle in self.controlled_vehicles) + or (self.config["offroad_terminal"] and not self.vehicle.on_road) + ) def _agent_is_terminal(self, vehicle: Vehicle) -> bool: """The episode is over when a collision occurs or when the access ramp has been passed.""" - return (vehicle.crashed or - self.has_arrived(vehicle)) + return vehicle.crashed or self.has_arrived(vehicle) def _is_truncated(self) -> bool: """The episode is truncated if the time limit is reached.""" @@ -111,8 +120,12 @@ def _is_truncated(self) -> bool: def _info(self, obs: np.ndarray, action: int) -> dict: info = super()._info(obs, action) - info["agents_rewards"] = tuple(self._agent_reward(action, vehicle) for vehicle in self.controlled_vehicles) - info["agents_dones"] = tuple(self._agent_is_terminal(vehicle) for vehicle in self.controlled_vehicles) + info["agents_rewards"] = tuple( + self._agent_reward(action, vehicle) for vehicle in self.controlled_vehicles + ) + info["agents_dones"] = tuple( + self._agent_is_terminal(vehicle) for vehicle in self.controlled_vehicles + ) return info def _reset(self) -> None: @@ -152,34 +165,87 @@ def _make_road(self) -> None: angle = np.radians(90 * corner) is_horizontal = corner % 2 priority = 3 if is_horizontal else 1 - rotation = np.array([[np.cos(angle), -np.sin(angle)], [np.sin(angle), np.cos(angle)]]) + rotation = np.array( + [[np.cos(angle), -np.sin(angle)], [np.sin(angle), np.cos(angle)]] + ) # Incoming - start = rotation @ np.array([lane_width / 2, access_length + outer_distance]) + start = rotation @ np.array( + [lane_width / 2, access_length + outer_distance] + ) end = rotation @ np.array([lane_width / 2, outer_distance]) - net.add_lane("o" + str(corner), "ir" + str(corner), - StraightLane(start, end, line_types=[s, c], priority=priority, speed_limit=10)) + net.add_lane( + "o" + str(corner), + "ir" + str(corner), + StraightLane( + start, end, line_types=[s, c], priority=priority, speed_limit=10 + ), + ) # Right turn r_center = rotation @ (np.array([outer_distance, outer_distance])) - net.add_lane("ir" + str(corner), "il" + str((corner - 1) % 4), - CircularLane(r_center, right_turn_radius, angle + np.radians(180), angle + np.radians(270), - line_types=[n, c], priority=priority, speed_limit=10)) + net.add_lane( + "ir" + str(corner), + "il" + str((corner - 1) % 4), + CircularLane( + r_center, + right_turn_radius, + angle + np.radians(180), + angle + np.radians(270), + line_types=[n, c], + priority=priority, + speed_limit=10, + ), + ) # Left turn - l_center = rotation @ (np.array([-left_turn_radius + lane_width / 2, left_turn_radius - lane_width / 2])) - net.add_lane("ir" + str(corner), "il" + str((corner + 1) % 4), - CircularLane(l_center, left_turn_radius, angle + np.radians(0), angle + np.radians(-90), - clockwise=False, line_types=[n, n], priority=priority - 1, speed_limit=10)) + l_center = rotation @ ( + np.array( + [ + -left_turn_radius + lane_width / 2, + left_turn_radius - lane_width / 2, + ] + ) + ) + net.add_lane( + "ir" + str(corner), + "il" + str((corner + 1) % 4), + CircularLane( + l_center, + left_turn_radius, + angle + np.radians(0), + angle + np.radians(-90), + clockwise=False, + line_types=[n, n], + priority=priority - 1, + speed_limit=10, + ), + ) # Straight start = rotation @ np.array([lane_width / 2, outer_distance]) end = rotation @ np.array([lane_width / 2, -outer_distance]) - net.add_lane("ir" + str(corner), "il" + str((corner + 2) % 4), - StraightLane(start, end, line_types=[s, n], priority=priority, speed_limit=10)) + net.add_lane( + "ir" + str(corner), + "il" + str((corner + 2) % 4), + StraightLane( + start, end, line_types=[s, n], priority=priority, speed_limit=10 + ), + ) # Exit - start = rotation @ np.flip([lane_width / 2, access_length + outer_distance], axis=0) + start = rotation @ np.flip( + [lane_width / 2, access_length + outer_distance], axis=0 + ) end = rotation @ np.flip([lane_width / 2, outer_distance], axis=0) - net.add_lane("il" + str((corner - 1) % 4), "o" + str((corner - 1) % 4), - StraightLane(end, start, line_types=[n, c], priority=priority, speed_limit=10)) - - road = RegulatedRoad(network=net, np_random=self.np_random, record_history=self.config["show_trajectories"]) + net.add_lane( + "il" + str((corner - 1) % 4), + "o" + str((corner - 1) % 4), + StraightLane( + end, start, line_types=[n, c], priority=priority, speed_limit=10 + ), + ) + + road = RegulatedRoad( + network=net, + np_random=self.np_random, + record_history=self.config["show_trajectories"], + ) self.road = road def _make_vehicles(self, n_vehicles: int = 10) -> None: @@ -199,50 +265,80 @@ def _make_vehicles(self, n_vehicles: int = 10) -> None: for t in range(n_vehicles - 1): self._spawn_vehicle(np.linspace(0, 80, n_vehicles)[t]) for _ in range(simulation_steps): - [(self.road.act(), self.road.step(1 / self.config["simulation_frequency"])) for _ in range(self.config["simulation_frequency"])] + [ + ( + self.road.act(), + self.road.step(1 / self.config["simulation_frequency"]), + ) + for _ in range(self.config["simulation_frequency"]) + ] # Challenger vehicle - self._spawn_vehicle(60, spawn_probability=1, go_straight=True, position_deviation=0.1, speed_deviation=0) + self._spawn_vehicle( + 60, + spawn_probability=1, + go_straight=True, + position_deviation=0.1, + speed_deviation=0, + ) # Controlled vehicles self.controlled_vehicles = [] for ego_id in range(0, self.config["controlled_vehicles"]): - ego_lane = self.road.network.get_lane(("o{}".format(ego_id % 4), "ir{}".format(ego_id % 4), 0)) - destination = self.config["destination"] or "o" + str(self.np_random.integers(1, 4)) + ego_lane = self.road.network.get_lane( + ("o{}".format(ego_id % 4), "ir{}".format(ego_id % 4), 0) + ) + destination = self.config["destination"] or "o" + str( + self.np_random.integers(1, 4) + ) ego_vehicle = self.action_type.vehicle_class( - self.road, - ego_lane.position(60 + 5*self.np_random.normal(1), 0), - speed=ego_lane.speed_limit, - heading=ego_lane.heading_at(60)) + self.road, + ego_lane.position(60 + 5 * self.np_random.normal(1), 0), + speed=ego_lane.speed_limit, + heading=ego_lane.heading_at(60), + ) try: ego_vehicle.plan_route_to(destination) - ego_vehicle.speed_index = ego_vehicle.speed_to_index(ego_lane.speed_limit) - ego_vehicle.target_speed = ego_vehicle.index_to_speed(ego_vehicle.speed_index) + ego_vehicle.speed_index = ego_vehicle.speed_to_index( + ego_lane.speed_limit + ) + ego_vehicle.target_speed = ego_vehicle.index_to_speed( + ego_vehicle.speed_index + ) except AttributeError: pass self.road.vehicles.append(ego_vehicle) self.controlled_vehicles.append(ego_vehicle) for v in self.road.vehicles: # Prevent early collisions - if v is not ego_vehicle and np.linalg.norm(v.position - ego_vehicle.position) < 20: + if ( + v is not ego_vehicle + and np.linalg.norm(v.position - ego_vehicle.position) < 20 + ): self.road.vehicles.remove(v) - def _spawn_vehicle(self, - longitudinal: float = 0, - position_deviation: float = 1., - speed_deviation: float = 1., - spawn_probability: float = 0.6, - go_straight: bool = False) -> None: + def _spawn_vehicle( + self, + longitudinal: float = 0, + position_deviation: float = 1.0, + speed_deviation: float = 1.0, + spawn_probability: float = 0.6, + go_straight: bool = False, + ) -> None: if self.np_random.uniform() > spawn_probability: return route = self.np_random.choice(range(4), size=2, replace=False) route[1] = (route[0] + 2) % 4 if go_straight else route[1] vehicle_type = utils.class_from_path(self.config["other_vehicles_type"]) - vehicle = vehicle_type.make_on_lane(self.road, ("o" + str(route[0]), "ir" + str(route[0]), 0), - longitudinal=(longitudinal + 5 - + self.np_random.normal() * position_deviation), - speed=8 + self.np_random.normal() * speed_deviation) + vehicle = vehicle_type.make_on_lane( + self.road, + ("o" + str(route[0]), "ir" + str(route[0]), 0), + longitudinal=( + longitudinal + 5 + self.np_random.normal() * position_deviation + ), + speed=8 + self.np_random.normal() * speed_deviation, + ) for v in self.road.vehicles: if np.linalg.norm(v.position - vehicle.position) < 15: return @@ -252,59 +348,81 @@ def _spawn_vehicle(self, return vehicle def _clear_vehicles(self) -> None: - is_leaving = lambda vehicle: "il" in vehicle.lane_index[0] and "o" in vehicle.lane_index[1] \ - and vehicle.lane.local_coordinates(vehicle.position)[0] \ - >= vehicle.lane.length - 4 * vehicle.LENGTH - self.road.vehicles = [vehicle for vehicle in self.road.vehicles if - vehicle in self.controlled_vehicles or not (is_leaving(vehicle) or vehicle.route is None)] + is_leaving = ( + lambda vehicle: "il" in vehicle.lane_index[0] + and "o" in vehicle.lane_index[1] + and vehicle.lane.local_coordinates(vehicle.position)[0] + >= vehicle.lane.length - 4 * vehicle.LENGTH + ) + self.road.vehicles = [ + vehicle + for vehicle in self.road.vehicles + if vehicle in self.controlled_vehicles + or not (is_leaving(vehicle) or vehicle.route is None) + ] def has_arrived(self, vehicle: Vehicle, exit_distance: float = 25) -> bool: - return "il" in vehicle.lane_index[0] \ - and "o" in vehicle.lane_index[1] \ - and vehicle.lane.local_coordinates(vehicle.position)[0] >= exit_distance + return ( + "il" in vehicle.lane_index[0] + and "o" in vehicle.lane_index[1] + and vehicle.lane.local_coordinates(vehicle.position)[0] >= exit_distance + ) class MultiAgentIntersectionEnv(IntersectionEnv): @classmethod def default_config(cls) -> dict: config = super().default_config() - config.update({ - "action": { - "type": "MultiAgentAction", - "action_config": { - "type": "DiscreteMetaAction", - "lateral": False, - "longitudinal": True - } - }, - "observation": { - "type": "MultiAgentObservation", - "observation_config": { - "type": "Kinematics" - } - }, - "controlled_vehicles": 2 - }) + config.update( + { + "action": { + "type": "MultiAgentAction", + "action_config": { + "type": "DiscreteMetaAction", + "lateral": False, + "longitudinal": True, + }, + }, + "observation": { + "type": "MultiAgentObservation", + "observation_config": {"type": "Kinematics"}, + }, + "controlled_vehicles": 2, + } + ) return config + class ContinuousIntersectionEnv(IntersectionEnv): @classmethod def default_config(cls) -> dict: config = super().default_config() - config.update({ - "observation": { - "type": "Kinematics", - "vehicles_count": 5, - "features": ["presence", "x", "y", "vx", "vy", "long_off", "lat_off", "ang_off"], - }, - "action": { - "type": "ContinuousAction", - "steering_range": [-np.pi / 3, np.pi / 3], - "longitudinal": True, - "lateral": True, - "dynamical": True - }, - }) + config.update( + { + "observation": { + "type": "Kinematics", + "vehicles_count": 5, + "features": [ + "presence", + "x", + "y", + "vx", + "vy", + "long_off", + "lat_off", + "ang_off", + ], + }, + "action": { + "type": "ContinuousAction", + "steering_range": [-np.pi / 3, np.pi / 3], + "longitudinal": True, + "lateral": True, + "dynamical": True, + }, + } + ) return config + TupleMultiAgentIntersectionEnv = MultiAgentWrapper(MultiAgentIntersectionEnv) diff --git a/highway_env/envs/lane_keeping_env.py b/highway_env/envs/lane_keeping_env.py index 7f6540c87..2d760cb78 100644 --- a/highway_env/envs/lane_keeping_env.py +++ b/highway_env/envs/lane_keeping_env.py @@ -1,4 +1,4 @@ -from __future__ import division, print_function, absolute_import +from __future__ import absolute_import, division, print_function import copy from typing import Tuple @@ -26,27 +26,29 @@ def __init__(self, config: dict = None) -> None: @classmethod def default_config(cls) -> dict: config = super().default_config() - config.update({ - "observation": { - "type": "AttributesObservation", - "attributes": ["state", "derivative", "reference_state"] - }, - "action": { - "type": "ContinuousAction", - "steering_range": [-np.pi / 3, np.pi / 3], - "longitudinal": False, - "lateral": True, - "dynamical": True - }, - "simulation_frequency": 10, - "policy_frequency": 10, - "state_noise": 0.05, - "derivative_noise": 0.05, - "screen_width": 600, - "screen_height": 250, - "scaling": 7, - "centering_position": [0.4, 0.5] - }) + config.update( + { + "observation": { + "type": "AttributesObservation", + "attributes": ["state", "derivative", "reference_state"], + }, + "action": { + "type": "ContinuousAction", + "steering_range": [-np.pi / 3, np.pi / 3], + "longitudinal": False, + "lateral": True, + "dynamical": True, + }, + "simulation_frequency": 10, + "policy_frequency": 10, + "state_noise": 0.05, + "derivative_noise": 0.05, + "screen_width": 600, + "screen_height": 250, + "scaling": 7, + "centering_position": [0.4, 0.5], + } + ) return config def step(self, action: np.ndarray) -> Tuple[np.ndarray, float, bool, bool, dict]: @@ -54,8 +56,9 @@ def step(self, action: np.ndarray) -> Tuple[np.ndarray, float, bool, bool, dict] self.lane = self.lanes.pop(0) self.store_data() if self.lpv: - self.lpv.set_control(control=action.squeeze(-1), - state=self.vehicle.state[[1, 2, 4, 5]]) + self.lpv.set_control( + control=action.squeeze(-1), state=self.vehicle.state[[1, 2, 4, 5]] + ) self.lpv.step(1 / self.config["simulation_frequency"]) self.action_type.act(action) @@ -70,7 +73,7 @@ def step(self, action: np.ndarray) -> Tuple[np.ndarray, float, bool, bool, dict] def _reward(self, action: np.ndarray) -> float: _, lat = self.lane.local_coordinates(self.vehicle.position) - return 1 - (lat/self.lane.width)**2 + return 1 - (lat / self.lane.width) ** 2 def _is_terminated(self) -> bool: return False @@ -84,25 +87,50 @@ def _reset(self) -> None: def _make_road(self) -> None: net = RoadNetwork() - lane = SineLane([0, 0], [500, 0], amplitude=5, pulsation=2*np.pi / 100, phase=0, - width=10, line_types=[LineType.STRIPED, LineType.STRIPED]) + lane = SineLane( + [0, 0], + [500, 0], + amplitude=5, + pulsation=2 * np.pi / 100, + phase=0, + width=10, + line_types=[LineType.STRIPED, LineType.STRIPED], + ) net.add_lane("a", "b", lane) - other_lane = StraightLane([50, 50], [115, 15], - line_types=(LineType.STRIPED, LineType.STRIPED), width=10) + other_lane = StraightLane( + [50, 50], + [115, 15], + line_types=(LineType.STRIPED, LineType.STRIPED), + width=10, + ) net.add_lane("c", "d", other_lane) self.lanes = [other_lane, lane] self.lane = self.lanes.pop(0) - net.add_lane("d", "a", StraightLane([115, 15], [115+20, 15+20*(15-50)/(115-50)], - line_types=(LineType.NONE, LineType.STRIPED), width=10)) - road = Road(network=net, np_random=self.np_random, record_history=self.config["show_trajectories"]) + net.add_lane( + "d", + "a", + StraightLane( + [115, 15], + [115 + 20, 15 + 20 * (15 - 50) / (115 - 50)], + line_types=(LineType.NONE, LineType.STRIPED), + width=10, + ), + ) + road = Road( + network=net, + np_random=self.np_random, + record_history=self.config["show_trajectories"], + ) self.road = road def _make_vehicles(self) -> None: road = self.road ego_vehicle = self.action_type.vehicle_class( - road, road.network.get_lane(("c", "d", 0)).position(50, -4), + road, + road.network.get_lane(("c", "d", 0)).position(50, -4), heading=road.network.get_lane(("c", "d", 0)).heading_at(0), - speed=8.3) + speed=8.3, + ) road.vehicles.append(ego_vehicle) self.vehicle = ego_vehicle @@ -114,19 +142,21 @@ def dynamics(self) -> BicycleVehicle: def state(self) -> np.ndarray: if not self.vehicle: return np.zeros((4, 1)) - return self.vehicle.state[[1, 2, 4, 5]] + \ - self.np_random.uniform(low=-self.config["state_noise"], - high=self.config["state_noise"], - size=self.vehicle.state[[0, 2, 4, 5]].shape) + return self.vehicle.state[[1, 2, 4, 5]] + self.np_random.uniform( + low=-self.config["state_noise"], + high=self.config["state_noise"], + size=self.vehicle.state[[0, 2, 4, 5]].shape, + ) @property def derivative(self) -> np.ndarray: if not self.vehicle: return np.zeros((4, 1)) - return self.vehicle.derivative[[1, 2, 4, 5]] + \ - self.np_random.uniform(low=-self.config["derivative_noise"], - high=self.config["derivative_noise"], - size=self.vehicle.derivative[[0, 2, 4, 5]].shape) + return self.vehicle.derivative[[1, 2, 4, 5]] + self.np_random.uniform( + low=-self.config["derivative_noise"], + high=self.config["derivative_noise"], + size=self.vehicle.derivative[[0, 2, 4, 5]].shape, + ) @property def reference_state(self) -> np.ndarray: @@ -141,7 +171,9 @@ def store_data(self) -> None: if self.lpv: state = self.vehicle.state.copy() interval = [] - for x_t in self.lpv.change_coordinates(self.lpv.x_i_t, back=True, interval=True): + for x_t in self.lpv.change_coordinates( + self.lpv.x_i_t, back=True, interval=True + ): # lateral state to full state np.put(state, [1, 2, 4, 5], x_t) # full state to absolute coordinates diff --git a/highway_env/envs/merge_env.py b/highway_env/envs/merge_env.py index 32faed792..6a1df275b 100644 --- a/highway_env/envs/merge_env.py +++ b/highway_env/envs/merge_env.py @@ -4,7 +4,7 @@ from highway_env import utils from highway_env.envs.common.abstract import AbstractEnv -from highway_env.road.lane import LineType, StraightLane, SineLane +from highway_env.road.lane import LineType, SineLane, StraightLane from highway_env.road.road import Road, RoadNetwork from highway_env.vehicle.controller import ControlledVehicle from highway_env.vehicle.objects import Obstacle @@ -23,14 +23,16 @@ class MergeEnv(AbstractEnv): @classmethod def default_config(cls) -> dict: cfg = super().default_config() - cfg.update({ - "collision_reward": -1, - "right_lane_reward": 0.1, - "high_speed_reward": 0.2, - "reward_speed_range": [20, 30], - "merging_speed_reward": -0.5, - "lane_change_reward": -0.05, - }) + cfg.update( + { + "collision_reward": -1, + "right_lane_reward": 0.1, + "high_speed_reward": 0.2, + "reward_speed_range": [20, 30], + "merging_speed_reward": -0.5, + "lane_change_reward": -0.05, + } + ) return cfg def _reward(self, action: int) -> float: @@ -42,14 +44,23 @@ def _reward(self, action: int) -> float: :param action: the action performed :return: the reward of the state-action transition """ - reward = sum(self.config.get(name, 0) * reward for name, reward in self._rewards(action).items()) - return utils.lmap(reward, - [self.config["collision_reward"] + self.config["merging_speed_reward"], - self.config["high_speed_reward"] + self.config["right_lane_reward"]], - [0, 1]) + reward = sum( + self.config.get(name, 0) * reward + for name, reward in self._rewards(action).items() + ) + return utils.lmap( + reward, + [ + self.config["collision_reward"] + self.config["merging_speed_reward"], + self.config["high_speed_reward"] + self.config["right_lane_reward"], + ], + [0, 1], + ) def _rewards(self, action: int) -> Dict[Text, float]: - scaled_speed = utils.lmap(self.vehicle.speed, self.config["reward_speed_range"], [0, 1]) + scaled_speed = utils.lmap( + self.vehicle.speed, self.config["reward_speed_range"], [0, 1] + ) return { "collision_reward": self.vehicle.crashed, "right_lane_reward": self.vehicle.lane_index[2] / 1, @@ -58,14 +69,15 @@ def _rewards(self, action: int) -> Dict[Text, float]: "merging_speed_reward": sum( # Altruistic penalty (vehicle.target_speed - vehicle.speed) / vehicle.target_speed for vehicle in self.road.vehicles - if vehicle.lane_index == ("b", "c", 2) and isinstance(vehicle, ControlledVehicle) - ) + if vehicle.lane_index == ("b", "c", 2) + and isinstance(vehicle, ControlledVehicle) + ), } def _is_terminated(self) -> bool: """The episode is over when a collision occurs or when the access ramp has been passed.""" print("crash" + str(self.vehicle.crashed)) - print("over" + str(self.vehicle.position[0] > 370)) + print("over" + str(self.vehicle.position[0] > 370)) return self.vehicle.crashed or bool(self.vehicle.position[0] > 370) def _is_truncated(self) -> bool: @@ -90,21 +102,56 @@ def _make_road(self) -> None: 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])) + 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) + 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, record_history=self.config["show_trajectories"]) + road = Road( + network=net, + np_random=self.np_random, + record_history=self.config["show_trajectories"], + ) road.objects.append(Obstacle(road, lbc.position(ends[2], 0))) self.road = road @@ -115,9 +162,9 @@ def _make_vehicles(self) -> None: :return: the ego-vehicle """ road = self.road - ego_vehicle = self.action_type.vehicle_class(road, - road.network.get_lane(("a", "b", 1)).position(30, 0), - speed=30) + ego_vehicle = self.action_type.vehicle_class( + road, road.network.get_lane(("a", "b", 1)).position(30, 0), speed=30 + ) road.vehicles.append(ego_vehicle) other_vehicles_type = utils.class_from_path(self.config["other_vehicles_type"]) @@ -128,7 +175,9 @@ def _make_vehicles(self) -> None: speed += self.np_random.uniform(-1, 1) road.vehicles.append(other_vehicles_type(road, position, speed=speed)) - merging_v = other_vehicles_type(road, road.network.get_lane(("j", "k", 0)).position(110, 0), speed=20) + merging_v = other_vehicles_type( + road, road.network.get_lane(("j", "k", 0)).position(110, 0), speed=20 + ) merging_v.target_speed = 30 road.vehicles.append(merging_v) self.vehicle = ego_vehicle diff --git a/highway_env/envs/parking_env.py b/highway_env/envs/parking_env.py index b24e32278..235875839 100644 --- a/highway_env/envs/parking_env.py +++ b/highway_env/envs/parking_env.py @@ -1,12 +1,15 @@ from abc import abstractmethod from typing import Optional -from gymnasium import Env import numpy as np +from gymnasium import Env from highway_env.envs.common.abstract import AbstractEnv -from highway_env.envs.common.observation import MultiAgentObservation, observation_factory -from highway_env.road.lane import StraightLane, LineType +from highway_env.envs.common.observation import ( + MultiAgentObservation, + observation_factory, +) +from highway_env.road.lane import LineType, StraightLane from highway_env.road.road import Road, RoadNetwork from highway_env.vehicle.graphics import VehicleGraphics from highway_env.vehicle.kinematics import Vehicle @@ -32,7 +35,9 @@ class GoalEnv(Env): """ @abstractmethod - def compute_reward(self, achieved_goal: np.ndarray, desired_goal: np.ndarray, info: dict) -> float: + def compute_reward( + self, achieved_goal: np.ndarray, desired_goal: np.ndarray, info: dict + ) -> float: """Compute the step reward. This externalizes the reward function and makes it dependent on a desired goal and the one that was achieved. If you wish to include additional rewards that are independent of the goal, you can include the necessary values @@ -63,12 +68,14 @@ class ParkingEnv(AbstractEnv, GoalEnv): # For parking env with GrayscaleObservation, the env need # this PARKING_OBS to calculate the reward and the info. # Bug fixed by Mcfly(https://github.com/McflyWZX) - PARKING_OBS = {"observation": { + PARKING_OBS = { + "observation": { "type": "KinematicsGoal", - "features": ['x', 'y', 'vx', 'vy', 'cos_h', 'sin_h'], + "features": ["x", "y", "vx", "vy", "cos_h", "sin_h"], "scales": [100, 100, 5, 5, 1, 1], - "normalize": False - }} + "normalize": False, + } + } def __init__(self, config: dict = None, render_mode: Optional[str] = None) -> None: super().__init__(config, render_mode) @@ -77,31 +84,31 @@ def __init__(self, config: dict = None, render_mode: Optional[str] = None) -> No @classmethod def default_config(cls) -> dict: config = super().default_config() - config.update({ - "observation": { - "type": "KinematicsGoal", - "features": ['x', 'y', 'vx', 'vy', 'cos_h', 'sin_h'], - "scales": [100, 100, 5, 5, 1, 1], - "normalize": False - }, - "action": { - "type": "ContinuousAction" - }, - "reward_weights": [1, 0.3, 0, 0, 0.02, 0.02], - "success_goal_reward": 0.12, - "collision_reward": -5, - "steering_range": np.deg2rad(45), - "simulation_frequency": 15, - "policy_frequency": 5, - "duration": 100, - "screen_width": 600, - "screen_height": 300, - "centering_position": [0.5, 0.5], - "scaling": 7, - "controlled_vehicles": 1, - "vehicles_count": 0, - "add_walls": True - }) + config.update( + { + "observation": { + "type": "KinematicsGoal", + "features": ["x", "y", "vx", "vy", "cos_h", "sin_h"], + "scales": [100, 100, 5, 5, 1, 1], + "normalize": False, + }, + "action": {"type": "ContinuousAction"}, + "reward_weights": [1, 0.3, 0, 0, 0.02, 0.02], + "success_goal_reward": 0.12, + "collision_reward": -5, + "steering_range": np.deg2rad(45), + "simulation_frequency": 15, + "policy_frequency": 5, + "duration": 100, + "screen_width": 600, + "screen_height": 300, + "centering_position": [0.5, 0.5], + "scaling": 7, + "controlled_vehicles": 1, + "vehicles_count": 0, + "add_walls": True, + } + ) return config def define_spaces(self) -> None: @@ -109,15 +116,20 @@ def define_spaces(self) -> None: Set the types and spaces of observation and action from config. """ super().define_spaces() - self.observation_type_parking = observation_factory(self, self.PARKING_OBS["observation"]) + self.observation_type_parking = observation_factory( + self, self.PARKING_OBS["observation"] + ) def _info(self, obs, action) -> dict: info = super(ParkingEnv, self)._info(obs, action) if isinstance(self.observation_type, MultiAgentObservation): - success = tuple(self._is_success(agent_obs['achieved_goal'], agent_obs['desired_goal']) for agent_obs in obs) + success = tuple( + self._is_success(agent_obs["achieved_goal"], agent_obs["desired_goal"]) + for agent_obs in obs + ) else: obs = self.observation_type_parking.observe() - success = self._is_success(obs['achieved_goal'], obs['desired_goal']) + success = self._is_success(obs["achieved_goal"], obs["desired_goal"]) info.update({"is_success": success}) return info @@ -139,12 +151,26 @@ def _create_road(self, spots: int = 14) -> None: length = 8 for k in range(spots): x = (k + 1 - 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, - record_history=self.config["show_trajectories"]) + 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, + record_history=self.config["show_trajectories"], + ) def _create_vehicles(self) -> None: """Create some new random vehicles of a given type, and add them on the road.""" @@ -153,7 +179,9 @@ def _create_vehicles(self) -> None: # Controlled vehicles self.controlled_vehicles = [] for i in range(self.config["controlled_vehicles"]): - vehicle = self.action_type.vehicle_class(self.road, [i*20, 0], 2*np.pi*self.np_random.uniform(), 0) + vehicle = self.action_type.vehicle_class( + self.road, [i * 20, 0], 2 * np.pi * self.np_random.uniform(), 0 + ) vehicle.color = VehicleGraphics.EGO_COLOR self.road.vehicles.append(vehicle) self.controlled_vehicles.append(vehicle) @@ -162,7 +190,9 @@ def _create_vehicles(self) -> None: # Goal lane_index = empty_spots[self.np_random.choice(np.arange(len(empty_spots)))] lane = self.road.network.get_lane(lane_index) - self.goal = Landmark(self.road, lane.position(lane.length/2, 0), heading=lane.heading) + self.goal = Landmark( + self.road, lane.position(lane.length / 2, 0), heading=lane.heading + ) self.road.objects.append(self.goal) empty_spots.remove(lane_index) @@ -176,7 +206,7 @@ def _create_vehicles(self) -> None: empty_spots.remove(lane_index) # Walls - if self.config['add_walls']: + if self.config["add_walls"]: width, height = 70, 42 for y in [-height / 2, height / 2]: obstacle = Obstacle(self.road, [0, y]) @@ -189,7 +219,13 @@ def _create_vehicles(self) -> None: obstacle.diagonal = np.sqrt(obstacle.LENGTH**2 + obstacle.WIDTH**2) self.road.objects.append(obstacle) - def compute_reward(self, achieved_goal: np.ndarray, desired_goal: np.ndarray, info: dict, p: float = 0.5) -> float: + def compute_reward( + self, + achieved_goal: np.ndarray, + desired_goal: np.ndarray, + info: dict, + p: float = 0.5, + ) -> float: """ Proximity to the goal is rewarded @@ -201,24 +237,43 @@ def compute_reward(self, achieved_goal: np.ndarray, desired_goal: np.ndarray, in :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(np.abs(achieved_goal - desired_goal), np.array(self.config["reward_weights"])), p) + return -np.power( + np.dot( + np.abs(achieved_goal - desired_goal), + np.array(self.config["reward_weights"]), + ), + p, + ) def _reward(self, action: np.ndarray) -> float: obs = self.observation_type_parking.observe() obs = obs if isinstance(obs, tuple) else (obs,) - reward = sum(self.compute_reward(agent_obs['achieved_goal'], agent_obs['desired_goal'], {}) for agent_obs in obs) - reward += self.config['collision_reward'] * sum(v.crashed for v in self.controlled_vehicles) + reward = sum( + self.compute_reward( + agent_obs["achieved_goal"], agent_obs["desired_goal"], {} + ) + for agent_obs in obs + ) + reward += self.config["collision_reward"] * sum( + v.crashed for v in self.controlled_vehicles + ) return reward def _is_success(self, achieved_goal: np.ndarray, desired_goal: np.ndarray) -> bool: - return self.compute_reward(achieved_goal, desired_goal, {}) > -self.config["success_goal_reward"] + return ( + self.compute_reward(achieved_goal, desired_goal, {}) + > -self.config["success_goal_reward"] + ) def _is_terminated(self) -> bool: """The episode is over if the ego vehicle crashed or the goal is reached or time is over.""" crashed = any(vehicle.crashed for vehicle in self.controlled_vehicles) obs = self.observation_type_parking.observe() obs = obs if isinstance(obs, tuple) else (obs,) - success = all(self._is_success(agent_obs['achieved_goal'], agent_obs['desired_goal']) for agent_obs in obs) + success = all( + self._is_success(agent_obs["achieved_goal"], agent_obs["desired_goal"]) + for agent_obs in obs + ) return bool(crashed or success) def _is_truncated(self) -> bool: diff --git a/highway_env/envs/racetrack_env.py b/highway_env/envs/racetrack_env.py index af191acd4..b8906dd5f 100644 --- a/highway_env/envs/racetrack_env.py +++ b/highway_env/envs/racetrack_env.py @@ -1,11 +1,10 @@ -from itertools import repeat, product -from typing import Tuple, Dict, Text +from typing import Dict, Text import numpy as np from highway_env import utils from highway_env.envs.common.abstract import AbstractEnv -from highway_env.road.lane import LineType, StraightLane, CircularLane, SineLane +from highway_env.road.lane import CircularLane, LineType, StraightLane from highway_env.road.road import Road, RoadNetwork from highway_env.vehicle.behavior import IDMVehicle @@ -25,39 +24,43 @@ class RacetrackEnv(AbstractEnv): @classmethod def default_config(cls) -> dict: config = super().default_config() - config.update({ - "observation": { - "type": "OccupancyGrid", - "features": ['presence', 'on_road'], - "grid_size": [[-18, 18], [-18, 18]], - "grid_step": [3, 3], - "as_image": False, - "align_to_vehicle_axes": True - }, - "action": { - "type": "ContinuousAction", - "longitudinal": False, - "lateral": True, - "target_speeds": [0, 5, 10] - }, - "simulation_frequency": 15, - "policy_frequency": 5, - "duration": 300, - "collision_reward": -1, - "lane_centering_cost": 4, - "lane_centering_reward": 1, - "action_reward": -0.3, - "controlled_vehicles": 1, - "other_vehicles": 1, - "screen_width": 600, - "screen_height": 600, - "centering_position": [0.5, 0.5], - }) + config.update( + { + "observation": { + "type": "OccupancyGrid", + "features": ["presence", "on_road"], + "grid_size": [[-18, 18], [-18, 18]], + "grid_step": [3, 3], + "as_image": False, + "align_to_vehicle_axes": True, + }, + "action": { + "type": "ContinuousAction", + "longitudinal": False, + "lateral": True, + "target_speeds": [0, 5, 10], + }, + "simulation_frequency": 15, + "policy_frequency": 5, + "duration": 300, + "collision_reward": -1, + "lane_centering_cost": 4, + "lane_centering_reward": 1, + "action_reward": -0.3, + "controlled_vehicles": 1, + "other_vehicles": 1, + "screen_width": 600, + "screen_height": 600, + "centering_position": [0.5, 0.5], + } + ) return config def _reward(self, action: np.ndarray) -> float: rewards = self._rewards(action) - reward = sum(self.config.get(name, 0) * reward for name, reward in rewards.items()) + reward = sum( + self.config.get(name, 0) * reward for name, reward in rewards.items() + ) reward = utils.lmap(reward, [self.config["collision_reward"], 1], [0, 1]) reward *= rewards["on_road_reward"] return reward @@ -65,7 +68,8 @@ def _reward(self, action: np.ndarray) -> float: def _rewards(self, action: np.ndarray) -> Dict[Text, float]: _, lateral = self.vehicle.lane.local_coordinates(self.vehicle.position) return { - "lane_centering_reward": 1/(1+self.config["lane_centering_cost"]*lateral**2), + "lane_centering_reward": 1 + / (1 + self.config["lane_centering_cost"] * lateral**2), "action_reward": np.linalg.norm(action), "collision_reward": self.vehicle.crashed, "on_road_reward": self.vehicle.on_road, @@ -88,98 +92,270 @@ def _make_road(self) -> None: speedlimits = [None, 10, 10, 10, 10, 10, 10, 10, 10] # Initialise First Lane - lane = StraightLane([42, 0], [100, 0], line_types=(LineType.CONTINUOUS, LineType.STRIPED), width=5, speed_limit=speedlimits[1]) + lane = StraightLane( + [42, 0], + [100, 0], + line_types=(LineType.CONTINUOUS, LineType.STRIPED), + width=5, + speed_limit=speedlimits[1], + ) self.lane = lane # Add Lanes to Road Network - Straight Section net.add_lane("a", "b", lane) - net.add_lane("a", "b", StraightLane([42, 5], [100, 5], line_types=(LineType.STRIPED, LineType.CONTINUOUS), width=5, speed_limit=speedlimits[1])) + net.add_lane( + "a", + "b", + StraightLane( + [42, 5], + [100, 5], + line_types=(LineType.STRIPED, LineType.CONTINUOUS), + width=5, + speed_limit=speedlimits[1], + ), + ) # 2 - Circular Arc #1 center1 = [100, -20] radii1 = 20 - net.add_lane("b", "c", - CircularLane(center1, radii1, np.deg2rad(90), np.deg2rad(-1), width=5, - clockwise=False, line_types=(LineType.CONTINUOUS, LineType.NONE), - speed_limit=speedlimits[2])) - net.add_lane("b", "c", - CircularLane(center1, radii1+5, np.deg2rad(90), np.deg2rad(-1), width=5, - clockwise=False, line_types=(LineType.STRIPED, LineType.CONTINUOUS), - speed_limit=speedlimits[2])) + net.add_lane( + "b", + "c", + CircularLane( + center1, + radii1, + np.deg2rad(90), + np.deg2rad(-1), + width=5, + clockwise=False, + line_types=(LineType.CONTINUOUS, LineType.NONE), + speed_limit=speedlimits[2], + ), + ) + net.add_lane( + "b", + "c", + CircularLane( + center1, + radii1 + 5, + np.deg2rad(90), + np.deg2rad(-1), + width=5, + clockwise=False, + line_types=(LineType.STRIPED, LineType.CONTINUOUS), + speed_limit=speedlimits[2], + ), + ) # 3 - Vertical Straight - net.add_lane("c", "d", StraightLane([120, -20], [120, -30], - line_types=(LineType.CONTINUOUS, LineType.NONE), width=5, - speed_limit=speedlimits[3])) - net.add_lane("c", "d", StraightLane([125, -20], [125, -30], - line_types=(LineType.STRIPED, LineType.CONTINUOUS), width=5, - speed_limit=speedlimits[3])) + net.add_lane( + "c", + "d", + StraightLane( + [120, -20], + [120, -30], + line_types=(LineType.CONTINUOUS, LineType.NONE), + width=5, + speed_limit=speedlimits[3], + ), + ) + net.add_lane( + "c", + "d", + StraightLane( + [125, -20], + [125, -30], + line_types=(LineType.STRIPED, LineType.CONTINUOUS), + width=5, + speed_limit=speedlimits[3], + ), + ) # 4 - Circular Arc #2 center2 = [105, -30] radii2 = 15 - net.add_lane("d", "e", - CircularLane(center2, radii2, np.deg2rad(0), np.deg2rad(-181), width=5, - clockwise=False, line_types=(LineType.CONTINUOUS, LineType.NONE), - speed_limit=speedlimits[4])) - net.add_lane("d", "e", - CircularLane(center2, radii2+5, np.deg2rad(0), np.deg2rad(-181), width=5, - clockwise=False, line_types=(LineType.STRIPED, LineType.CONTINUOUS), - speed_limit=speedlimits[4])) + net.add_lane( + "d", + "e", + CircularLane( + center2, + radii2, + np.deg2rad(0), + np.deg2rad(-181), + width=5, + clockwise=False, + line_types=(LineType.CONTINUOUS, LineType.NONE), + speed_limit=speedlimits[4], + ), + ) + net.add_lane( + "d", + "e", + CircularLane( + center2, + radii2 + 5, + np.deg2rad(0), + np.deg2rad(-181), + width=5, + clockwise=False, + line_types=(LineType.STRIPED, LineType.CONTINUOUS), + speed_limit=speedlimits[4], + ), + ) # 5 - Circular Arc #3 center3 = [70, -30] radii3 = 15 - net.add_lane("e", "f", - CircularLane(center3, radii3+5, np.deg2rad(0), np.deg2rad(136), width=5, - clockwise=True, line_types=(LineType.CONTINUOUS, LineType.STRIPED), - speed_limit=speedlimits[5])) - net.add_lane("e", "f", - CircularLane(center3, radii3, np.deg2rad(0), np.deg2rad(137), width=5, - clockwise=True, line_types=(LineType.NONE, LineType.CONTINUOUS), - speed_limit=speedlimits[5])) + net.add_lane( + "e", + "f", + CircularLane( + center3, + radii3 + 5, + np.deg2rad(0), + np.deg2rad(136), + width=5, + clockwise=True, + line_types=(LineType.CONTINUOUS, LineType.STRIPED), + speed_limit=speedlimits[5], + ), + ) + net.add_lane( + "e", + "f", + CircularLane( + center3, + radii3, + np.deg2rad(0), + np.deg2rad(137), + width=5, + clockwise=True, + line_types=(LineType.NONE, LineType.CONTINUOUS), + speed_limit=speedlimits[5], + ), + ) # 6 - Slant - net.add_lane("f", "g", StraightLane([55.7, -15.7], [35.7, -35.7], - line_types=(LineType.CONTINUOUS, LineType.NONE), width=5, - speed_limit=speedlimits[6])) - net.add_lane("f", "g", StraightLane([59.3934, -19.2], [39.3934, -39.2], - line_types=(LineType.STRIPED, LineType.CONTINUOUS), width=5, - speed_limit=speedlimits[6])) + net.add_lane( + "f", + "g", + StraightLane( + [55.7, -15.7], + [35.7, -35.7], + line_types=(LineType.CONTINUOUS, LineType.NONE), + width=5, + speed_limit=speedlimits[6], + ), + ) + net.add_lane( + "f", + "g", + StraightLane( + [59.3934, -19.2], + [39.3934, -39.2], + line_types=(LineType.STRIPED, LineType.CONTINUOUS), + width=5, + speed_limit=speedlimits[6], + ), + ) # 7 - Circular Arc #4 - Bugs out when arc is too large, hence written in 2 sections center4 = [18.1, -18.1] radii4 = 25 - net.add_lane("g", "h", - CircularLane(center4, radii4, np.deg2rad(315), np.deg2rad(170), width=5, - clockwise=False, line_types=(LineType.CONTINUOUS, LineType.NONE), - speed_limit=speedlimits[7])) - net.add_lane("g", "h", - CircularLane(center4, radii4+5, np.deg2rad(315), np.deg2rad(165), width=5, - clockwise=False, line_types=(LineType.STRIPED, LineType.CONTINUOUS), - speed_limit=speedlimits[7])) - net.add_lane("h", "i", - CircularLane(center4, radii4, np.deg2rad(170), np.deg2rad(56), width=5, - clockwise=False, line_types=(LineType.CONTINUOUS, LineType.NONE), - speed_limit=speedlimits[7])) - net.add_lane("h", "i", - CircularLane(center4, radii4+5, np.deg2rad(170), np.deg2rad(58), width=5, - clockwise=False, line_types=(LineType.STRIPED, LineType.CONTINUOUS), - speed_limit=speedlimits[7])) + net.add_lane( + "g", + "h", + CircularLane( + center4, + radii4, + np.deg2rad(315), + np.deg2rad(170), + width=5, + clockwise=False, + line_types=(LineType.CONTINUOUS, LineType.NONE), + speed_limit=speedlimits[7], + ), + ) + net.add_lane( + "g", + "h", + CircularLane( + center4, + radii4 + 5, + np.deg2rad(315), + np.deg2rad(165), + width=5, + clockwise=False, + line_types=(LineType.STRIPED, LineType.CONTINUOUS), + speed_limit=speedlimits[7], + ), + ) + net.add_lane( + "h", + "i", + CircularLane( + center4, + radii4, + np.deg2rad(170), + np.deg2rad(56), + width=5, + clockwise=False, + line_types=(LineType.CONTINUOUS, LineType.NONE), + speed_limit=speedlimits[7], + ), + ) + net.add_lane( + "h", + "i", + CircularLane( + center4, + radii4 + 5, + np.deg2rad(170), + np.deg2rad(58), + width=5, + clockwise=False, + line_types=(LineType.STRIPED, LineType.CONTINUOUS), + speed_limit=speedlimits[7], + ), + ) # 8 - Circular Arc #5 - Reconnects to Start center5 = [43.2, 23.4] radii5 = 18.5 - net.add_lane("i", "a", - CircularLane(center5, radii5+5, np.deg2rad(240), np.deg2rad(270), width=5, - clockwise=True, line_types=(LineType.CONTINUOUS, LineType.STRIPED), - speed_limit=speedlimits[8])) - net.add_lane("i", "a", - CircularLane(center5, radii5, np.deg2rad(238), np.deg2rad(268), width=5, - clockwise=True, line_types=(LineType.NONE, LineType.CONTINUOUS), - speed_limit=speedlimits[8])) - - road = Road(network=net, np_random=self.np_random, record_history=self.config["show_trajectories"]) + net.add_lane( + "i", + "a", + CircularLane( + center5, + radii5 + 5, + np.deg2rad(240), + np.deg2rad(270), + width=5, + clockwise=True, + line_types=(LineType.CONTINUOUS, LineType.STRIPED), + speed_limit=speedlimits[8], + ), + ) + net.add_lane( + "i", + "a", + CircularLane( + center5, + radii5, + np.deg2rad(238), + np.deg2rad(268), + width=5, + clockwise=True, + line_types=(LineType.NONE, LineType.CONTINUOUS), + speed_limit=speedlimits[8], + ), + ) + + road = Road( + network=net, + np_random=self.np_random, + record_history=self.config["show_trajectories"], + ) self.road = road def _make_vehicles(self) -> None: @@ -191,32 +367,40 @@ def _make_vehicles(self) -> None: # Controlled vehicles self.controlled_vehicles = [] for i in range(self.config["controlled_vehicles"]): - lane_index = ("a", "b", rng.integers(2)) if i == 0 else \ - self.road.network.random_lane_index(rng) - controlled_vehicle = self.action_type.vehicle_class.make_on_lane(self.road, lane_index, speed=None, - longitudinal=rng.uniform(20, 50)) + lane_index = ( + ("a", "b", rng.integers(2)) + if i == 0 + else self.road.network.random_lane_index(rng) + ) + controlled_vehicle = self.action_type.vehicle_class.make_on_lane( + self.road, lane_index, speed=None, longitudinal=rng.uniform(20, 50) + ) self.controlled_vehicles.append(controlled_vehicle) self.road.vehicles.append(controlled_vehicle) # Front vehicle - vehicle = IDMVehicle.make_on_lane(self.road, ("b", "c", lane_index[-1]), - longitudinal=rng.uniform( - low=0, - high=self.road.network.get_lane(("b", "c", 0)).length - ), - speed=6+rng.uniform(high=3)) + vehicle = IDMVehicle.make_on_lane( + self.road, + ("b", "c", lane_index[-1]), + longitudinal=rng.uniform( + low=0, high=self.road.network.get_lane(("b", "c", 0)).length + ), + speed=6 + rng.uniform(high=3), + ) self.road.vehicles.append(vehicle) # Other vehicles for i in range(rng.integers(self.config["other_vehicles"])): random_lane_index = self.road.network.random_lane_index(rng) - vehicle = IDMVehicle.make_on_lane(self.road, random_lane_index, - longitudinal=rng.uniform( - low=0, - high=self.road.network.get_lane(random_lane_index).length - ), - speed=6+rng.uniform(high=3)) + vehicle = IDMVehicle.make_on_lane( + self.road, + random_lane_index, + longitudinal=rng.uniform( + low=0, high=self.road.network.get_lane(random_lane_index).length + ), + speed=6 + rng.uniform(high=3), + ) # Prevent early collisions for v in self.road.vehicles: if np.linalg.norm(vehicle.position - v.position) < 20: diff --git a/highway_env/envs/roundabout_env.py b/highway_env/envs/roundabout_env.py index d1b5f4ae0..cc91a570d 100644 --- a/highway_env/envs/roundabout_env.py +++ b/highway_env/envs/roundabout_env.py @@ -1,57 +1,66 @@ -from typing import Tuple, Dict, Text +from typing import Dict, Text import numpy as np from highway_env import utils from highway_env.envs.common.abstract import AbstractEnv -from highway_env.road.lane import LineType, StraightLane, CircularLane, SineLane +from highway_env.road.lane import CircularLane, LineType, SineLane, StraightLane from highway_env.road.road import Road, RoadNetwork from highway_env.vehicle.controller import MDPVehicle class RoundaboutEnv(AbstractEnv): - @classmethod def default_config(cls) -> dict: config = super().default_config() - config.update({ - "observation": { - "type": "Kinematics", - "absolute": True, - "features_range": {"x": [-100, 100], "y": [-100, 100], "vx": [-15, 15], "vy": [-15, 15]}, - }, - "action": { - "type": "DiscreteMetaAction", - "target_speeds": [0, 8, 16] - }, - "incoming_vehicle_destination": None, - "collision_reward": -1, - "high_speed_reward": 0.2, - "right_lane_reward": 0, - "lane_change_reward": -0.05, - "screen_width": 600, - "screen_height": 600, - "centering_position": [0.5, 0.6], - "duration": 11, - "normalize_reward": True - }) + config.update( + { + "observation": { + "type": "Kinematics", + "absolute": True, + "features_range": { + "x": [-100, 100], + "y": [-100, 100], + "vx": [-15, 15], + "vy": [-15, 15], + }, + }, + "action": {"type": "DiscreteMetaAction", "target_speeds": [0, 8, 16]}, + "incoming_vehicle_destination": None, + "collision_reward": -1, + "high_speed_reward": 0.2, + "right_lane_reward": 0, + "lane_change_reward": -0.05, + "screen_width": 600, + "screen_height": 600, + "centering_position": [0.5, 0.6], + "duration": 11, + "normalize_reward": True, + } + ) return config def _reward(self, action: int) -> float: rewards = self._rewards(action) - reward = sum(self.config.get(name, 0) * reward for name, reward in rewards.items()) + reward = sum( + self.config.get(name, 0) * reward for name, reward in rewards.items() + ) if self.config["normalize_reward"]: - reward = utils.lmap(reward, [self.config["collision_reward"], self.config["high_speed_reward"]], [0, 1]) + reward = utils.lmap( + reward, + [self.config["collision_reward"], self.config["high_speed_reward"]], + [0, 1], + ) reward *= rewards["on_road_reward"] return reward def _rewards(self, action: int) -> Dict[Text, float]: return { "collision_reward": self.vehicle.crashed, - "high_speed_reward": - MDPVehicle.get_speed_index(self.vehicle) / (MDPVehicle.DEFAULT_TARGET_SPEEDS.size - 1), + "high_speed_reward": MDPVehicle.get_speed_index(self.vehicle) + / (MDPVehicle.DEFAULT_TARGET_SPEEDS.size - 1), "lane_change_reward": action in [0, 2], - "on_road_reward": self.vehicle.on_road + "on_road_reward": self.vehicle.on_road, } def _is_terminated(self) -> bool: @@ -71,64 +80,244 @@ def _make_road(self) -> None: alpha = 24 # [deg] net = RoadNetwork() - radii = [radius, radius+4] + 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], np.deg2rad(90 - alpha), np.deg2rad(alpha), - clockwise=False, line_types=line[lane])) - net.add_lane("ex", "ee", - CircularLane(center, radii[lane], np.deg2rad(alpha), np.deg2rad(-alpha), - clockwise=False, line_types=line[lane])) - net.add_lane("ee", "nx", - CircularLane(center, radii[lane], np.deg2rad(-alpha), np.deg2rad(-90 + alpha), - clockwise=False, line_types=line[lane])) - net.add_lane("nx", "ne", - CircularLane(center, radii[lane], np.deg2rad(-90 + alpha), np.deg2rad(-90 - alpha), - clockwise=False, line_types=line[lane])) - net.add_lane("ne", "wx", - CircularLane(center, radii[lane], np.deg2rad(-90 - alpha), np.deg2rad(-180 + alpha), - clockwise=False, line_types=line[lane])) - net.add_lane("wx", "we", - CircularLane(center, radii[lane], np.deg2rad(-180 + alpha), np.deg2rad(-180 - alpha), - clockwise=False, line_types=line[lane])) - net.add_lane("we", "sx", - CircularLane(center, radii[lane], np.deg2rad(180 - alpha), np.deg2rad(90 + alpha), - clockwise=False, line_types=line[lane])) - net.add_lane("sx", "se", - CircularLane(center, radii[lane], np.deg2rad(90 + alpha), np.deg2rad(90 - alpha), - clockwise=False, line_types=line[lane])) + net.add_lane( + "se", + "ex", + CircularLane( + center, + radii[lane], + np.deg2rad(90 - alpha), + np.deg2rad(alpha), + clockwise=False, + line_types=line[lane], + ), + ) + net.add_lane( + "ex", + "ee", + CircularLane( + center, + radii[lane], + np.deg2rad(alpha), + np.deg2rad(-alpha), + clockwise=False, + line_types=line[lane], + ), + ) + net.add_lane( + "ee", + "nx", + CircularLane( + center, + radii[lane], + np.deg2rad(-alpha), + np.deg2rad(-90 + alpha), + clockwise=False, + line_types=line[lane], + ), + ) + net.add_lane( + "nx", + "ne", + CircularLane( + center, + radii[lane], + np.deg2rad(-90 + alpha), + np.deg2rad(-90 - alpha), + clockwise=False, + line_types=line[lane], + ), + ) + net.add_lane( + "ne", + "wx", + CircularLane( + center, + radii[lane], + np.deg2rad(-90 - alpha), + np.deg2rad(-180 + alpha), + clockwise=False, + line_types=line[lane], + ), + ) + net.add_lane( + "wx", + "we", + CircularLane( + center, + radii[lane], + np.deg2rad(-180 + alpha), + np.deg2rad(-180 - alpha), + clockwise=False, + line_types=line[lane], + ), + ) + net.add_lane( + "we", + "sx", + CircularLane( + center, + radii[lane], + np.deg2rad(180 - alpha), + np.deg2rad(90 + alpha), + clockwise=False, + line_types=line[lane], + ), + ) + net.add_lane( + "sx", + "se", + CircularLane( + center, + radii[lane], + np.deg2rad(90 + alpha), + np.deg2rad(90 - alpha), + clockwise=False, + line_types=line[lane], + ), + ) # Access lanes: (r)oad/(s)ine access = 170 # [m] dev = 85 # [m] a = 5 # [m] - delta_st = 0.2*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))) - - net.add_lane("wer", "wes", StraightLane([-access, 2], [-dev / 2, 2], line_types=(s, c))) - net.add_lane("wes", "we", SineLane([-dev / 2, 2+a], [-dev / 2 + delta_st, 2+a], a, w, -np.pi / 2, line_types=(c, c))) - net.add_lane("wx", "wxs", 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("wxs", "wxr", StraightLane([-dev / 2, -2], [-access, -2], line_types=(n, c))) - - road = Road(network=net, np_random=self.np_random, record_history=self.config["show_trajectories"]) + delta_st = 0.2 * 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)) + ) + + net.add_lane( + "wer", "wes", StraightLane([-access, 2], [-dev / 2, 2], line_types=(s, c)) + ) + net.add_lane( + "wes", + "we", + SineLane( + [-dev / 2, 2 + a], + [-dev / 2 + delta_st, 2 + a], + a, + w, + -np.pi / 2, + line_types=(c, c), + ), + ) + net.add_lane( + "wx", + "wxs", + 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( + "wxs", "wxr", StraightLane([-dev / 2, -2], [-access, -2], line_types=(n, c)) + ) + + road = Road( + network=net, + np_random=self.np_random, + record_history=self.config["show_trajectories"], + ) self.road = road def _make_vehicles(self) -> None: @@ -142,10 +331,12 @@ def _make_vehicles(self) -> None: # Ego-vehicle ego_lane = self.road.network.get_lane(("ser", "ses", 0)) - ego_vehicle = self.action_type.vehicle_class(self.road, - ego_lane.position(125, 0), - speed=8, - heading=ego_lane.heading_at(140)) + ego_vehicle = self.action_type.vehicle_class( + self.road, + ego_lane.position(125, 0), + speed=8, + heading=ego_lane.heading_at(140), + ) try: ego_vehicle.plan_route_to("nxs") except AttributeError: @@ -156,10 +347,12 @@ def _make_vehicles(self) -> None: # Incoming vehicle destinations = ["exr", "sxr", "nxr"] other_vehicles_type = utils.class_from_path(self.config["other_vehicles_type"]) - vehicle = other_vehicles_type.make_on_lane(self.road, - ("we", "sx", 1), - longitudinal=5 + self.np_random.normal()*position_deviation, - speed=16 + self.np_random.normal() * speed_deviation) + vehicle = other_vehicles_type.make_on_lane( + self.road, + ("we", "sx", 1), + longitudinal=5 + self.np_random.normal() * position_deviation, + speed=16 + self.np_random.normal() * speed_deviation, + ) if self.config["incoming_vehicle_destination"] is not None: destination = destinations[self.config["incoming_vehicle_destination"]] @@ -171,19 +364,23 @@ def _make_vehicles(self) -> None: # Other vehicles for i in list(range(1, 2)) + list(range(-1, 0)): - vehicle = other_vehicles_type.make_on_lane(self.road, - ("we", "sx", 0), - longitudinal=20*i + self.np_random.normal()*position_deviation, - speed=16 + self.np_random.normal() * speed_deviation) + vehicle = other_vehicles_type.make_on_lane( + self.road, + ("we", "sx", 0), + longitudinal=20 * i + self.np_random.normal() * position_deviation, + speed=16 + self.np_random.normal() * speed_deviation, + ) vehicle.plan_route_to(self.np_random.choice(destinations)) vehicle.randomize_behavior() self.road.vehicles.append(vehicle) # Entering vehicle - vehicle = other_vehicles_type.make_on_lane(self.road, - ("eer", "ees", 0), - longitudinal=50 + self.np_random.normal() * position_deviation, - speed=16 + self.np_random.normal() * speed_deviation) + vehicle = other_vehicles_type.make_on_lane( + self.road, + ("eer", "ees", 0), + longitudinal=50 + self.np_random.normal() * position_deviation, + speed=16 + self.np_random.normal() * speed_deviation, + ) vehicle.plan_route_to(self.np_random.choice(destinations)) vehicle.randomize_behavior() self.road.vehicles.append(vehicle) diff --git a/highway_env/envs/two_way_env.py b/highway_env/envs/two_way_env.py index 08fd3cf0b..3fc042fc3 100644 --- a/highway_env/envs/two_way_env.py +++ b/highway_env/envs/two_way_env.py @@ -6,7 +6,6 @@ from highway_env.envs.common.abstract import AbstractEnv from highway_env.road.lane import LineType, StraightLane from highway_env.road.road import Road, RoadNetwork -from highway_env.vehicle.controller import MDPVehicle class TwoWayEnv(AbstractEnv): @@ -23,19 +22,18 @@ class TwoWayEnv(AbstractEnv): @classmethod def default_config(cls) -> dict: config = super().default_config() - config.update({ - "observation": { - "type": "TimeToCollision", - "horizon": 5 - }, - "action": { - "type": "DiscreteMetaAction", - }, - "collision_reward": 0, - "left_lane_constraint": 1, - "left_lane_reward": 0.2, - "high_speed_reward": 0.8, - }) + config.update( + { + "observation": {"type": "TimeToCollision", "horizon": 5}, + "action": { + "type": "DiscreteMetaAction", + }, + "collision_reward": 0, + "left_lane_constraint": 1, + "left_lane_reward": 0.2, + "high_speed_reward": 0.8, + } + ) return config def _reward(self, action: int) -> float: @@ -44,13 +42,20 @@ def _reward(self, action: int) -> float: :param action: the action performed :return: the reward of the state-action transition """ - return sum(self.config.get(name, 0) * reward for name, reward in self._rewards(action).items()) + return sum( + self.config.get(name, 0) * reward + for name, reward in self._rewards(action).items() + ) def _rewards(self, action: int) -> Dict[Text, float]: neighbours = self.road.network.all_side_lanes(self.vehicle.lane_index) return { - "high_speed_reward": self.vehicle.speed_index / (self.vehicle.target_speeds.size - 1), - "left_lane_reward": (len(neighbours) - 1 - self.vehicle.target_lane_index[2]) / (len(neighbours) - 1), + "high_speed_reward": self.vehicle.speed_index + / (self.vehicle.target_speeds.size - 1), + "left_lane_reward": ( + len(neighbours) - 1 - self.vehicle.target_lane_index[2] + ) + / (len(neighbours) - 1), } def _is_terminated(self) -> bool: @@ -73,14 +78,37 @@ def _make_road(self, length=800): net = RoadNetwork() # Lanes - net.add_lane("a", "b", StraightLane([0, 0], [length, 0], - line_types=(LineType.CONTINUOUS_LINE, LineType.STRIPED))) - net.add_lane("a", "b", StraightLane([0, StraightLane.DEFAULT_WIDTH], [length, StraightLane.DEFAULT_WIDTH], - line_types=(LineType.NONE, LineType.CONTINUOUS_LINE))) - net.add_lane("b", "a", StraightLane([length, 0], [0, 0], - line_types=(LineType.NONE, LineType.NONE))) - - road = Road(network=net, np_random=self.np_random, record_history=self.config["show_trajectories"]) + net.add_lane( + "a", + "b", + StraightLane( + [0, 0], + [length, 0], + line_types=(LineType.CONTINUOUS_LINE, LineType.STRIPED), + ), + ) + net.add_lane( + "a", + "b", + StraightLane( + [0, StraightLane.DEFAULT_WIDTH], + [length, StraightLane.DEFAULT_WIDTH], + line_types=(LineType.NONE, LineType.CONTINUOUS_LINE), + ), + ) + net.add_lane( + "b", + "a", + StraightLane( + [length, 0], [0, 0], line_types=(LineType.NONE, LineType.NONE) + ), + ) + + road = Road( + network=net, + np_random=self.np_random, + record_history=self.config["show_trajectories"], + ) self.road = road def _make_vehicles(self) -> None: @@ -90,28 +118,36 @@ def _make_vehicles(self) -> None: :return: the ego-vehicle """ road = self.road - ego_vehicle = self.action_type.vehicle_class(road, - road.network.get_lane(("a", "b", 1)).position(30, 0), - speed=30) + ego_vehicle = self.action_type.vehicle_class( + road, road.network.get_lane(("a", "b", 1)).position(30, 0), speed=30 + ) road.vehicles.append(ego_vehicle) self.vehicle = ego_vehicle vehicles_type = utils.class_from_path(self.config["other_vehicles_type"]) for i in range(3): self.road.vehicles.append( - vehicles_type(road, - position=road.network.get_lane(("a", "b", 1)) - .position(70+40*i + 10*self.np_random.normal(), 0), - heading=road.network.get_lane(("a", "b", 1)).heading_at(70+40*i), - speed=24 + 2*self.np_random.normal(), - enable_lane_change=False) + vehicles_type( + road, + position=road.network.get_lane(("a", "b", 1)).position( + 70 + 40 * i + 10 * self.np_random.normal(), 0 + ), + heading=road.network.get_lane(("a", "b", 1)).heading_at( + 70 + 40 * i + ), + speed=24 + 2 * self.np_random.normal(), + enable_lane_change=False, + ) ) for i in range(2): - v = vehicles_type(road, - position=road.network.get_lane(("b", "a", 0)) - .position(200+100*i + 10*self.np_random.normal(), 0), - heading=road.network.get_lane(("b", "a", 0)).heading_at(200+100*i), - speed=20 + 5*self.np_random.normal(), - enable_lane_change=False) + v = vehicles_type( + road, + position=road.network.get_lane(("b", "a", 0)).position( + 200 + 100 * i + 10 * self.np_random.normal(), 0 + ), + heading=road.network.get_lane(("b", "a", 0)).heading_at(200 + 100 * i), + speed=20 + 5 * self.np_random.normal(), + enable_lane_change=False, + ) v.target_lane_index = ("b", "a", 0) self.road.vehicles.append(v) diff --git a/highway_env/envs/u_turn_env.py b/highway_env/envs/u_turn_env.py index 625ab362e..ba1dcb52b 100644 --- a/highway_env/envs/u_turn_env.py +++ b/highway_env/envs/u_turn_env.py @@ -2,10 +2,9 @@ import numpy as np - from highway_env import utils from highway_env.envs.common.abstract import AbstractEnv -from highway_env.road.lane import LineType, StraightLane, CircularLane +from highway_env.road.lane import CircularLane, LineType, StraightLane from highway_env.road.road import Road, RoadNetwork from highway_env.vehicle.controller import MDPVehicle @@ -20,25 +19,21 @@ class UTurnEnv(AbstractEnv): @classmethod def default_config(cls) -> dict: config = super().default_config() - config.update({ - "observation": { - "type": "TimeToCollision", - "horizon": 16 - }, - "action": { - "type": "DiscreteMetaAction", - "target_speeds": [8, 16, 24] - }, - "screen_width": 789, - "screen_height": 289, - "duration": 10, - "collision_reward": -1.0, # Penalization received for vehicle collision. - "left_lane_reward": 0.1, # Reward received for maintaining left most lane. - "high_speed_reward": 0.4, # Reward received for maintaining cruising speed. - "reward_speed_range": [8, 24], - "normalize_reward": True, - "offroad_terminal": False - }) + config.update( + { + "observation": {"type": "TimeToCollision", "horizon": 16}, + "action": {"type": "DiscreteMetaAction", "target_speeds": [8, 16, 24]}, + "screen_width": 789, + "screen_height": 289, + "duration": 10, + "collision_reward": -1.0, # Penalization received for vehicle collision. + "left_lane_reward": 0.1, # Reward received for maintaining left most lane. + "high_speed_reward": 0.4, # Reward received for maintaining cruising speed. + "reward_speed_range": [8, 24], + "normalize_reward": True, + "offroad_terminal": False, + } + ) return config def _reward(self, action: int) -> float: @@ -48,22 +43,32 @@ def _reward(self, action: int) -> float: :return: the reward of the state-action transition """ rewards = self._rewards(action) - reward = sum(self.config.get(name, 0) * reward for name, reward in rewards.items()) + reward = sum( + self.config.get(name, 0) * reward for name, reward in rewards.items() + ) if self.config["normalize_reward"]: - reward = utils.lmap(reward, [self.config["collision_reward"], - self.config["high_speed_reward"] + self.config["left_lane_reward"]], [0, 1]) + reward = utils.lmap( + reward, + [ + self.config["collision_reward"], + self.config["high_speed_reward"] + self.config["left_lane_reward"], + ], + [0, 1], + ) reward *= rewards["on_road_reward"] return reward def _rewards(self, action: int) -> Dict[Text, float]: neighbours = self.road.network.all_side_lanes(self.vehicle.lane_index) lane = self.vehicle.lane_index[2] - scaled_speed = utils.lmap(self.vehicle.speed, self.config["reward_speed_range"], [0, 1]) + scaled_speed = utils.lmap( + self.vehicle.speed, self.config["reward_speed_range"], [0, 1] + ) return { "collision_reward": self.vehicle.crashed, "left_lane_reward": lane / max(len(neighbours) - 1, 1), "high_speed_reward": np.clip(scaled_speed, 0, 1), - "on_road_reward": self.vehicle.on_road + "on_road_reward": self.vehicle.on_road, } def _is_terminated(self) -> bool: @@ -85,38 +90,87 @@ def _make_road(self, length=128): # Defining upper starting lanes after the U-Turn. # These Lanes are defined from x-coordinate 'length' to 0. - net.add_lane("c", "d", StraightLane([length, StraightLane.DEFAULT_WIDTH], [0, StraightLane.DEFAULT_WIDTH], - line_types=(LineType.CONTINUOUS_LINE, LineType.STRIPED))) - net.add_lane("c", "d", StraightLane([length, 0], [0, 0], - line_types=(LineType.NONE, LineType.CONTINUOUS_LINE))) + net.add_lane( + "c", + "d", + StraightLane( + [length, StraightLane.DEFAULT_WIDTH], + [0, StraightLane.DEFAULT_WIDTH], + line_types=(LineType.CONTINUOUS_LINE, LineType.STRIPED), + ), + ) + net.add_lane( + "c", + "d", + StraightLane( + [length, 0], + [0, 0], + line_types=(LineType.NONE, LineType.CONTINUOUS_LINE), + ), + ) # Defining counter-clockwise circular U-Turn lanes. center = [length, StraightLane.DEFAULT_WIDTH + 20] # [m] radius = 20 # [m] alpha = 0 # [deg] - radii = [radius, radius+StraightLane.DEFAULT_WIDTH] + radii = [radius, radius + StraightLane.DEFAULT_WIDTH] n, c, s = LineType.NONE, LineType.CONTINUOUS, LineType.STRIPED line = [[c, s], [n, c]] for lane in [0, 1]: - net.add_lane("b", "c", - CircularLane(center, radii[lane], np.deg2rad(90 - alpha), np.deg2rad(-90+alpha), - clockwise=False, line_types=line[lane])) - - offset = 2*radius + net.add_lane( + "b", + "c", + CircularLane( + center, + radii[lane], + np.deg2rad(90 - alpha), + np.deg2rad(-90 + alpha), + clockwise=False, + line_types=line[lane], + ), + ) + + offset = 2 * radius # Defining lower starting lanes before the U-Turn. # These Lanes are defined from x-coordinate 0 to 'length'. - net.add_lane("a", "b", StraightLane([0, ((2 * StraightLane.DEFAULT_WIDTH + offset) - StraightLane.DEFAULT_WIDTH)], - [length, ((2 * StraightLane.DEFAULT_WIDTH + offset) - StraightLane.DEFAULT_WIDTH)], - line_types=(LineType.CONTINUOUS_LINE, - LineType.STRIPED))) - net.add_lane("a", "b", StraightLane([0, (2 * StraightLane.DEFAULT_WIDTH + offset)], - [length, (2 * StraightLane.DEFAULT_WIDTH + offset)], - line_types=(LineType.NONE, - LineType.CONTINUOUS_LINE))) - - road = Road(network=net, np_random=self.np_random, record_history=self.config["show_trajectories"]) + net.add_lane( + "a", + "b", + StraightLane( + [ + 0, + ( + (2 * StraightLane.DEFAULT_WIDTH + offset) + - StraightLane.DEFAULT_WIDTH + ), + ], + [ + length, + ( + (2 * StraightLane.DEFAULT_WIDTH + offset) + - StraightLane.DEFAULT_WIDTH + ), + ], + line_types=(LineType.CONTINUOUS_LINE, LineType.STRIPED), + ), + ) + net.add_lane( + "a", + "b", + StraightLane( + [0, (2 * StraightLane.DEFAULT_WIDTH + offset)], + [length, (2 * StraightLane.DEFAULT_WIDTH + offset)], + line_types=(LineType.NONE, LineType.CONTINUOUS_LINE), + ), + ) + + road = Road( + network=net, + np_random=self.np_random, + record_history=self.config["show_trajectories"], + ) self.road = road def _make_vehicles(self) -> None: @@ -132,9 +186,9 @@ def _make_vehicles(self) -> None: speed_deviation = 2 ego_lane = self.road.network.get_lane(("a", "b", 0)) - ego_vehicle = self.action_type.vehicle_class(self.road, - ego_lane.position(0, 0), - speed=16) + ego_vehicle = self.action_type.vehicle_class( + self.road, ego_lane.position(0, 0), speed=16 + ) # Stronger anticipation for the turn ego_vehicle.PURSUIT_TAU = MDPVehicle.TAU_HEADING try: @@ -151,58 +205,67 @@ def _make_vehicles(self) -> None: # vehicle interactions are deemed necessary for the experimentation. # Vehicle 1: Blocking the ego vehicle - vehicle = vehicles_type.make_on_lane(self.road, - ("a", "b", 0), - longitudinal=25 + self.np_random.normal()*position_deviation, - speed=13.5 + self.np_random.normal() * speed_deviation) - vehicle.plan_route_to('d') + vehicle = vehicles_type.make_on_lane( + self.road, + ("a", "b", 0), + longitudinal=25 + self.np_random.normal() * position_deviation, + speed=13.5 + self.np_random.normal() * speed_deviation, + ) + vehicle.plan_route_to("d") vehicle.randomize_behavior() self.road.vehicles.append(vehicle) # Vehicle 2: Forcing risky overtake - vehicle = vehicles_type.make_on_lane(self.road, - ("a", "b", 1), - longitudinal=56 + self.np_random.normal()*position_deviation, - speed=14.5 + self.np_random.normal() * speed_deviation) - vehicle.plan_route_to('d') + vehicle = vehicles_type.make_on_lane( + self.road, + ("a", "b", 1), + longitudinal=56 + self.np_random.normal() * position_deviation, + speed=14.5 + self.np_random.normal() * speed_deviation, + ) + vehicle.plan_route_to("d") # vehicle.randomize_behavior() self.road.vehicles.append(vehicle) # Vehicle 3: Blocking the ego vehicle - vehicle = vehicles_type.make_on_lane(self.road, - ("b", "c", 1), - longitudinal=0.5 + self.np_random.normal()*position_deviation, - speed=4.5 + self.np_random.normal() * speed_deviation) - vehicle.plan_route_to('d') + vehicle = vehicles_type.make_on_lane( + self.road, + ("b", "c", 1), + longitudinal=0.5 + self.np_random.normal() * position_deviation, + speed=4.5 + self.np_random.normal() * speed_deviation, + ) + vehicle.plan_route_to("d") # vehicle.randomize_behavior() self.road.vehicles.append(vehicle) # Vehicle 4: Forcing risky overtake - vehicle = vehicles_type.make_on_lane(self.road, - ("b", "c", 0), - longitudinal=17.5 + self.np_random.normal()*position_deviation, - speed=5.5 + self.np_random.normal() * speed_deviation) - vehicle.plan_route_to('d') + vehicle = vehicles_type.make_on_lane( + self.road, + ("b", "c", 0), + longitudinal=17.5 + self.np_random.normal() * position_deviation, + speed=5.5 + self.np_random.normal() * speed_deviation, + ) + vehicle.plan_route_to("d") # vehicle.randomize_behavior() self.road.vehicles.append(vehicle) # Vehicle 5: Blocking the ego vehicle - vehicle = vehicles_type.make_on_lane(self.road, - ("c", "d", 0), - longitudinal=1 + self.np_random.normal()*position_deviation, - speed=3.5 + self.np_random.normal() * speed_deviation) - vehicle.plan_route_to('d') + vehicle = vehicles_type.make_on_lane( + self.road, + ("c", "d", 0), + longitudinal=1 + self.np_random.normal() * position_deviation, + speed=3.5 + self.np_random.normal() * speed_deviation, + ) + vehicle.plan_route_to("d") # vehicle.randomize_behavior() self.road.vehicles.append(vehicle) # Vehicle 6: Forcing risky overtake - vehicle = vehicles_type.make_on_lane(self.road, - ("c", "d", 1), - longitudinal=30 + self.np_random.normal()*position_deviation, - speed=5.5 + self.np_random.normal() * speed_deviation) - vehicle.plan_route_to('d') + vehicle = vehicles_type.make_on_lane( + self.road, + ("c", "d", 1), + longitudinal=30 + self.np_random.normal() * position_deviation, + speed=5.5 + self.np_random.normal() * speed_deviation, + ) + vehicle.plan_route_to("d") # vehicle.randomize_behavior() self.road.vehicles.append(vehicle) - - - diff --git a/highway_env/interval.py b/highway_env/interval.py index e6bf74673..be1352675 100644 --- a/highway_env/interval.py +++ b/highway_env/interval.py @@ -1,11 +1,11 @@ import itertools -from typing import Tuple, Union, List, Callable +from typing import Callable, List, Tuple, Union import numpy as np from numpy.linalg import LinAlgError from highway_env.road.lane import AbstractLane -from highway_env.utils import Vector, Matrix, Interval +from highway_env.utils import Interval, Matrix, Vector def intervals_product(a: Interval, b: Interval) -> np.ndarray: @@ -19,8 +19,17 @@ def intervals_product(a: Interval, b: Interval) -> np.ndarray: p = lambda x: np.maximum(x, 0) n = lambda x: np.maximum(-x, 0) return np.array( - [np.dot(p(a[0]), p(b[0])) - np.dot(p(a[1]), n(b[0])) - np.dot(n(a[0]), p(b[1])) + np.dot(n(a[1]), n(b[1])), - np.dot(p(a[1]), p(b[1])) - np.dot(p(a[0]), n(b[1])) - np.dot(n(a[1]), p(b[0])) + np.dot(n(a[0]), n(b[0]))]) + [ + np.dot(p(a[0]), p(b[0])) + - np.dot(p(a[1]), n(b[0])) + - np.dot(n(a[0]), p(b[1])) + + np.dot(n(a[1]), n(b[1])), + np.dot(p(a[1]), p(b[1])) + - np.dot(p(a[0]), n(b[1])) + - np.dot(n(a[1]), p(b[0])) + + np.dot(n(a[0]), n(b[0])), + ] + ) def intervals_scaling(a: Interval, b: Interval) -> np.ndarray: @@ -34,8 +43,11 @@ def intervals_scaling(a: Interval, b: Interval) -> np.ndarray: p = lambda x: np.maximum(x, 0) n = lambda x: np.maximum(-x, 0) return np.array( - [np.dot(p(a), b[0]) - np.dot(n(a), b[1]), - np.dot(p(a), b[1]) - np.dot(n(a), b[0])]) + [ + np.dot(p(a), b[0]) - np.dot(n(a), b[1]), + np.dot(p(a), b[1]) - np.dot(n(a), b[0]), + ] + ) def intervals_diff(a: Interval, b: Interval) -> np.ndarray: @@ -74,19 +86,25 @@ def integrator_interval(x: Interval, k: Interval) -> np.ndarray: interval_gain = -k else: interval_gain = -np.array([k[0], k[0]]) - return interval_gain*x # Note: no flip of x, contrary to using intervals_product(k,interval_minus(x)) + return ( + interval_gain * x + ) # Note: no flip of x, contrary to using intervals_product(k,interval_minus(x)) def vector_interval_section(v_i: Interval, direction: Vector) -> np.ndarray: - corners = [[v_i[0, 0], v_i[0, 1]], - [v_i[0, 0], v_i[1, 1]], - [v_i[1, 0], v_i[0, 1]], - [v_i[1, 0], v_i[1, 1]]] + corners = [ + [v_i[0, 0], v_i[0, 1]], + [v_i[0, 0], v_i[1, 1]], + [v_i[1, 0], v_i[0, 1]], + [v_i[1, 0], v_i[1, 1]], + ] corners_dist = [np.dot(corner, direction) for corner in corners] return np.array([min(corners_dist), max(corners_dist)]) -def interval_absolute_to_local(position_i: Interval, lane: AbstractLane) -> Tuple[np.ndarray, np.ndarray]: +def interval_absolute_to_local( + position_i: Interval, lane: AbstractLane +) -> Tuple[np.ndarray, np.ndarray]: """ Converts an interval in absolute x,y coordinates to an interval in local (longiturinal, lateral) coordinates @@ -94,17 +112,23 @@ def interval_absolute_to_local(position_i: Interval, lane: AbstractLane) -> Tupl :param lane: the lane giving the local frame :return: the corresponding local interval """ - position_corners = np.array([[position_i[0, 0], position_i[0, 1]], - [position_i[0, 0], position_i[1, 1]], - [position_i[1, 0], position_i[0, 1]], - [position_i[1, 0], position_i[1, 1]]]) + position_corners = np.array( + [ + [position_i[0, 0], position_i[0, 1]], + [position_i[0, 0], position_i[1, 1]], + [position_i[1, 0], position_i[0, 1]], + [position_i[1, 0], position_i[1, 1]], + ] + ) corners_local = np.array([lane.local_coordinates(c) for c in position_corners]) longitudinal_i = np.array([min(corners_local[:, 0]), max(corners_local[:, 0])]) lateral_i = np.array([min(corners_local[:, 1]), max(corners_local[:, 1])]) return longitudinal_i, lateral_i -def interval_local_to_absolute(longitudinal_i: Interval, lateral_i: Interval, lane: AbstractLane) -> Interval: +def interval_local_to_absolute( + longitudinal_i: Interval, lateral_i: Interval, lane: AbstractLane +) -> Interval: """ Converts an interval in local (longiturinal, lateral) coordinates to an interval in absolute x,y coordinates @@ -113,17 +137,22 @@ def interval_local_to_absolute(longitudinal_i: Interval, lateral_i: Interval, la :param lane: the lane giving the local frame :return: the corresponding absolute interval """ - corners_local = [[longitudinal_i[0], lateral_i[0]], - [longitudinal_i[0], lateral_i[1]], - [longitudinal_i[1], lateral_i[0]], - [longitudinal_i[1], lateral_i[1]]] + corners_local = [ + [longitudinal_i[0], lateral_i[0]], + [longitudinal_i[0], lateral_i[1]], + [longitudinal_i[1], lateral_i[0]], + [longitudinal_i[1], lateral_i[1]], + ] corners_absolute = np.array([lane.position(*c) for c in corners_local]) - position_i = np.array([np.amin(corners_absolute, axis=0), np.amax(corners_absolute, axis=0)]) + position_i = np.array( + [np.amin(corners_absolute, axis=0), np.amax(corners_absolute, axis=0)] + ) return position_i -def polytope(parametrized_f: Callable[[np.ndarray], np.ndarray], params_intervals: np.ndarray) \ - -> Tuple[np.ndarray, List[np.ndarray]]: +def polytope( + parametrized_f: Callable[[np.ndarray], np.ndarray], params_intervals: np.ndarray +) -> Tuple[np.ndarray, List[np.ndarray]]: """ Get a matrix polytope from a parametrized matrix function and parameter box @@ -147,17 +176,19 @@ def is_metzler(matrix: np.ndarray, eps: float = 1e-9) -> bool: class LPV(object): - def __init__(self, - x0: Vector, - a0: Matrix, - da: List[Vector], - b: Matrix = None, - d: Matrix = None, - omega_i: Matrix = None, - u: Vector = None, - k: Matrix = None, - center: Vector = None, - x_i: Matrix = None) -> None: + def __init__( + self, + x0: Vector, + a0: Matrix, + da: List[Vector], + b: Matrix = None, + d: Matrix = None, + omega_i: Matrix = None, + u: Vector = None, + k: Matrix = None, + center: Vector = None, + x_i: Matrix = None, + ) -> None: """ A Linear Parameter-Varying system: @@ -181,8 +212,14 @@ def __init__(self, self.d = np.array(d) if d is not None else np.zeros((*self.x0.shape, 1)) self.omega_i = np.array(omega_i) if omega_i is not None else np.zeros((2, 1)) self.u = np.array(u) if u is not None else np.zeros((1,)) - self.k = np.array(k) if k is not None else np.zeros((self.b.shape[1], self.b.shape[0])) - self.center = np.array(center) if center is not None else np.zeros(self.x0.shape) + self.k = ( + np.array(k) + if k is not None + else np.zeros((self.b.shape[1], self.b.shape[0])) + ) + self.center = ( + np.array(center) if center is not None else np.zeros(self.x0.shape) + ) # Closed-loop dynamics self.a0 += self.b @ self.k @@ -224,11 +261,19 @@ def update_coordinates_frame(self, a0: np.ndarray) -> None: def set_control(self, control: np.ndarray, state: np.ndarray = None) -> None: if state is not None: - control = control - self.k @ state # the Kx part of the control is already present in A0. + control = ( + control - self.k @ state + ) # the Kx part of the control is already present in A0. self.u = control - def change_coordinates(self, value: Union[np.ndarray, List[np.ndarray]], matrix: bool = False, back: bool = False, - interval: bool = False, offset: bool = True) -> Union[np.ndarray, List[np.ndarray]]: + def change_coordinates( + self, + value: Union[np.ndarray, List[np.ndarray]], + matrix: bool = False, + back: bool = False, + interval: bool = False, + offset: bool = True, + ) -> Union[np.ndarray, List[np.ndarray]]: """ Perform a change of coordinate: rotation and centering. @@ -245,12 +290,15 @@ def change_coordinates(self, value: Union[np.ndarray, List[np.ndarray]], matrix: transformation, transformation_inv = self.coordinates if interval: if back: - value = intervals_scaling(transformation, value[:, :, np.newaxis]).squeeze() + \ - offset * np.array([self.center, self.center]) + value = intervals_scaling( + transformation, value[:, :, np.newaxis] + ).squeeze() + offset * np.array([self.center, self.center]) return value else: value = value - offset * np.array([self.center, self.center]) - value = intervals_scaling(transformation_inv, value[:, :, np.newaxis]).squeeze() + value = intervals_scaling( + transformation_inv, value[:, :, np.newaxis] + ).squeeze() return value elif matrix: # Matrix if back: @@ -286,11 +334,22 @@ def step_naive_predictor(self, x_i: Interval, dt: float) -> np.ndarray: :param dt: time step :return: state interval at time t+dt """ - a0, da, d, omega_i, b, u = self.a0, self.da, self.d, self.omega_i, self.b, self.u + a0, da, d, omega_i, b, u = ( + self.a0, + self.da, + self.d, + self.omega_i, + self.b, + self.u, + ) a_i = a0 + sum(intervals_product([0, 1], [da_i, da_i]) for da_i in da) bu = (b @ u).squeeze(-1) - dx_i = intervals_product(a_i, x_i) + intervals_product([d, d], omega_i) + np.array([bu, bu]) - return x_i + dx_i*dt + dx_i = ( + intervals_product(a_i, x_i) + + intervals_product([d, d], omega_i) + + np.array([bu, bu]) + ) + return x_i + dx_i * dt def step_interval_predictor(self, x_i: Interval, dt: float) -> np.ndarray: """ @@ -300,14 +359,25 @@ def step_interval_predictor(self, x_i: Interval, dt: float) -> np.ndarray: :param dt: time step :return: state interval at time t+dt """ - a0, da, d, omega_i, b, u = self.a0, self.da, self.d, self.omega_i, self.b, self.u + a0, da, d, omega_i, b, u = ( + self.a0, + self.da, + self.d, + self.omega_i, + self.b, + self.u, + ) p = lambda x: np.maximum(x, 0) n = lambda x: np.maximum(-x, 0) da_p = sum(p(da_i) for da_i in da) da_n = sum(n(da_i) for da_i in da) x_m, x_M = x_i[0, :, np.newaxis], x_i[1, :, np.newaxis] o_m, o_M = omega_i[0, :, np.newaxis], omega_i[1, :, np.newaxis] - dx_m = a0 @ x_m - da_p @ n(x_m) - da_n @ p(x_M) + p(d) @ o_m - n(d) @ o_M + b @ u - dx_M = a0 @ x_M + da_p @ p(x_M) + da_n @ n(x_m) + p(d) @ o_M - n(d) @ o_m + b @ u + dx_m = ( + a0 @ x_m - da_p @ n(x_m) - da_n @ p(x_M) + p(d) @ o_m - n(d) @ o_M + b @ u + ) + dx_M = ( + a0 @ x_M + da_p @ p(x_M) + da_n @ n(x_m) + p(d) @ o_M - n(d) @ o_m + b @ u + ) dx_i = np.array([dx_m.squeeze(axis=-1), dx_M.squeeze(axis=-1)]) return x_i + dx_i * dt diff --git a/highway_env/road/graphics.py b/highway_env/road/graphics.py index 23dd3766c..06f7ada7e 100644 --- a/highway_env/road/graphics.py +++ b/highway_env/road/graphics.py @@ -1,13 +1,13 @@ -from typing import List, Tuple, Union, TYPE_CHECKING +from typing import TYPE_CHECKING, List, Tuple, Union import numpy as np import pygame -from highway_env.road.lane import LineType, AbstractLane +from highway_env.road.lane import AbstractLane, LineType from highway_env.road.road import Road from highway_env.utils import Vector from highway_env.vehicle.graphics import VehicleGraphics -from highway_env.vehicle.objects import Obstacle, Landmark +from highway_env.vehicle.objects import Landmark, Obstacle if TYPE_CHECKING: from highway_env.vehicle.objects import RoadObject @@ -29,7 +29,9 @@ class WorldSurface(pygame.Surface): SCALING_FACTOR = 1.3 MOVING_FACTOR = 0.1 - def __init__(self, size: Tuple[int, int], flags: object, surf: pygame.SurfaceType) -> None: + def __init__( + self, size: Tuple[int, int], flags: object, surf: pygame.SurfaceType + ) -> None: super().__init__(size, flags, surf) self.origin = np.array([0, 0]) self.scaling = self.INITIAL_SCALING @@ -71,7 +73,10 @@ def is_visible(self, vec: PositionType, margin: int = 50) -> bool: :return: whether the position is visible """ x, y = self.vec2pix(vec) - return -margin < x < self.get_width() + margin and -margin < y < self.get_height() + margin + return ( + -margin < x < self.get_width() + margin + and -margin < y < self.get_height() + margin + ) def move_display_window_to(self, position: PositionType) -> None: """ @@ -80,8 +85,11 @@ def move_display_window_to(self, position: PositionType) -> None: :param position: a world position [m] """ self.origin = position - np.array( - [self.centering_position[0] * self.get_width() / self.scaling, - self.centering_position[1] * self.get_height() / self.scaling]) + [ + self.centering_position[0] * self.get_width() / self.scaling, + self.centering_position[1] * self.get_height() / self.scaling, + ] + ) def handle_event(self, event: pygame.event.EventType) -> None: """ @@ -122,9 +130,15 @@ def display(cls, lane: AbstractLane, surface: WorldSurface) -> None: :param lane: the lane to be displayed :param surface: the pygame surface """ - stripes_count = int(2 * (surface.get_height() + surface.get_width()) / (cls.STRIPE_SPACING * surface.scaling)) + stripes_count = int( + 2 + * (surface.get_height() + surface.get_width()) + / (cls.STRIPE_SPACING * surface.scaling) + ) s_origin, _ = lane.local_coordinates(surface.origin) - s0 = (int(s_origin) // cls.STRIPE_SPACING - stripes_count // 2) * cls.STRIPE_SPACING + s0 = ( + int(s_origin) // cls.STRIPE_SPACING - stripes_count // 2 + ) * cls.STRIPE_SPACING for side in range(2): if lane.line_types[side] == LineType.STRIPED: cls.striped_line(lane, surface, stripes_count, s0, side) @@ -134,8 +148,14 @@ def display(cls, lane: AbstractLane, surface: WorldSurface) -> None: cls.continuous_line(lane, surface, stripes_count, s0, side) @classmethod - def striped_line(cls, lane: AbstractLane, surface: WorldSurface, stripes_count: int, longitudinal: float, - side: int) -> None: + def striped_line( + cls, + lane: AbstractLane, + surface: WorldSurface, + stripes_count: int, + longitudinal: float, + side: int, + ) -> None: """ Draw a striped line on one side of a lane, on a surface. @@ -146,13 +166,23 @@ def striped_line(cls, lane: AbstractLane, surface: WorldSurface, stripes_count: :param side: which side of the road to draw [0:left, 1:right] """ starts = longitudinal + np.arange(stripes_count) * cls.STRIPE_SPACING - ends = longitudinal + np.arange(stripes_count) * cls.STRIPE_SPACING + cls.STRIPE_LENGTH + ends = ( + longitudinal + + np.arange(stripes_count) * cls.STRIPE_SPACING + + cls.STRIPE_LENGTH + ) lats = [(side - 0.5) * lane.width_at(s) for s in starts] cls.draw_stripes(lane, surface, starts, ends, lats) @classmethod - def continuous_curve(cls, lane: AbstractLane, surface: WorldSurface, stripes_count: int, - longitudinal: float, side: int) -> None: + def continuous_curve( + cls, + lane: AbstractLane, + surface: WorldSurface, + stripes_count: int, + longitudinal: float, + side: int, + ) -> None: """ Draw a striped line on one side of a lane, on a surface. @@ -163,13 +193,23 @@ def continuous_curve(cls, lane: AbstractLane, surface: WorldSurface, stripes_cou :param side: which side of the road to draw [0:left, 1:right] """ starts = longitudinal + np.arange(stripes_count) * cls.STRIPE_SPACING - ends = longitudinal + np.arange(stripes_count) * cls.STRIPE_SPACING + cls.STRIPE_SPACING + ends = ( + longitudinal + + np.arange(stripes_count) * cls.STRIPE_SPACING + + cls.STRIPE_SPACING + ) lats = [(side - 0.5) * lane.width_at(s) for s in starts] cls.draw_stripes(lane, surface, starts, ends, lats) @classmethod - def continuous_line(cls, lane: AbstractLane, surface: WorldSurface, stripes_count: int, longitudinal: float, - side: int) -> None: + def continuous_line( + cls, + lane: AbstractLane, + surface: WorldSurface, + stripes_count: int, + longitudinal: float, + side: int, + ) -> None: """ Draw a continuous line on one side of a lane, on a surface. @@ -185,8 +225,14 @@ def continuous_line(cls, lane: AbstractLane, surface: WorldSurface, stripes_coun cls.draw_stripes(lane, surface, starts, ends, lats) @classmethod - def draw_stripes(cls, lane: AbstractLane, surface: WorldSurface, - starts: List[float], ends: List[float], lats: List[float]) -> None: + def draw_stripes( + cls, + lane: AbstractLane, + surface: WorldSurface, + starts: List[float], + ends: List[float], + lats: List[float], + ) -> None: """ Draw a set of stripes along a lane. @@ -200,23 +246,43 @@ def draw_stripes(cls, lane: AbstractLane, surface: WorldSurface, ends = np.clip(ends, 0, lane.length) for k, _ in enumerate(starts): if abs(starts[k] - ends[k]) > 0.5 * cls.STRIPE_LENGTH: - pygame.draw.line(surface, surface.WHITE, - (surface.vec2pix(lane.position(starts[k], lats[k]))), - (surface.vec2pix(lane.position(ends[k], lats[k]))), - max(surface.pix(cls.STRIPE_WIDTH), 1)) + pygame.draw.line( + surface, + surface.WHITE, + (surface.vec2pix(lane.position(starts[k], lats[k]))), + (surface.vec2pix(lane.position(ends[k], lats[k]))), + max(surface.pix(cls.STRIPE_WIDTH), 1), + ) @classmethod - def draw_ground(cls, lane: AbstractLane, surface: WorldSurface, color: Tuple[float], width: float, - draw_surface: pygame.Surface = None) -> None: + def draw_ground( + cls, + lane: AbstractLane, + surface: WorldSurface, + color: Tuple[float], + width: float, + draw_surface: pygame.Surface = None, + ) -> None: draw_surface = draw_surface or surface - stripes_count = int(2 * (surface.get_height() + surface.get_width()) / (cls.STRIPE_SPACING * surface.scaling)) + stripes_count = int( + 2 + * (surface.get_height() + surface.get_width()) + / (cls.STRIPE_SPACING * surface.scaling) + ) s_origin, _ = lane.local_coordinates(surface.origin) - s0 = (int(s_origin) // cls.STRIPE_SPACING - stripes_count // 2) * cls.STRIPE_SPACING + s0 = ( + int(s_origin) // cls.STRIPE_SPACING - stripes_count // 2 + ) * cls.STRIPE_SPACING dots = [] for side in range(2): - longis = np.clip(s0 + np.arange(stripes_count) * cls.STRIPE_SPACING, 0, lane.length) + longis = np.clip( + s0 + np.arange(stripes_count) * cls.STRIPE_SPACING, 0, lane.length + ) lats = [2 * (side - 0.5) * width for _ in longis] - new_dots = [surface.vec2pix(lane.position(longi, lat)) for longi, lat in zip(longis, lats)] + new_dots = [ + surface.vec2pix(lane.position(longi, lat)) + for longi, lat in zip(longis, lats) + ] new_dots = reversed(new_dots) if side else new_dots dots.extend(new_dots) pygame.draw.polygon(draw_surface, color, dots, 0) @@ -241,8 +307,12 @@ def display(road: Road, surface: WorldSurface) -> None: LaneGraphics.display(l, surface) @staticmethod - def display_traffic(road: Road, surface: WorldSurface, simulation_frequency: int = 15, offscreen: bool = False) \ - -> None: + def display_traffic( + road: Road, + surface: WorldSurface, + simulation_frequency: int = 15, + offscreen: bool = False, + ) -> None: """ Display the road vehicles on a surface. @@ -253,12 +323,16 @@ def display_traffic(road: Road, surface: WorldSurface, simulation_frequency: int """ if road.record_history: for v in road.vehicles: - VehicleGraphics.display_history(v, surface, simulation=simulation_frequency, offscreen=offscreen) + VehicleGraphics.display_history( + v, surface, simulation=simulation_frequency, offscreen=offscreen + ) for v in road.vehicles: VehicleGraphics.display(v, surface, offscreen=offscreen) @staticmethod - def display_road_objects(road: Road, surface: WorldSurface, offscreen: bool = False) -> None: + def display_road_objects( + road: Road, surface: WorldSurface, offscreen: bool = False + ) -> None: """ Display the road objects on a surface. @@ -282,8 +356,13 @@ class RoadObjectGraphics: DEFAULT_COLOR = YELLOW @classmethod - def display(cls, object_: 'RoadObject', surface: WorldSurface, transparent: bool = False, - offscreen: bool = False): + def display( + cls, + object_: "RoadObject", + surface: WorldSurface, + transparent: bool = False, + offscreen: bool = False, + ): """ Display a road objects on a pygame surface. @@ -295,27 +374,48 @@ def display(cls, object_: 'RoadObject', surface: WorldSurface, transparent: bool :param offscreen: whether the rendering should be done offscreen or not """ o = object_ - s = pygame.Surface((surface.pix(o.LENGTH), surface.pix(o.LENGTH)), pygame.SRCALPHA) # per-pixel alpha - rect = (0, surface.pix(o.LENGTH / 2 - o.WIDTH / 2), surface.pix(o.LENGTH), surface.pix(o.WIDTH)) + s = pygame.Surface( + (surface.pix(o.LENGTH), surface.pix(o.LENGTH)), pygame.SRCALPHA + ) # per-pixel alpha + rect = ( + 0, + surface.pix(o.LENGTH / 2 - o.WIDTH / 2), + surface.pix(o.LENGTH), + surface.pix(o.WIDTH), + ) pygame.draw.rect(s, cls.get_color(o, transparent), rect, 0) pygame.draw.rect(s, cls.BLACK, rect, 1) - if not offscreen: # convert_alpha throws errors in offscreen mode TODO() Explain why + if ( + not offscreen + ): # convert_alpha throws errors in offscreen mode TODO() Explain why s = pygame.Surface.convert_alpha(s) h = o.heading if abs(o.heading) > 2 * np.pi / 180 else 0 # Centered rotation - position = (surface.pos2pix(o.position[0], o.position[1])) + position = surface.pos2pix(o.position[0], o.position[1]) cls.blit_rotate(surface, s, position, np.rad2deg(-h)) @staticmethod - def blit_rotate(surf: pygame.SurfaceType, image: pygame.SurfaceType, pos: Vector, angle: float, - origin_pos: Vector = None, show_rect: bool = False) -> None: + def blit_rotate( + surf: pygame.SurfaceType, + image: pygame.SurfaceType, + pos: Vector, + angle: float, + origin_pos: Vector = None, + show_rect: bool = False, + ) -> None: """Many thanks to https://stackoverflow.com/a/54714144.""" # calculate the axis aligned bounding box of the rotated image w, h = image.get_size() box = [pygame.math.Vector2(p) for p in [(0, 0), (w, 0), (w, -h), (0, -h)]] box_rotate = [p.rotate(angle) for p in box] - min_box = (min(box_rotate, key=lambda p: p[0])[0], min(box_rotate, key=lambda p: p[1])[1]) - max_box = (max(box_rotate, key=lambda p: p[0])[0], max(box_rotate, key=lambda p: p[1])[1]) + min_box = ( + min(box_rotate, key=lambda p: p[0])[0], + min(box_rotate, key=lambda p: p[1])[1], + ) + max_box = ( + max(box_rotate, key=lambda p: p[0])[0], + max(box_rotate, key=lambda p: p[1])[1], + ) # calculate the translation of the pivot if origin_pos is None: @@ -325,8 +425,10 @@ def blit_rotate(surf: pygame.SurfaceType, image: pygame.SurfaceType, pos: Vector pivot_move = pivot_rotate - pivot # calculate the upper left origin of the rotated image - origin = (pos[0] - origin_pos[0] + min_box[0] - pivot_move[0], - pos[1] - origin_pos[1] - max_box[1] + pivot_move[1]) + origin = ( + pos[0] - origin_pos[0] + min_box[0] - pivot_move[0], + pos[1] - origin_pos[1] - max_box[1] + pivot_move[1], + ) # get a rotated image rotated_image = pygame.transform.rotate(image, angle) # rotate and blit the image @@ -336,7 +438,7 @@ def blit_rotate(surf: pygame.SurfaceType, image: pygame.SurfaceType, pos: Vector pygame.draw.rect(surf, (255, 0, 0), (*origin, *rotated_image.get_size()), 2) @classmethod - def get_color(cls, object_: 'RoadObject', transparent: bool = False): + def get_color(cls, object_: "RoadObject", transparent: bool = False): color = cls.DEFAULT_COLOR if isinstance(object_, Obstacle): diff --git a/highway_env/road/lane.py b/highway_env/road/lane.py index 87d302d33..1878231ed 100644 --- a/highway_env/road/lane.py +++ b/highway_env/road/lane.py @@ -1,11 +1,11 @@ from abc import ABCMeta, abstractmethod -from copy import deepcopy -from typing import Tuple, List, Optional, Union +from typing import List, Optional, Tuple, Union + import numpy as np from highway_env import utils from highway_env.road.spline import LinearSpline2D -from highway_env.utils import wrap_to_pi, Vector, get_class_path, class_from_path +from highway_env.utils import Vector, class_from_path, get_class_path, wrap_to_pi class AbstractLane(object): @@ -77,8 +77,13 @@ def to_config(self) -> dict: """ raise NotImplementedError() - def on_lane(self, position: np.ndarray, longitudinal: float = None, lateral: float = None, margin: float = 0) \ - -> bool: + def on_lane( + self, + position: np.ndarray, + longitudinal: float = None, + lateral: float = None, + margin: float = 0, + ) -> bool: """ Whether a given world position is on the lane. @@ -90,8 +95,10 @@ def on_lane(self, position: np.ndarray, longitudinal: float = None, lateral: flo """ if longitudinal is None or lateral is None: longitudinal, lateral = self.local_coordinates(position) - is_on = np.abs(lateral) <= self.width_at(longitudinal) / 2 + margin and \ - -self.VEHICLE_LENGTH <= longitudinal < self.length + self.VEHICLE_LENGTH + is_on = ( + np.abs(lateral) <= self.width_at(longitudinal) / 2 + margin + and -self.VEHICLE_LENGTH <= longitudinal < self.length + self.VEHICLE_LENGTH + ) return is_on def is_reachable_from(self, position: np.ndarray) -> bool: @@ -104,11 +111,15 @@ def is_reachable_from(self, position: np.ndarray) -> bool: if self.forbidden: return False longitudinal, lateral = self.local_coordinates(position) - is_close = np.abs(lateral) <= 2 * self.width_at(longitudinal) and \ - 0 <= longitudinal < self.length + self.VEHICLE_LENGTH + is_close = ( + np.abs(lateral) <= 2 * self.width_at(longitudinal) + and 0 <= longitudinal < self.length + self.VEHICLE_LENGTH + ) return is_close - def after_end(self, position: np.ndarray, longitudinal: float = None, lateral: float = None) -> bool: + def after_end( + self, position: np.ndarray, longitudinal: float = None, lateral: float = None + ) -> bool: if not longitudinal: longitudinal, _ = self.local_coordinates(position) return longitudinal > self.length - self.VEHICLE_LENGTH / 2 @@ -118,13 +129,18 @@ def distance(self, position: np.ndarray): s, r = self.local_coordinates(position) return abs(r) + max(s - self.length, 0) + max(0 - s, 0) - def distance_with_heading(self, position: np.ndarray, heading: Optional[float], heading_weight: float = 1.0): + def distance_with_heading( + self, + position: np.ndarray, + heading: Optional[float], + heading_weight: float = 1.0, + ): """Compute a weighted distance in position and heading to the lane.""" if heading is None: return self.distance(position) s, r = self.local_coordinates(position) angle = np.abs(self.local_angle(heading, s)) - return abs(r) + max(s - self.length, 0) + max(0 - s, 0) + heading_weight*angle + return abs(r) + max(s - self.length, 0) + max(0 - s, 0) + heading_weight * angle def local_angle(self, heading: float, long_offset: float): """Compute non-normalised angle of heading to the lane.""" @@ -145,14 +161,16 @@ class StraightLane(AbstractLane): """A lane going in straight line.""" - def __init__(self, - start: Vector, - end: Vector, - width: float = AbstractLane.DEFAULT_WIDTH, - line_types: Tuple[LineType, LineType] = None, - forbidden: bool = False, - speed_limit: float = 20, - priority: int = 0) -> None: + def __init__( + self, + start: Vector, + end: Vector, + width: float = AbstractLane.DEFAULT_WIDTH, + line_types: Tuple[LineType, LineType] = None, + forbidden: bool = False, + speed_limit: float = 20, + priority: int = 0, + ) -> None: """ New straight lane. @@ -166,7 +184,9 @@ def __init__(self, self.start = np.array(start) self.end = np.array(end) self.width = width - self.heading = np.arctan2(self.end[1] - self.start[1], self.end[0] - self.start[0]) + self.heading = np.arctan2( + self.end[1] - self.start[1], self.end[0] - self.start[0] + ) self.length = np.linalg.norm(self.end - self.start) self.line_types = line_types or [LineType.STRIPED, LineType.STRIPED] self.direction = (self.end - self.start) / self.length @@ -176,7 +196,11 @@ def __init__(self, self.speed_limit = speed_limit def position(self, longitudinal: float, lateral: float) -> np.ndarray: - return self.start + longitudinal * self.direction + lateral * self.direction_lateral + return ( + self.start + + longitudinal * self.direction + + lateral * self.direction_lateral + ) def heading_at(self, longitudinal: float) -> float: return self.heading @@ -206,8 +230,8 @@ def to_config(self) -> dict: "line_types": self.line_types, "forbidden": self.forbidden, "speed_limit": self.speed_limit, - "priority": self.priority - } + "priority": self.priority, + }, } @@ -215,17 +239,19 @@ class SineLane(StraightLane): """A sinusoidal lane.""" - def __init__(self, - start: Vector, - end: Vector, - amplitude: float, - pulsation: float, - phase: float, - width: float = StraightLane.DEFAULT_WIDTH, - line_types: List[LineType] = None, - forbidden: bool = False, - speed_limit: float = 20, - priority: int = 0) -> None: + def __init__( + self, + start: Vector, + end: Vector, + amplitude: float, + pulsation: float, + phase: float, + width: float = StraightLane.DEFAULT_WIDTH, + line_types: List[LineType] = None, + forbidden: bool = False, + speed_limit: float = 20, + priority: int = 0, + ) -> None: """ New sinusoidal lane. @@ -235,22 +261,32 @@ def __init__(self, :param pulsation: the lane pulsation [rad/m] :param phase: the lane initial phase [rad] """ - super().__init__(start, end, width, line_types, forbidden, speed_limit, priority) + super().__init__( + start, end, width, line_types, forbidden, speed_limit, priority + ) self.amplitude = amplitude self.pulsation = pulsation self.phase = phase def position(self, longitudinal: float, lateral: float) -> np.ndarray: - return super().position(longitudinal, - lateral + self.amplitude * np.sin(self.pulsation * longitudinal + self.phase)) + return super().position( + longitudinal, + lateral + + self.amplitude * np.sin(self.pulsation * longitudinal + self.phase), + ) def heading_at(self, longitudinal: float) -> float: return super().heading_at(longitudinal) + np.arctan( - self.amplitude * self.pulsation * np.cos(self.pulsation * longitudinal + self.phase)) + self.amplitude + * self.pulsation + * np.cos(self.pulsation * longitudinal + self.phase) + ) def local_coordinates(self, position: np.ndarray) -> Tuple[float, float]: longitudinal, lateral = super().local_coordinates(position) - return longitudinal, lateral - self.amplitude * np.sin(self.pulsation * longitudinal + self.phase) + return longitudinal, lateral - self.amplitude * np.sin( + self.pulsation * longitudinal + self.phase + ) @classmethod def from_config(cls, config: dict): @@ -260,14 +296,18 @@ def from_config(cls, config: dict): def to_config(self) -> dict: config = super().to_config() - config.update({ - "class_path": get_class_path(self.__class__), - }) - config["config"].update({ - "amplitude": self.amplitude, - "pulsation": self.pulsation, - "phase": self.phase - }) + config.update( + { + "class_path": get_class_path(self.__class__), + } + ) + config["config"].update( + { + "amplitude": self.amplitude, + "pulsation": self.pulsation, + "phase": self.phase, + } + ) return config @@ -275,17 +315,19 @@ class CircularLane(AbstractLane): """A lane going in circle arc.""" - def __init__(self, - center: Vector, - radius: float, - start_phase: float, - end_phase: float, - clockwise: bool = True, - width: float = AbstractLane.DEFAULT_WIDTH, - line_types: List[LineType] = None, - forbidden: bool = False, - speed_limit: float = 20, - priority: int = 0) -> None: + def __init__( + self, + center: Vector, + radius: float, + start_phase: float, + end_phase: float, + clockwise: bool = True, + width: float = AbstractLane.DEFAULT_WIDTH, + line_types: List[LineType] = None, + forbidden: bool = False, + speed_limit: float = 20, + priority: int = 0, + ) -> None: super().__init__() self.center = np.array(center) self.radius = radius @@ -296,17 +338,19 @@ def __init__(self, self.width = width self.line_types = line_types or [LineType.STRIPED, LineType.STRIPED] self.forbidden = forbidden - self.length = radius*(end_phase - start_phase) * self.direction + self.length = radius * (end_phase - start_phase) * self.direction self.priority = priority self.speed_limit = speed_limit def position(self, longitudinal: float, lateral: float) -> np.ndarray: phi = self.direction * longitudinal / self.radius + self.start_phase - return self.center + (self.radius - lateral * self.direction)*np.array([np.cos(phi), np.sin(phi)]) + return self.center + (self.radius - lateral * self.direction) * np.array( + [np.cos(phi), np.sin(phi)] + ) def heading_at(self, longitudinal: float) -> float: phi = self.direction * longitudinal / self.radius + self.start_phase - psi = phi + np.pi/2 * self.direction + psi = phi + np.pi / 2 * self.direction return psi def width_at(self, longitudinal: float) -> float: @@ -317,8 +361,8 @@ def local_coordinates(self, position: np.ndarray) -> Tuple[float, float]: phi = np.arctan2(delta[1], delta[0]) phi = self.start_phase + utils.wrap_to_pi(phi - self.start_phase) r = np.linalg.norm(delta) - longitudinal = self.direction*(phi - self.start_phase)*self.radius - lateral = self.direction*(self.radius - r) + longitudinal = self.direction * (phi - self.start_phase) * self.radius + lateral = self.direction * (self.radius - r) return longitudinal, lateral @classmethod @@ -339,8 +383,8 @@ def to_config(self) -> dict: "line_types": self.line_types, "forbidden": self.forbidden, "speed_limit": self.speed_limit, - "priority": self.priority - } + "priority": self.priority, + }, } diff --git a/highway_env/road/regulation.py b/highway_env/road/regulation.py index 0681a61e9..d0ed4328a 100644 --- a/highway_env/road/regulation.py +++ b/highway_env/road/regulation.py @@ -5,16 +5,23 @@ from highway_env import utils from highway_env.road.road import Road, RoadNetwork from highway_env.vehicle.controller import ControlledVehicle, MDPVehicle -from highway_env.vehicle.kinematics import Vehicle, Obstacle +from highway_env.vehicle.objects import Obstacle +from highway_env.vehicle.kinematics import Vehicle class RegulatedRoad(Road): YIELDING_COLOR: Tuple[float, float, float] = None REGULATION_FREQUENCY: int = 2 - YIELD_DURATION: float = 0. + YIELD_DURATION: float = 0.0 - def __init__(self, network: RoadNetwork = None, vehicles: List[Vehicle] = None, obstacles: List[Obstacle] = None, - np_random: np.random.RandomState = None, record_history: bool = False) -> None: + def __init__( + self, + network: RoadNetwork = None, + vehicles: List[Vehicle] = None, + obstacles: List[Obstacle] = None, + np_random: np.random.RandomState = None, + record_history: bool = False, + ) -> None: super().__init__(network, vehicles, obstacles, np_random, record_history) self.steps = 0 @@ -39,12 +46,16 @@ def enforce_road_rules(self) -> None: # Find new conflicts and resolve them for i in range(len(self.vehicles) - 1): - for j in range(i+1, len(self.vehicles)): + for j in range(i + 1, len(self.vehicles)): if self.is_conflict_possible(self.vehicles[i], self.vehicles[j]): - yielding_vehicle = self.respect_priorities(self.vehicles[i], self.vehicles[j]) - if yielding_vehicle is not None and \ - isinstance(yielding_vehicle, ControlledVehicle) and \ - not isinstance(yielding_vehicle, MDPVehicle): + yielding_vehicle = self.respect_priorities( + self.vehicles[i], self.vehicles[j] + ) + if ( + yielding_vehicle is not None + and isinstance(yielding_vehicle, ControlledVehicle) + and not isinstance(yielding_vehicle, MDPVehicle) + ): yielding_vehicle.color = self.YIELDING_COLOR yielding_vehicle.target_speed = 0 yielding_vehicle.is_yielding = True @@ -67,17 +78,26 @@ def respect_priorities(v1: Vehicle, v2: Vehicle) -> Vehicle: return v1 if v1.front_distance_to(v2) > v2.front_distance_to(v1) else v2 @staticmethod - def is_conflict_possible(v1: ControlledVehicle, v2: ControlledVehicle, horizon: int = 3, step: float = 0.25) -> bool: + def is_conflict_possible( + v1: ControlledVehicle, + v2: ControlledVehicle, + horizon: int = 3, + step: float = 0.25, + ) -> bool: times = np.arange(step, horizon, step) positions_1, headings_1 = v1.predict_trajectory_constant_speed(times) positions_2, headings_2 = v2.predict_trajectory_constant_speed(times) - for position_1, heading_1, position_2, heading_2 in zip(positions_1, headings_1, positions_2, headings_2): + for position_1, heading_1, position_2, heading_2 in zip( + positions_1, headings_1, positions_2, headings_2 + ): # Fast spherical pre-check if np.linalg.norm(position_2 - position_1) > v1.LENGTH: continue # Accurate rectangular check - if utils.rotated_rectangles_intersect((position_1, 1.5*v1.LENGTH, 0.9*v1.WIDTH, heading_1), - (position_2, 1.5*v2.LENGTH, 0.9*v2.WIDTH, heading_2)): + if utils.rotated_rectangles_intersect( + (position_1, 1.5 * v1.LENGTH, 0.9 * v1.WIDTH, heading_1), + (position_2, 1.5 * v2.LENGTH, 0.9 * v2.WIDTH, heading_2), + ): return True diff --git a/highway_env/road/road.py b/highway_env/road/road.py index 4bca66d67..998eb017e 100644 --- a/highway_env/road/road.py +++ b/highway_env/road/road.py @@ -1,8 +1,9 @@ -import numpy as np import logging -from typing import List, Tuple, Dict, TYPE_CHECKING, Optional +from typing import TYPE_CHECKING, Dict, List, Optional, Tuple + +import numpy as np -from highway_env.road.lane import LineType, StraightLane, AbstractLane, lane_from_config +from highway_env.road.lane import AbstractLane, LineType, StraightLane, lane_from_config from highway_env.vehicle.objects import Landmark if TYPE_CHECKING: @@ -48,7 +49,9 @@ def get_lane(self, index: LaneIndex) -> AbstractLane: _id = 0 return self.graph[_from][_to][_id] - def get_closest_lane_index(self, position: np.ndarray, heading: Optional[float] = None) -> LaneIndex: + def get_closest_lane_index( + self, position: np.ndarray, heading: Optional[float] = None + ) -> LaneIndex: """ Get the index of the lane closest to a world position. @@ -64,8 +67,13 @@ def get_closest_lane_index(self, position: np.ndarray, heading: Optional[float] indexes.append((_from, _to, _id)) return indexes[int(np.argmin(distances))] - def next_lane(self, current_index: LaneIndex, route: Route = None, position: np.ndarray = None, - np_random: np.random.RandomState = np.random) -> LaneIndex: + def next_lane( + self, + current_index: LaneIndex, + route: Route = None, + position: np.ndarray = None, + np_random: np.random.RandomState = np.random, + ) -> LaneIndex: """ Get the index of the next lane that should be followed after finishing the current lane. @@ -83,12 +91,20 @@ def next_lane(self, current_index: LaneIndex, route: Route = None, position: np. next_to = next_id = None # Pick next road according to planned route if route: - if route[0][:2] == current_index[:2]: # We just finished the first step of the route, drop it. + if ( + route[0][:2] == current_index[:2] + ): # We just finished the first step of the route, drop it. route.pop(0) - if route and route[0][0] == _to: # Next road in route is starting at the end of current road. + if ( + route and route[0][0] == _to + ): # Next road in route is starting at the end of current road. _, next_to, next_id = route[0] elif route: - logger.warning("Route {} does not start after current road {}.".format(route[0], current_index)) + logger.warning( + "Route {} does not start after current road {}.".format( + route[0], current_index + ) + ) # Compute current projected (desired) position long, lat = self.get_lane(current_index).local_coordinates(position) @@ -97,19 +113,34 @@ def next_lane(self, current_index: LaneIndex, route: Route = None, position: np. if not next_to: # Pick the one with the closest lane to projected target position try: - lanes_dists = [(next_to, - *self.next_lane_given_next_road(_from, _to, _id, next_to, next_id, projected_position)) - for next_to in self.graph[_to].keys()] # (next_to, next_id, distance) + lanes_dists = [ + ( + next_to, + *self.next_lane_given_next_road( + _from, _to, _id, next_to, next_id, projected_position + ), + ) + for next_to in self.graph[_to].keys() + ] # (next_to, next_id, distance) next_to, next_id, _ = min(lanes_dists, key=lambda x: x[-1]) except KeyError: return current_index else: # If it is known, follow it and get the closest lane - next_id, _ = self.next_lane_given_next_road(_from, _to, _id, next_to, next_id, projected_position) + next_id, _ = self.next_lane_given_next_road( + _from, _to, _id, next_to, next_id, projected_position + ) return _to, next_to, next_id - def next_lane_given_next_road(self, _from: str, _to: str, _id: int, - next_to: str, next_id: int, position: np.ndarray) -> Tuple[int, float]: + def next_lane_given_next_road( + self, + _from: str, + _to: str, + _id: int, + next_to: str, + next_id: int, + position: np.ndarray, + ) -> Tuple[int, float]: # If next road has same number of lane, stay on the same lane if len(self.graph[_from][_to]) == len(self.graph[_to][next_to]): if next_id is None: @@ -117,8 +148,9 @@ def next_lane_given_next_road(self, _from: str, _to: str, _id: int, # Else, pick closest lane else: lanes = range(len(self.graph[_to][next_to])) - next_id = min(lanes, - key=lambda l: self.get_lane((_to, next_to, l)).distance(position)) + next_id = min( + lanes, key=lambda l: self.get_lane((_to, next_to, l)).distance(position) + ) return next_id, self.get_lane((_to, next_to, next_id)).distance(position) def bfs_paths(self, start: str, goal: str) -> List[List[str]]: @@ -134,7 +166,9 @@ def bfs_paths(self, start: str, goal: str) -> List[List[str]]: (node, path) = queue.pop(0) if node not in self.graph: yield [] - for _next in sorted([key for key in self.graph[node].keys() if key not in path]): + for _next in sorted( + [key for key in self.graph[node].keys() if key not in path] + ): if _next == goal: yield path + [_next] elif _next in self.graph: @@ -155,13 +189,16 @@ def all_side_lanes(self, lane_index: LaneIndex) -> List[LaneIndex]: :param lane_index: the index of a lane. :return: all lanes belonging to the same road. """ - return [(lane_index[0], lane_index[1], i) for i in range(len(self.graph[lane_index[0]][lane_index[1]]))] + return [ + (lane_index[0], lane_index[1], i) + for i in range(len(self.graph[lane_index[0]][lane_index[1]])) + ] def side_lanes(self, lane_index: LaneIndex) -> List[LaneIndex]: """ - :param lane_index: the index of a lane. - :return: indexes of lanes next to a an input lane, to its right or left. - """ + :param lane_index: the index of a lane. + :return: indexes of lanes next to a an input lane, to its right or left. + """ _from, _to, _id = lane_index lanes = [] if _id > 0: @@ -171,17 +208,31 @@ def side_lanes(self, lane_index: LaneIndex) -> List[LaneIndex]: return lanes @staticmethod - def is_same_road(lane_index_1: LaneIndex, lane_index_2: LaneIndex, same_lane: bool = False) -> bool: + def is_same_road( + lane_index_1: LaneIndex, lane_index_2: LaneIndex, same_lane: bool = False + ) -> bool: """Is lane 1 in the same road as lane 2?""" - return lane_index_1[:2] == lane_index_2[:2] and (not same_lane or lane_index_1[2] == lane_index_2[2]) + return lane_index_1[:2] == lane_index_2[:2] and ( + not same_lane or lane_index_1[2] == lane_index_2[2] + ) @staticmethod - def is_leading_to_road(lane_index_1: LaneIndex, lane_index_2: LaneIndex, same_lane: bool = False) -> bool: + def is_leading_to_road( + lane_index_1: LaneIndex, lane_index_2: LaneIndex, same_lane: bool = False + ) -> bool: """Is lane 1 leading to of lane 2?""" - return lane_index_1[1] == lane_index_2[0] and (not same_lane or lane_index_1[2] == lane_index_2[2]) - - def is_connected_road(self, lane_index_1: LaneIndex, lane_index_2: LaneIndex, route: Route = None, - same_lane: bool = False, depth: int = 0) -> bool: + return lane_index_1[1] == lane_index_2[0] and ( + not same_lane or lane_index_1[2] == lane_index_2[2] + ) + + def is_connected_road( + self, + lane_index_1: LaneIndex, + lane_index_2: LaneIndex, + route: Route = None, + same_lane: bool = False, + depth: int = 0, + ) -> bool: """ Is the lane 2 leading to a road within lane 1's route? @@ -193,55 +244,86 @@ def is_connected_road(self, lane_index_1: LaneIndex, lane_index_2: LaneIndex, ro :param depth: search depth from lane 1 along its route :return: whether the roads are connected """ - if RoadNetwork.is_same_road(lane_index_2, lane_index_1, same_lane) \ - or RoadNetwork.is_leading_to_road(lane_index_2, lane_index_1, same_lane): + if RoadNetwork.is_same_road( + lane_index_2, lane_index_1, same_lane + ) or RoadNetwork.is_leading_to_road(lane_index_2, lane_index_1, same_lane): return True if depth > 0: if route and route[0][:2] == lane_index_1[:2]: # Route is starting at current road, skip it - return self.is_connected_road(lane_index_1, lane_index_2, route[1:], same_lane, depth) + return self.is_connected_road( + lane_index_1, lane_index_2, route[1:], same_lane, depth + ) elif route and route[0][0] == lane_index_1[1]: # Route is continuing from current road, follow it - return self.is_connected_road(route[0], lane_index_2, route[1:], same_lane, depth - 1) + return self.is_connected_road( + route[0], lane_index_2, route[1:], same_lane, depth - 1 + ) else: # Recursively search all roads at intersection _from, _to, _id = lane_index_1 - return any([self.is_connected_road((_to, l1_to, _id), lane_index_2, route, same_lane, depth - 1) - for l1_to in self.graph.get(_to, {}).keys()]) + return any( + [ + self.is_connected_road( + (_to, l1_to, _id), lane_index_2, route, same_lane, depth - 1 + ) + for l1_to in self.graph.get(_to, {}).keys() + ] + ) return False def lanes_list(self) -> List[AbstractLane]: - return [lane for to in self.graph.values() for ids in to.values() for lane in ids] + return [ + lane for to in self.graph.values() for ids in to.values() for lane in ids + ] def lanes_dict(self) -> Dict[str, AbstractLane]: - return {(from_, to_, i): lane - for from_, tos in self.graph.items() for to_, ids in tos.items() for i, lane in enumerate(ids)} + return { + (from_, to_, i): lane + for from_, tos in self.graph.items() + for to_, ids in tos.items() + for i, lane in enumerate(ids) + } @staticmethod - def straight_road_network(lanes: int = 4, - start: float = 0, - length: float = 10000, - angle: float = 0, - speed_limit: float = 30, - nodes_str: Optional[Tuple[str, str]] = None, - net: Optional['RoadNetwork'] = None) \ - -> 'RoadNetwork': + def straight_road_network( + lanes: int = 4, + start: float = 0, + length: float = 10000, + angle: float = 0, + speed_limit: float = 30, + nodes_str: Optional[Tuple[str, str]] = None, + net: Optional["RoadNetwork"] = None, + ) -> "RoadNetwork": net = net or RoadNetwork() nodes_str = nodes_str or ("0", "1") for lane in range(lanes): origin = np.array([start, lane * StraightLane.DEFAULT_WIDTH]) end = np.array([start + length, lane * StraightLane.DEFAULT_WIDTH]) - rotation = np.array([[np.cos(angle), np.sin(angle)], [-np.sin(angle), np.cos(angle)]]) + rotation = np.array( + [[np.cos(angle), np.sin(angle)], [-np.sin(angle), np.cos(angle)]] + ) origin = rotation @ origin end = rotation @ end - line_types = [LineType.CONTINUOUS_LINE if lane == 0 else LineType.STRIPED, - LineType.CONTINUOUS_LINE if lane == lanes - 1 else LineType.NONE] - net.add_lane(*nodes_str, StraightLane(origin, end, line_types=line_types, speed_limit=speed_limit)) + line_types = [ + LineType.CONTINUOUS_LINE if lane == 0 else LineType.STRIPED, + LineType.CONTINUOUS_LINE if lane == lanes - 1 else LineType.NONE, + ] + net.add_lane( + *nodes_str, + StraightLane( + origin, end, line_types=line_types, speed_limit=speed_limit + ) + ) return net - def position_heading_along_route(self, route: Route, longitudinal: float, lateral: float, - current_lane_index: LaneIndex) \ - -> Tuple[np.ndarray, float]: + def position_heading_along_route( + self, + route: Route, + longitudinal: float, + lateral: float, + current_lane_index: LaneIndex, + ) -> Tuple[np.ndarray, float]: """ Get the absolute position and heading along a route composed of several lanes at some local coordinates. @@ -251,13 +333,18 @@ def position_heading_along_route(self, route: Route, longitudinal: float, latera :param current_lane_index: current lane index of the vehicle :return: position, heading """ + def _get_route_head_with_id(route_): lane_index_ = route_[0] if lane_index_[2] is None: # We know which road segment will be followed by the vehicle, but not which lane. # Hypothesis: the vehicle will keep the same lane_id as the current one. - id_ = (current_lane_index[2] - if current_lane_index[2] < len(self.graph[current_lane_index[0]][current_lane_index[1]]) else 0) + id_ = ( + current_lane_index[2] + if current_lane_index[2] + < len(self.graph[current_lane_index[0]][current_lane_index[1]]) + else 0 + ) lane_index_ = (lane_index_[0], lane_index_[1], id_) return lane_index_ @@ -267,8 +354,9 @@ def _get_route_head_with_id(route_): route = route[1:] lane_index = _get_route_head_with_id(route) - return self.get_lane(lane_index).position(longitudinal, lateral),\ - self.get_lane(lane_index).heading_at(longitudinal) + return self.get_lane(lane_index).position(longitudinal, lateral), self.get_lane( + lane_index + ).heading_at(longitudinal) def random_lane_index(self, np_random: np.random.RandomState) -> LaneIndex: _from = np_random.choice(list(self.graph.keys())) @@ -284,9 +372,7 @@ def from_config(cls, config: dict) -> None: for _to, lanes_dict in to_dict.items(): net.graph[_from][_to] = [] for lane_dict in lanes_dict: - net.graph[_from][_to].append( - lane_from_config(lane_dict) - ) + net.graph[_from][_to].append(lane_from_config(lane_dict)) return net def to_config(self) -> dict: @@ -296,9 +382,7 @@ def to_config(self) -> dict: for _to, lanes in to_dict.items(): graph_dict[_from][_to] = [] for lane in lanes: - graph_dict[_from][_to].append( - lane.to_config() - ) + graph_dict[_from][_to].append(lane.to_config()) return graph_dict @@ -306,12 +390,14 @@ class Road(object): """A road is a set of lanes, and a set of vehicles driving on these lanes.""" - def __init__(self, - network: RoadNetwork = None, - vehicles: List['kinematics.Vehicle'] = None, - road_objects: List['objects.RoadObject'] = None, - np_random: np.random.RandomState = None, - record_history: bool = False) -> None: + def __init__( + self, + network: RoadNetwork = None, + vehicles: List["kinematics.Vehicle"] = None, + road_objects: List["objects.RoadObject"] = None, + np_random: np.random.RandomState = None, + record_history: bool = False, + ) -> None: """ New road. @@ -327,15 +413,28 @@ def __init__(self, self.np_random = np_random if np_random else np.random.RandomState() self.record_history = record_history - def close_objects_to(self, vehicle: 'kinematics.Vehicle', distance: float, count: Optional[int] = None, - see_behind: bool = True, sort: bool = True, vehicles_only: bool = False) -> object: - vehicles = [v for v in self.vehicles - if np.linalg.norm(v.position - vehicle.position) < distance - and v is not vehicle - and (see_behind or -2 * vehicle.LENGTH < vehicle.lane_distance_to(v))] - obstacles = [o for o in self.objects - if np.linalg.norm(o.position - vehicle.position) < distance - and -2 * vehicle.LENGTH < vehicle.lane_distance_to(o)] + def close_objects_to( + self, + vehicle: "kinematics.Vehicle", + distance: float, + count: Optional[int] = None, + see_behind: bool = True, + sort: bool = True, + vehicles_only: bool = False, + ) -> object: + vehicles = [ + v + for v in self.vehicles + if np.linalg.norm(v.position - vehicle.position) < distance + and v is not vehicle + and (see_behind or -2 * vehicle.LENGTH < vehicle.lane_distance_to(v)) + ] + obstacles = [ + o + for o in self.objects + if np.linalg.norm(o.position - vehicle.position) < distance + and -2 * vehicle.LENGTH < vehicle.lane_distance_to(o) + ] objects_ = vehicles if vehicles_only else vehicles + obstacles @@ -345,9 +444,17 @@ def close_objects_to(self, vehicle: 'kinematics.Vehicle', distance: float, count objects_ = objects_[:count] return objects_ - def close_vehicles_to(self, vehicle: 'kinematics.Vehicle', distance: float, count: Optional[int] = None, - see_behind: bool = True, sort: bool = True) -> object: - return self.close_objects_to(vehicle, distance, count, see_behind, sort, vehicles_only=True) + def close_vehicles_to( + self, + vehicle: "kinematics.Vehicle", + distance: float, + count: Optional[int] = None, + see_behind: bool = True, + sort: bool = True, + ) -> object: + return self.close_objects_to( + vehicle, distance, count, see_behind, sort, vehicles_only=True + ) def act(self) -> None: """Decide the actions of each entity on the road.""" @@ -363,13 +470,14 @@ def step(self, dt: float) -> None: for vehicle in self.vehicles: vehicle.step(dt) for i, vehicle in enumerate(self.vehicles): - for other in self.vehicles[i+1:]: + for other in self.vehicles[i + 1 :]: vehicle.handle_collisions(other, dt) for other in self.objects: vehicle.handle_collisions(other, dt) - def neighbour_vehicles(self, vehicle: 'kinematics.Vehicle', lane_index: LaneIndex = None) \ - -> Tuple[Optional['kinematics.Vehicle'], Optional['kinematics.Vehicle']]: + def neighbour_vehicles( + self, vehicle: "kinematics.Vehicle", lane_index: LaneIndex = None + ) -> Tuple[Optional["kinematics.Vehicle"], Optional["kinematics.Vehicle"]]: """ Find the preceding and following vehicles of a given vehicle. @@ -387,7 +495,9 @@ def neighbour_vehicles(self, vehicle: 'kinematics.Vehicle', lane_index: LaneInde s_front = s_rear = None v_front = v_rear = None for v in self.vehicles + self.objects: - if v is not vehicle and not isinstance(v, Landmark): # self.network.is_connected_road(v.lane_index, + if v is not vehicle and not isinstance( + v, Landmark + ): # self.network.is_connected_road(v.lane_index, # lane_index, same_lane=True): s_v, lat_v = lane.local_coordinates(v.position) if not lane.on_lane(v.position, s_v, lat_v, margin=1): diff --git a/highway_env/road/spline.py b/highway_env/road/spline.py index 0f27c65cf..ef6ae3d84 100644 --- a/highway_env/road/spline.py +++ b/highway_env/road/spline.py @@ -1,6 +1,7 @@ +from typing import List, Tuple + import numpy as np from scipy import interpolate -from typing import List, Tuple class LinearSpline2D: diff --git a/highway_env/utils.py b/highway_env/utils.py index d7c1cea1b..15ff45cbc 100644 --- a/highway_env/utils.py +++ b/highway_env/utils.py @@ -1,20 +1,22 @@ import copy import importlib import itertools -from typing import Tuple, Dict, Callable, List, Optional, Union, Sequence +from typing import Callable, Dict, List, Optional, Sequence, Tuple, Union import numpy as np # Useful types Vector = Union[np.ndarray, Sequence[float]] Matrix = Union[np.ndarray, Sequence[Sequence[float]]] -Interval = Union[np.ndarray, - Tuple[Vector, Vector], - Tuple[Matrix, Matrix], - Tuple[float, float], - List[Vector], - List[Matrix], - List[float]] +Interval = Union[ + np.ndarray, + Tuple[Vector, Vector], + Tuple[Matrix, Matrix], + Tuple[float, float], + List[Vector], + List[Matrix], + List[float], +] def do_every(duration: float, timer: float) -> bool: @@ -61,11 +63,15 @@ def point_in_rectangle(point: Vector, rect_min: Vector, rect_max: Vector) -> boo :param rect_min: x_min, y_min :param rect_max: x_max, y_max """ - return rect_min[0] <= point[0] <= rect_max[0] and rect_min[1] <= point[1] <= rect_max[1] + return ( + rect_min[0] <= point[0] <= rect_max[0] + and rect_min[1] <= point[1] <= rect_max[1] + ) -def point_in_rotated_rectangle(point: np.ndarray, center: np.ndarray, length: float, width: float, angle: float) \ - -> bool: +def point_in_rotated_rectangle( + point: np.ndarray, center: np.ndarray, length: float, width: float, angle: float +) -> bool: """ Check if a point is inside a rotated rectangle @@ -79,10 +85,12 @@ def point_in_rotated_rectangle(point: np.ndarray, center: np.ndarray, length: fl c, s = np.cos(angle), np.sin(angle) r = np.array([[c, -s], [s, c]]) ru = r.dot(point - center) - return point_in_rectangle(ru, (-length/2, -width/2), (length/2, width/2)) + return point_in_rectangle(ru, (-length / 2, -width / 2), (length / 2, width / 2)) -def point_in_ellipse(point: Vector, center: Vector, angle: float, length: float, width: float) -> bool: +def point_in_ellipse( + point: Vector, center: Vector, angle: float, length: float, width: float +) -> bool: """ Check if a point is inside an ellipse @@ -99,8 +107,9 @@ def point_in_ellipse(point: Vector, center: Vector, angle: float, length: float, return np.sum(np.square(ru / np.array([length, width]))) < 1 -def rotated_rectangles_intersect(rect1: Tuple[Vector, float, float, float], - rect2: Tuple[Vector, float, float, float]) -> bool: +def rotated_rectangles_intersect( + rect1: Tuple[Vector, float, float, float], rect2: Tuple[Vector, float, float, float] +) -> bool: """ Do two rotated rectangles intersect? @@ -111,8 +120,14 @@ def rotated_rectangles_intersect(rect1: Tuple[Vector, float, float, float], return has_corner_inside(rect1, rect2) or has_corner_inside(rect2, rect1) -def rect_corners(center: np.ndarray, length: float, width: float, angle: float, - include_midpoints: bool = False, include_center: bool = False) -> List[np.ndarray]: +def rect_corners( + center: np.ndarray, + length: float, + width: float, + angle: float, + include_midpoints: bool = False, + include_center: bool = False, +) -> List[np.ndarray]: """ Returns the positions of the corners of a rectangle. :param center: the rectangle center @@ -124,32 +139,34 @@ def rect_corners(center: np.ndarray, length: float, width: float, angle: float, :return: a list of positions """ center = np.array(center) - half_l = np.array([length/2, 0]) - half_w = np.array([0, width/2]) - corners = [- half_l - half_w, - - half_l + half_w, - + half_l + half_w, - + half_l - half_w] + half_l = np.array([length / 2, 0]) + half_w = np.array([0, width / 2]) + corners = [-half_l - half_w, -half_l + half_w, +half_l + half_w, +half_l - half_w] if include_center: corners += [[0, 0]] if include_midpoints: - corners += [- half_l, half_l, -half_w, half_w] + corners += [-half_l, half_l, -half_w, half_w] c, s = np.cos(angle), np.sin(angle) rotation = np.array([[c, -s], [s, c]]) return (rotation @ np.array(corners).T).T + np.tile(center, (len(corners), 1)) -def has_corner_inside(rect1: Tuple[Vector, float, float, float], - rect2: Tuple[Vector, float, float, float]) -> bool: +def has_corner_inside( + rect1: Tuple[Vector, float, float, float], rect2: Tuple[Vector, float, float, float] +) -> bool: """ Check if rect1 has a corner inside rect2 :param rect1: (center, length, width, angle) :param rect2: (center, length, width, angle) """ - return any([point_in_rotated_rectangle(p1, *rect2) - for p1 in rect_corners(*rect1, include_midpoints=True, include_center=True)]) + return any( + [ + point_in_rotated_rectangle(p1, *rect2) + for p1 in rect_corners(*rect1, include_midpoints=True, include_center=True) + ] + ) def project_polygon(polygon: Vector, axis: Vector) -> Tuple[float, float]: @@ -171,9 +188,9 @@ def interval_distance(min_a: float, max_a: float, min_b: float, max_b: float): return min_b - max_a if min_a < min_b else min_a - max_b -def are_polygons_intersecting(a: Vector, b: Vector, - displacement_a: Vector, displacement_b: Vector) \ - -> Tuple[bool, bool, Optional[np.ndarray]]: +def are_polygons_intersecting( + a: Vector, b: Vector, displacement_a: Vector, displacement_b: Vector +) -> Tuple[bool, bool, Optional[np.ndarray]]: """ Checks if the two polygons are intersecting. @@ -219,8 +236,13 @@ def are_polygons_intersecting(a: Vector, b: Vector, return intersecting, will_intersect, translation -def confidence_ellipsoid(data: Dict[str, np.ndarray], lambda_: float = 1e-5, delta: float = 0.1, sigma: float = 0.1, - param_bound: float = 1.0) -> Tuple[np.ndarray, np.ndarray, float]: +def confidence_ellipsoid( + data: Dict[str, np.ndarray], + lambda_: float = 1e-5, + delta: float = 0.1, + sigma: float = 0.1, + param_bound: float = 1.0, +) -> Tuple[np.ndarray, np.ndarray, float]: """ Compute a confidence ellipsoid over the parameter theta, where y = theta^T phi @@ -233,15 +255,21 @@ def confidence_ellipsoid(data: Dict[str, np.ndarray], lambda_: float = 1e-5, del """ phi = np.array(data["features"]) y = np.array(data["outputs"]) - g_n_lambda = 1/sigma * np.transpose(phi) @ phi + lambda_ * np.identity(phi.shape[-1]) + g_n_lambda = 1 / sigma * np.transpose(phi) @ phi + lambda_ * np.identity( + phi.shape[-1] + ) theta_n_lambda = np.linalg.inv(g_n_lambda) @ np.transpose(phi) @ y / sigma d = theta_n_lambda.shape[0] - beta_n = np.sqrt(2*np.log(np.sqrt(np.linalg.det(g_n_lambda) / lambda_ ** d) / delta)) + \ - np.sqrt(lambda_*d) * param_bound + beta_n = ( + np.sqrt(2 * np.log(np.sqrt(np.linalg.det(g_n_lambda) / lambda_**d) / delta)) + + np.sqrt(lambda_ * d) * param_bound + ) return theta_n_lambda, g_n_lambda, beta_n -def confidence_polytope(data: dict, parameter_box: np.ndarray) -> Tuple[np.ndarray, np.ndarray, np.ndarray, float]: +def confidence_polytope( + data: dict, parameter_box: np.ndarray +) -> Tuple[np.ndarray, np.ndarray, np.ndarray, float]: """ Compute a confidence polytope over the parameter theta, where y = theta^T phi @@ -250,7 +278,9 @@ def confidence_polytope(data: dict, parameter_box: np.ndarray) -> Tuple[np.ndarr :return: estimated theta, polytope vertices, Gramian matrix G_N_lambda, radius beta_N """ param_bound = np.amax(np.abs(parameter_box)) - theta_n_lambda, g_n_lambda, beta_n = confidence_ellipsoid(data, param_bound=param_bound) + theta_n_lambda, g_n_lambda, beta_n = confidence_ellipsoid( + data, param_bound=param_bound + ) values, pp = np.linalg.eig(g_n_lambda) radius_matrix = np.sqrt(beta_n) * np.linalg.inv(pp) @ np.diag(np.sqrt(1 / values)) @@ -260,12 +290,22 @@ def confidence_polytope(data: dict, parameter_box: np.ndarray) -> Tuple[np.ndarr # Clip the parameter and confidence region within the prior parameter box. theta_n_lambda = np.clip(theta_n_lambda, parameter_box[0], parameter_box[1]) for k, _ in enumerate(d_theta): - d_theta[k] = np.clip(d_theta[k], parameter_box[0] - theta_n_lambda, parameter_box[1] - theta_n_lambda) + d_theta[k] = np.clip( + d_theta[k], + parameter_box[0] - theta_n_lambda, + parameter_box[1] - theta_n_lambda, + ) return theta_n_lambda, d_theta, g_n_lambda, beta_n -def is_valid_observation(y: np.ndarray, phi: np.ndarray, theta: np.ndarray, gramian: np.ndarray, - beta: float, sigma: float = 0.1) -> bool: +def is_valid_observation( + y: np.ndarray, + phi: np.ndarray, + theta: np.ndarray, + gramian: np.ndarray, + beta: float, + sigma: float = 0.1, +) -> bool: """ Check if a new observation (phi, y) is valid according to a confidence ellipsoid on theta. @@ -299,7 +339,9 @@ def is_consistent_dataset(data: dict, parameter_box: np.ndarray = None) -> bool: y, phi = train_set["outputs"].pop(-1), train_set["features"].pop(-1) y, phi = np.array(y)[..., np.newaxis], np.array(phi)[..., np.newaxis] if train_set["outputs"] and train_set["features"]: - theta, _, gramian, beta = confidence_polytope(train_set, parameter_box=parameter_box) + theta, _, gramian, beta = confidence_polytope( + train_set, parameter_box=parameter_box + ) return is_valid_observation(y, phi, theta, gramian, beta) else: return True @@ -351,24 +393,26 @@ def distance_to_rect(line: Tuple[np.ndarray, np.ndarray], rect: List[np.ndarray] a, b, c, d = rect u = b - a v = d - a - u, v = u/np.linalg.norm(u), v/np.linalg.norm(v) + u, v = u / np.linalg.norm(u), v / np.linalg.norm(v) rqu = (q - r) @ u rqv = (q - r) @ v interval_1 = [(a - r) @ u / rqu, (b - r) @ u / rqu] interval_2 = [(a - r) @ v / rqv, (d - r) @ v / rqv] interval_1 = interval_1 if rqu >= 0 else list(reversed(interval_1)) interval_2 = interval_2 if rqv >= 0 else list(reversed(interval_2)) - if interval_distance(*interval_1, *interval_2) <= 0 \ - and interval_distance(0, 1, *interval_1) <= 0 \ - and interval_distance(0, 1, *interval_2) <= 0: + if ( + interval_distance(*interval_1, *interval_2) <= 0 + and interval_distance(0, 1, *interval_1) <= 0 + and interval_distance(0, 1, *interval_2) <= 0 + ): return max(interval_1[0], interval_2[0]) * np.linalg.norm(q - r) else: return np.inf def solve_trinom(a, b, c): - delta = b ** 2 - 4 * a * c + delta = b**2 - 4 * a * c if delta >= 0: return (-b - np.sqrt(delta)) / (2 * a), (-b + np.sqrt(delta)) / (2 * a) else: - return None, None \ No newline at end of file + return None, None diff --git a/highway_env/vehicle/behavior.py b/highway_env/vehicle/behavior.py index 28100042a..05371598c 100644 --- a/highway_env/vehicle/behavior.py +++ b/highway_env/vehicle/behavior.py @@ -1,11 +1,11 @@ -from typing import Tuple, Union +from typing import Union import numpy as np -from highway_env.road.road import Road, Route, LaneIndex +from highway_env import utils +from highway_env.road.road import LaneIndex, Road, Route from highway_env.utils import Vector from highway_env.vehicle.controller import ControlledVehicle -from highway_env import utils from highway_env.vehicle.kinematics import Vehicle @@ -40,27 +40,33 @@ class IDMVehicle(ControlledVehicle): """Range of delta when chosen randomly.""" # Lateral policy parameters - POLITENESS = 0. # in [0, 1] + POLITENESS = 0.0 # in [0, 1] LANE_CHANGE_MIN_ACC_GAIN = 0.2 # [m/s2] LANE_CHANGE_MAX_BRAKING_IMPOSED = 2.0 # [m/s2] LANE_CHANGE_DELAY = 1.0 # [s] - def __init__(self, - road: Road, - position: Vector, - heading: float = 0, - speed: float = 0, - target_lane_index: int = None, - target_speed: float = None, - route: Route = None, - enable_lane_change: bool = True, - timer: float = None): - super().__init__(road, position, heading, speed, target_lane_index, target_speed, route) + def __init__( + self, + road: Road, + position: Vector, + heading: float = 0, + speed: float = 0, + target_lane_index: int = None, + target_speed: float = None, + route: Route = None, + enable_lane_change: bool = True, + timer: float = None, + ): + super().__init__( + road, position, heading, speed, target_lane_index, target_speed, route + ) self.enable_lane_change = enable_lane_change - self.timer = timer or (np.sum(self.position)*np.pi) % self.LANE_CHANGE_DELAY + self.timer = timer or (np.sum(self.position) * np.pi) % self.LANE_CHANGE_DELAY def randomize_behavior(self): - self.DELTA = self.road.np_random.uniform(low=self.DELTA_RANGE[0], high=self.DELTA_RANGE[1]) + self.DELTA = self.road.np_random.uniform( + low=self.DELTA_RANGE[0], high=self.DELTA_RANGE[1] + ) @classmethod def create_from(cls, vehicle: ControlledVehicle) -> "IDMVehicle": @@ -72,9 +78,16 @@ def create_from(cls, vehicle: ControlledVehicle) -> "IDMVehicle": :param vehicle: a vehicle :return: a new vehicle at the same dynamical state """ - v = cls(vehicle.road, vehicle.position, heading=vehicle.heading, speed=vehicle.speed, - target_lane_index=vehicle.target_lane_index, target_speed=vehicle.target_speed, - route=vehicle.route, timer=getattr(vehicle, 'timer', None)) + v = cls( + vehicle.road, + vehicle.position, + heading=vehicle.heading, + speed=vehicle.speed, + target_lane_index=vehicle.target_lane_index, + target_speed=vehicle.target_speed, + route=vehicle.route, + timer=getattr(vehicle, "timer", None), + ) return v def act(self, action: Union[dict, str] = None): @@ -93,24 +106,36 @@ def act(self, action: Union[dict, str] = None): self.follow_road() if self.enable_lane_change: self.change_lane_policy() - action['steering'] = self.steering_control(self.target_lane_index) - action['steering'] = np.clip(action['steering'], -self.MAX_STEERING_ANGLE, self.MAX_STEERING_ANGLE) + action["steering"] = self.steering_control(self.target_lane_index) + action["steering"] = np.clip( + action["steering"], -self.MAX_STEERING_ANGLE, self.MAX_STEERING_ANGLE + ) # Longitudinal: IDM - front_vehicle, rear_vehicle = self.road.neighbour_vehicles(self, self.lane_index) - action['acceleration'] = self.acceleration(ego_vehicle=self, - front_vehicle=front_vehicle, - rear_vehicle=rear_vehicle) + front_vehicle, rear_vehicle = self.road.neighbour_vehicles( + self, self.lane_index + ) + action["acceleration"] = self.acceleration( + ego_vehicle=self, front_vehicle=front_vehicle, rear_vehicle=rear_vehicle + ) # When changing lane, check both current and target lanes if self.lane_index != self.target_lane_index: - front_vehicle, rear_vehicle = self.road.neighbour_vehicles(self, self.target_lane_index) - target_idm_acceleration = self.acceleration(ego_vehicle=self, - front_vehicle=front_vehicle, - rear_vehicle=rear_vehicle) - action['acceleration'] = min(action['acceleration'], target_idm_acceleration) + front_vehicle, rear_vehicle = self.road.neighbour_vehicles( + self, self.target_lane_index + ) + target_idm_acceleration = self.acceleration( + ego_vehicle=self, front_vehicle=front_vehicle, rear_vehicle=rear_vehicle + ) + action["acceleration"] = min( + action["acceleration"], target_idm_acceleration + ) # action['acceleration'] = self.recover_from_stop(action['acceleration']) - action['acceleration'] = np.clip(action['acceleration'], -self.ACC_MAX, self.ACC_MAX) - Vehicle.act(self, action) # Skip ControlledVehicle.act(), or the command will be overriden. + action["acceleration"] = np.clip( + action["acceleration"], -self.ACC_MAX, self.ACC_MAX + ) + Vehicle.act( + self, action + ) # Skip ControlledVehicle.act(), or the command will be overriden. def step(self, dt: float): """ @@ -123,10 +148,12 @@ def step(self, dt: float): self.timer += dt super().step(dt) - def acceleration(self, - ego_vehicle: ControlledVehicle, - front_vehicle: Vehicle = None, - rear_vehicle: Vehicle = None) -> float: + def acceleration( + self, + ego_vehicle: ControlledVehicle, + front_vehicle: Vehicle = None, + rear_vehicle: Vehicle = None, + ) -> float: """ Compute an acceleration command with the Intelligent Driver Model. @@ -145,17 +172,30 @@ def acceleration(self, return 0 ego_target_speed = getattr(ego_vehicle, "target_speed", 0) if ego_vehicle.lane and ego_vehicle.lane.speed_limit is not None: - ego_target_speed = np.clip(ego_target_speed, 0, ego_vehicle.lane.speed_limit) - acceleration = self.COMFORT_ACC_MAX * (1 - np.power( - max(ego_vehicle.speed, 0) / abs(utils.not_zero(ego_target_speed)), self.DELTA)) + ego_target_speed = np.clip( + ego_target_speed, 0, ego_vehicle.lane.speed_limit + ) + acceleration = self.COMFORT_ACC_MAX * ( + 1 + - np.power( + max(ego_vehicle.speed, 0) / abs(utils.not_zero(ego_target_speed)), + self.DELTA, + ) + ) if front_vehicle: d = ego_vehicle.lane_distance_to(front_vehicle) - acceleration -= self.COMFORT_ACC_MAX * \ - np.power(self.desired_gap(ego_vehicle, front_vehicle) / utils.not_zero(d), 2) + acceleration -= self.COMFORT_ACC_MAX * np.power( + self.desired_gap(ego_vehicle, front_vehicle) / utils.not_zero(d), 2 + ) return acceleration - def desired_gap(self, ego_vehicle: Vehicle, front_vehicle: Vehicle = None, projected: bool = True) -> float: + def desired_gap( + self, + ego_vehicle: Vehicle, + front_vehicle: Vehicle = None, + projected: bool = True, + ) -> float: """ Compute the desired distance between a vehicle and its leading vehicle. @@ -167,9 +207,14 @@ def desired_gap(self, ego_vehicle: Vehicle, front_vehicle: Vehicle = None, proje d0 = self.DISTANCE_WANTED tau = self.TIME_WANTED ab = -self.COMFORT_ACC_MAX * self.COMFORT_ACC_MIN - dv = np.dot(ego_vehicle.velocity - front_vehicle.velocity, ego_vehicle.direction) if projected \ + dv = ( + np.dot(ego_vehicle.velocity - front_vehicle.velocity, ego_vehicle.direction) + if projected else ego_vehicle.speed - front_vehicle.speed - d_star = d0 + ego_vehicle.speed * tau + ego_vehicle.speed * dv / (2 * np.sqrt(ab)) + ) + d_star = ( + d0 + ego_vehicle.speed * tau + ego_vehicle.speed * dv / (2 * np.sqrt(ab)) + ) return d_star def change_lane_policy(self) -> None: @@ -186,10 +231,12 @@ def change_lane_policy(self) -> None: # If we are on correct route but bad lane: abort it if someone else is already changing into the same lane if self.lane_index[:2] == self.target_lane_index[:2]: for v in self.road.vehicles: - if v is not self \ - and v.lane_index != self.target_lane_index \ - and isinstance(v, ControlledVehicle) \ - and v.target_lane_index == self.target_lane_index: + if ( + v is not self + and v.lane_index != self.target_lane_index + and isinstance(v, ControlledVehicle) + and v.target_lane_index == self.target_lane_index + ): d = self.lane_distance_to(v) d_star = self.desired_gap(self, v) if 0 < d < d_star: @@ -205,7 +252,9 @@ def change_lane_policy(self) -> None: # decide to make a lane change for lane_index in self.road.network.side_lanes(self.lane_index): # Is the candidate lane close enough? - if not self.road.network.get_lane(lane_index).is_reachable_from(self.position): + if not self.road.network.get_lane(lane_index).is_reachable_from( + self.position + ): continue # Only change lane when the vehicle is moving if np.abs(self.speed) < 1: @@ -227,8 +276,12 @@ def mobil(self, lane_index: LaneIndex) -> bool: """ # Is the maneuver unsafe for the new following vehicle? new_preceding, new_following = self.road.neighbour_vehicles(self, lane_index) - new_following_a = self.acceleration(ego_vehicle=new_following, front_vehicle=new_preceding) - new_following_pred_a = self.acceleration(ego_vehicle=new_following, front_vehicle=self) + new_following_a = self.acceleration( + ego_vehicle=new_following, front_vehicle=new_preceding + ) + new_following_pred_a = self.acceleration( + ego_vehicle=new_following, front_vehicle=self + ) if new_following_pred_a < -self.LANE_CHANGE_MAX_BRAKING_IMPOSED: return False @@ -237,7 +290,9 @@ def mobil(self, lane_index: LaneIndex) -> bool: self_pred_a = self.acceleration(ego_vehicle=self, front_vehicle=new_preceding) if self.route and self.route[0][2] is not None: # Wrong direction - if np.sign(lane_index[2] - self.target_lane_index[2]) != np.sign(self.route[0][2] - self.target_lane_index[2]): + if np.sign(lane_index[2] - self.target_lane_index[2]) != np.sign( + self.route[0][2] - self.target_lane_index[2] + ): return False # Unsafe braking required elif self_pred_a < -self.LANE_CHANGE_MAX_BRAKING_IMPOSED: @@ -246,10 +301,23 @@ def mobil(self, lane_index: LaneIndex) -> bool: # Is there an acceleration advantage for me and/or my followers to change lane? else: self_a = self.acceleration(ego_vehicle=self, front_vehicle=old_preceding) - old_following_a = self.acceleration(ego_vehicle=old_following, front_vehicle=self) - old_following_pred_a = self.acceleration(ego_vehicle=old_following, front_vehicle=old_preceding) - jerk = self_pred_a - self_a + self.POLITENESS * (new_following_pred_a - new_following_a - + old_following_pred_a - old_following_a) + old_following_a = self.acceleration( + ego_vehicle=old_following, front_vehicle=self + ) + old_following_pred_a = self.acceleration( + ego_vehicle=old_following, front_vehicle=old_preceding + ) + jerk = ( + self_pred_a + - self_a + + self.POLITENESS + * ( + new_following_pred_a + - new_following_a + + old_following_pred_a + - old_following_a + ) + ) if jerk < self.LANE_CHANGE_MIN_ACC_GAIN: return False @@ -268,10 +336,13 @@ def recover_from_stop(self, acceleration: float) -> float: # Is the vehicle stopped on the wrong lane? if self.target_lane_index != self.lane_index and self.speed < stopped_speed: _, rear = self.road.neighbour_vehicles(self) - _, new_rear = self.road.neighbour_vehicles(self, self.road.network.get_lane(self.target_lane_index)) + _, new_rear = self.road.neighbour_vehicles( + self, self.road.network.get_lane(self.target_lane_index) + ) # Check for free room behind on both lanes - if (not rear or rear.lane_distance_to(self) > safe_distance) and \ - (not new_rear or new_rear.lane_distance_to(self) > safe_distance): + if (not rear or rear.lane_distance_to(self) > safe_distance) and ( + not new_rear or new_rear.lane_distance_to(self) > safe_distance + ): # Reverse return -self.COMFORT_ACC_MAX / 2 return acceleration @@ -282,27 +353,50 @@ class LinearVehicle(IDMVehicle): """A Vehicle whose longitudinal and lateral controllers are linear with respect to parameters.""" ACCELERATION_PARAMETERS = [0.3, 0.3, 2.0] - STEERING_PARAMETERS = [ControlledVehicle.KP_HEADING, ControlledVehicle.KP_HEADING * ControlledVehicle.KP_LATERAL] - - ACCELERATION_RANGE = np.array([0.5*np.array(ACCELERATION_PARAMETERS), 1.5*np.array(ACCELERATION_PARAMETERS)]) - STEERING_RANGE = np.array([np.array(STEERING_PARAMETERS) - np.array([0.07, 1.5]), - np.array(STEERING_PARAMETERS) + np.array([0.07, 1.5])]) + STEERING_PARAMETERS = [ + ControlledVehicle.KP_HEADING, + ControlledVehicle.KP_HEADING * ControlledVehicle.KP_LATERAL, + ] + + ACCELERATION_RANGE = np.array( + [ + 0.5 * np.array(ACCELERATION_PARAMETERS), + 1.5 * np.array(ACCELERATION_PARAMETERS), + ] + ) + STEERING_RANGE = np.array( + [ + np.array(STEERING_PARAMETERS) - np.array([0.07, 1.5]), + np.array(STEERING_PARAMETERS) + np.array([0.07, 1.5]), + ] + ) TIME_WANTED = 2.5 - def __init__(self, - road: Road, - position: Vector, - heading: float = 0, - speed: float = 0, - target_lane_index: int = None, - target_speed: float = None, - route: Route = None, - enable_lane_change: bool = True, - timer: float = None, - data: dict = None): - super().__init__(road, position, heading, speed, target_lane_index, target_speed, route, - enable_lane_change, timer) + def __init__( + self, + road: Road, + position: Vector, + heading: float = 0, + speed: float = 0, + target_lane_index: int = None, + target_speed: float = None, + route: Route = None, + enable_lane_change: bool = True, + timer: float = None, + data: dict = None, + ): + super().__init__( + road, + position, + heading, + speed, + target_lane_index, + target_speed, + route, + enable_lane_change, + timer, + ) self.data = data if data is not None else {} self.collecting_data = True @@ -313,15 +407,20 @@ def act(self, action: Union[dict, str] = None): def randomize_behavior(self): ua = self.road.np_random.uniform(size=np.shape(self.ACCELERATION_PARAMETERS)) - self.ACCELERATION_PARAMETERS = self.ACCELERATION_RANGE[0] + ua*(self.ACCELERATION_RANGE[1] - - self.ACCELERATION_RANGE[0]) + self.ACCELERATION_PARAMETERS = self.ACCELERATION_RANGE[0] + ua * ( + self.ACCELERATION_RANGE[1] - self.ACCELERATION_RANGE[0] + ) ub = self.road.np_random.uniform(size=np.shape(self.STEERING_PARAMETERS)) - self.STEERING_PARAMETERS = self.STEERING_RANGE[0] + ub*(self.STEERING_RANGE[1] - self.STEERING_RANGE[0]) - - def acceleration(self, - ego_vehicle: ControlledVehicle, - front_vehicle: Vehicle = None, - rear_vehicle: Vehicle = None) -> float: + self.STEERING_PARAMETERS = self.STEERING_RANGE[0] + ub * ( + self.STEERING_RANGE[1] - self.STEERING_RANGE[0] + ) + + def acceleration( + self, + ego_vehicle: ControlledVehicle, + front_vehicle: Vehicle = None, + rear_vehicle: Vehicle = None, + ) -> float: """ Compute an acceleration command with a Linear Model. @@ -337,16 +436,26 @@ def acceleration(self, :param rear_vehicle: the vehicle following the ego-vehicle :return: the acceleration command for the ego-vehicle [m/s2] """ - return float(np.dot(self.ACCELERATION_PARAMETERS, - self.acceleration_features(ego_vehicle, front_vehicle, rear_vehicle))) - - def acceleration_features(self, ego_vehicle: ControlledVehicle, - front_vehicle: Vehicle = None, - rear_vehicle: Vehicle = None) -> np.ndarray: + return float( + np.dot( + self.ACCELERATION_PARAMETERS, + self.acceleration_features(ego_vehicle, front_vehicle, rear_vehicle), + ) + ) + + def acceleration_features( + self, + ego_vehicle: ControlledVehicle, + front_vehicle: Vehicle = None, + rear_vehicle: Vehicle = None, + ) -> np.ndarray: vt, dv, dp = 0, 0, 0 if ego_vehicle: vt = ego_vehicle.target_speed - ego_vehicle.speed - d_safe = self.DISTANCE_WANTED + np.maximum(ego_vehicle.speed, 0) * self.TIME_WANTED + d_safe = ( + self.DISTANCE_WANTED + + np.maximum(ego_vehicle.speed, 0) * self.TIME_WANTED + ) if front_vehicle: d = ego_vehicle.lane_distance_to(front_vehicle) dv = min(front_vehicle.speed - ego_vehicle.speed, 0) @@ -362,7 +471,12 @@ def steering_control(self, target_lane_index: LaneIndex) -> float: :param target_lane_index: index of the lane to follow :return: a steering wheel angle command [rad] """ - return float(np.dot(np.array(self.STEERING_PARAMETERS), self.steering_features(target_lane_index))) + return float( + np.dot( + np.array(self.STEERING_PARAMETERS), + self.steering_features(target_lane_index), + ) + ) def steering_features(self, target_lane_index: LaneIndex) -> np.ndarray: """ @@ -375,40 +489,27 @@ def steering_features(self, target_lane_index: LaneIndex) -> np.ndarray: lane_coords = lane.local_coordinates(self.position) lane_next_coords = lane_coords[0] + self.speed * self.TAU_PURSUIT lane_future_heading = lane.heading_at(lane_next_coords) - features = np.array([utils.wrap_to_pi(lane_future_heading - self.heading) * - self.LENGTH / utils.not_zero(self.speed), - -lane_coords[1] * self.LENGTH / (utils.not_zero(self.speed) ** 2)]) + features = np.array( + [ + utils.wrap_to_pi(lane_future_heading - self.heading) + * self.LENGTH + / utils.not_zero(self.speed), + -lane_coords[1] * self.LENGTH / (utils.not_zero(self.speed) ** 2), + ] + ) return features def longitudinal_structure(self): # Nominal dynamics: integrate speed - A = np.array([ - [0, 0, 1, 0], - [0, 0, 0, 1], - [0, 0, 0, 0], - [0, 0, 0, 0] - ]) + A = np.array([[0, 0, 1, 0], [0, 0, 0, 1], [0, 0, 0, 0], [0, 0, 0, 0]]) # Target speed dynamics - phi0 = np.array([ - [0, 0, 0, 0], - [0, 0, 0, 0], - [0, 0, -1, 0], - [0, 0, 0, -1] - ]) + phi0 = np.array([[0, 0, 0, 0], [0, 0, 0, 0], [0, 0, -1, 0], [0, 0, 0, -1]]) # Front speed control - phi1 = np.array([ - [0, 0, 0, 0], - [0, 0, 0, 0], - [0, 0, -1, 1], - [0, 0, 0, 0] - ]) + phi1 = np.array([[0, 0, 0, 0], [0, 0, 0, 0], [0, 0, -1, 1], [0, 0, 0, 0]]) # Front position control - phi2 = np.array([ - [0, 0, 0, 0], - [0, 0, 0, 0], - [-1, 1, -self.TIME_WANTED, 0], - [0, 0, 0, 0] - ]) + phi2 = np.array( + [[0, 0, 0, 0], [0, 0, 0, 0], [-1, 1, -self.TIME_WANTED, 0], [0, 0, 0, 0]] + ) # Disable speed control front_vehicle, _ = self.road.neighbour_vehicles(self) if not front_vehicle or self.speed < front_vehicle.speed: @@ -426,18 +527,9 @@ def longitudinal_structure(self): return A, phi def lateral_structure(self): - A = np.array([ - [0, 1], - [0, 0] - ]) - phi0 = np.array([ - [0, 0], - [0, -1] - ]) - phi1 = np.array([ - [0, 0], - [-1, 0] - ]) + A = np.array([[0, 1], [0, 0]]) + phi0 = np.array([[0, 0], [0, -1]]) + phi1 = np.array([[0, 0], [-1, 0]]) phi = np.array([phi0, phi1]) return A, phi @@ -446,7 +538,6 @@ def collect_data(self): self.add_features(self.data, self.target_lane_index) def add_features(self, data, lane_index, output_lane=None): - front_vehicle, rear_vehicle = self.road.neighbour_vehicles(self) features = self.acceleration_features(self, front_vehicle, rear_vehicle) output = np.dot(self.ACCELERATION_PARAMETERS, features) @@ -471,9 +562,11 @@ class AggressiveVehicle(LinearVehicle): MERGE_ACC_GAIN = 0.8 MERGE_VEL_RATIO = 0.75 MERGE_TARGET_VEL = 30 - ACCELERATION_PARAMETERS = [MERGE_ACC_GAIN / ((1 - MERGE_VEL_RATIO) * MERGE_TARGET_VEL), - MERGE_ACC_GAIN / (MERGE_VEL_RATIO * MERGE_TARGET_VEL), - 0.5] + ACCELERATION_PARAMETERS = [ + MERGE_ACC_GAIN / ((1 - MERGE_VEL_RATIO) * MERGE_TARGET_VEL), + MERGE_ACC_GAIN / (MERGE_VEL_RATIO * MERGE_TARGET_VEL), + 0.5, + ] class DefensiveVehicle(LinearVehicle): @@ -481,6 +574,8 @@ class DefensiveVehicle(LinearVehicle): MERGE_ACC_GAIN = 1.2 MERGE_VEL_RATIO = 0.75 MERGE_TARGET_VEL = 30 - ACCELERATION_PARAMETERS = [MERGE_ACC_GAIN / ((1 - MERGE_VEL_RATIO) * MERGE_TARGET_VEL), - MERGE_ACC_GAIN / (MERGE_VEL_RATIO * MERGE_TARGET_VEL), - 2.0] + ACCELERATION_PARAMETERS = [ + MERGE_ACC_GAIN / ((1 - MERGE_VEL_RATIO) * MERGE_TARGET_VEL), + MERGE_ACC_GAIN / (MERGE_VEL_RATIO * MERGE_TARGET_VEL), + 2.0, + ] diff --git a/highway_env/vehicle/controller.py b/highway_env/vehicle/controller.py index 07dc88362..114a643f0 100644 --- a/highway_env/vehicle/controller.py +++ b/highway_env/vehicle/controller.py @@ -1,9 +1,10 @@ -from typing import List, Tuple, Union, Optional +import copy +from typing import List, Optional, Tuple, Union import numpy as np -import copy + from highway_env import utils -from highway_env.road.road import Road, LaneIndex, Route +from highway_env.road.road import LaneIndex, Road, Route from highway_env.utils import Vector from highway_env.vehicle.kinematics import Vehicle @@ -31,14 +32,16 @@ class ControlledVehicle(Vehicle): MAX_STEERING_ANGLE = np.pi / 3 # [rad] DELTA_SPEED = 5 # [m/s] - def __init__(self, - road: Road, - position: Vector, - heading: float = 0, - speed: float = 0, - target_lane_index: LaneIndex = None, - target_speed: float = None, - route: Route = None): + def __init__( + self, + road: Road, + position: Vector, + heading: float = 0, + speed: float = 0, + target_lane_index: LaneIndex = None, + target_speed: float = None, + route: Route = None, + ): super().__init__(road, position, heading, speed) self.target_lane_index = target_lane_index or self.lane_index self.target_speed = target_speed or self.speed @@ -54,9 +57,15 @@ def create_from(cls, vehicle: "ControlledVehicle") -> "ControlledVehicle": :param vehicle: a vehicle :return: a new vehicle at the same dynamical state """ - v = cls(vehicle.road, vehicle.position, heading=vehicle.heading, speed=vehicle.speed, - target_lane_index=vehicle.target_lane_index, target_speed=vehicle.target_speed, - route=vehicle.route) + v = cls( + vehicle.road, + vehicle.position, + heading=vehicle.heading, + speed=vehicle.speed, + target_lane_index=vehicle.target_lane_index, + target_speed=vehicle.target_speed, + route=vehicle.route, + ) return v def plan_route_to(self, destination: str) -> "ControlledVehicle": @@ -70,7 +79,9 @@ def plan_route_to(self, destination: str) -> "ControlledVehicle": except KeyError: path = [] if path: - self.route = [self.lane_index] + [(path[i], path[i + 1], None) for i in range(len(path) - 1)] + self.route = [self.lane_index] + [ + (path[i], path[i + 1], None) for i in range(len(path) - 1) + ] else: self.route = [self.lane_index] return self @@ -91,27 +102,45 @@ def act(self, action: Union[dict, str] = None) -> None: self.target_speed -= self.DELTA_SPEED elif action == "LANE_RIGHT": _from, _to, _id = self.target_lane_index - target_lane_index = _from, _to, np.clip(_id + 1, 0, len(self.road.network.graph[_from][_to]) - 1) - if self.road.network.get_lane(target_lane_index).is_reachable_from(self.position): + target_lane_index = ( + _from, + _to, + np.clip(_id + 1, 0, len(self.road.network.graph[_from][_to]) - 1), + ) + if self.road.network.get_lane(target_lane_index).is_reachable_from( + self.position + ): self.target_lane_index = target_lane_index elif action == "LANE_LEFT": _from, _to, _id = self.target_lane_index - target_lane_index = _from, _to, np.clip(_id - 1, 0, len(self.road.network.graph[_from][_to]) - 1) - if self.road.network.get_lane(target_lane_index).is_reachable_from(self.position): + target_lane_index = ( + _from, + _to, + np.clip(_id - 1, 0, len(self.road.network.graph[_from][_to]) - 1), + ) + if self.road.network.get_lane(target_lane_index).is_reachable_from( + self.position + ): self.target_lane_index = target_lane_index - action = {"steering": self.steering_control(self.target_lane_index), - "acceleration": self.speed_control(self.target_speed)} - action['steering'] = np.clip(action['steering'], -self.MAX_STEERING_ANGLE, self.MAX_STEERING_ANGLE) + action = { + "steering": self.steering_control(self.target_lane_index), + "acceleration": self.speed_control(self.target_speed), + } + action["steering"] = np.clip( + action["steering"], -self.MAX_STEERING_ANGLE, self.MAX_STEERING_ANGLE + ) super().act(action) def follow_road(self) -> None: """At the end of a lane, automatically switch to a next one.""" if self.road.network.get_lane(self.target_lane_index).after_end(self.position): - self.target_lane_index = self.road.network.next_lane(self.target_lane_index, - route=self.route, - position=self.position, - np_random=self.road.np_random) + self.target_lane_index = self.road.network.next_lane( + self.target_lane_index, + route=self.route, + position=self.position, + np_random=self.road.np_random, + ) def steering_control(self, target_lane_index: LaneIndex) -> float: """ @@ -131,16 +160,30 @@ def steering_control(self, target_lane_index: LaneIndex) -> float: lane_future_heading = target_lane.heading_at(lane_next_coords) # Lateral position control - lateral_speed_command = - self.KP_LATERAL * lane_coords[1] + lateral_speed_command = -self.KP_LATERAL * lane_coords[1] # Lateral speed to heading - heading_command = np.arcsin(np.clip(lateral_speed_command / utils.not_zero(self.speed), -1, 1)) - heading_ref = lane_future_heading + np.clip(heading_command, -np.pi/4, np.pi/4) + heading_command = np.arcsin( + np.clip(lateral_speed_command / utils.not_zero(self.speed), -1, 1) + ) + heading_ref = lane_future_heading + np.clip( + heading_command, -np.pi / 4, np.pi / 4 + ) # Heading control - heading_rate_command = self.KP_HEADING * utils.wrap_to_pi(heading_ref - self.heading) + heading_rate_command = self.KP_HEADING * utils.wrap_to_pi( + heading_ref - self.heading + ) # Heading rate to steering angle - slip_angle = np.arcsin(np.clip(self.LENGTH / 2 / utils.not_zero(self.speed) * heading_rate_command, -1, 1)) + slip_angle = np.arcsin( + np.clip( + self.LENGTH / 2 / utils.not_zero(self.speed) * heading_rate_command, + -1, + 1, + ) + ) steering_angle = np.arctan(2 * np.tan(slip_angle)) - steering_angle = np.clip(steering_angle, -self.MAX_STEERING_ANGLE, self.MAX_STEERING_ANGLE) + steering_angle = np.clip( + steering_angle, -self.MAX_STEERING_ANGLE, self.MAX_STEERING_ANGLE + ) return float(steering_angle) def speed_control(self, target_speed: float) -> float: @@ -168,8 +211,11 @@ def get_routes_at_intersection(self) -> List[Route]: else: return [self.route] next_destinations_from = list(next_destinations.keys()) - routes = [self.route[0:index+1] + [(self.route[index][1], destination, self.route[index][2])] - for destination in next_destinations_from] + routes = [ + self.route[0 : index + 1] + + [(self.route[index][1], destination, self.route[index][2])] + for destination in next_destinations_from + ] return routes def set_route_at_intersection(self, _to: int) -> None: @@ -187,7 +233,9 @@ def set_route_at_intersection(self, _to: int) -> None: _to = self.road.np_random.integers(len(routes)) self.route = routes[_to % len(routes)] - def predict_trajectory_constant_speed(self, times: np.ndarray) -> Tuple[List[np.ndarray], List[float]]: + def predict_trajectory_constant_speed( + self, times: np.ndarray + ) -> Tuple[List[np.ndarray], List[float]]: """ Predict the future positions of the vehicle along its planned route, under constant speed @@ -196,28 +244,32 @@ def predict_trajectory_constant_speed(self, times: np.ndarray) -> Tuple[List[np. """ coordinates = self.lane.local_coordinates(self.position) route = self.route or [self.lane_index] - pos_heads = [self.road.network.position_heading_along_route(route, - coordinates[0] + self.speed * t, - 0, - self.lane_index) - for t in times] + pos_heads = [ + self.road.network.position_heading_along_route( + route, coordinates[0] + self.speed * t, 0, self.lane_index + ) + for t in times + ] return tuple(zip(*pos_heads)) class MDPVehicle(ControlledVehicle): """A controlled vehicle with a specified discrete range of allowed target speeds.""" + DEFAULT_TARGET_SPEEDS = np.linspace(20, 30, 3) - def __init__(self, - road: Road, - position: List[float], - heading: float = 0, - speed: float = 0, - target_lane_index: Optional[LaneIndex] = None, - target_speed: Optional[float] = None, - target_speeds: Optional[Vector] = None, - route: Optional[Route] = None) -> None: + def __init__( + self, + road: Road, + position: List[float], + heading: float = 0, + speed: float = 0, + target_lane_index: Optional[LaneIndex] = None, + target_speed: Optional[float] = None, + target_speeds: Optional[Vector] = None, + route: Optional[Route] = None, + ) -> None: """ Initializes an MDPVehicle @@ -230,8 +282,14 @@ def __init__(self, :param target_speeds: the discrete list of speeds the vehicle is able to track, through faster/slower actions :param route: the planned route of the vehicle, to handle intersections """ - super().__init__(road, position, heading, speed, target_lane_index, target_speed, route) - self.target_speeds = np.array(target_speeds) if target_speeds is not None else self.DEFAULT_TARGET_SPEEDS + super().__init__( + road, position, heading, speed, target_lane_index, target_speed, route + ) + self.target_speeds = ( + np.array(target_speeds) + if target_speeds is not None + else self.DEFAULT_TARGET_SPEEDS + ) self.speed_index = self.speed_to_index(self.target_speed) self.target_speed = self.index_to_speed(self.speed_index) @@ -251,7 +309,9 @@ def act(self, action: Union[dict, str] = None) -> None: else: super().act(action) return - self.speed_index = int(np.clip(self.speed_index, 0, self.target_speeds.size - 1)) + self.speed_index = int( + np.clip(self.speed_index, 0, self.target_speeds.size - 1) + ) self.target_speed = self.index_to_speed(self.speed_index) super().act() @@ -273,8 +333,16 @@ def speed_to_index(self, speed: float) -> int: :param speed: an input speed [m/s] :return: the index of the closest speed allowed [] """ - x = (speed - self.target_speeds[0]) / (self.target_speeds[-1] - self.target_speeds[0]) - return np.int64(np.clip(np.round(x * (self.target_speeds.size - 1)), 0, self.target_speeds.size - 1)) + x = (speed - self.target_speeds[0]) / ( + self.target_speeds[-1] - self.target_speeds[0] + ) + return np.int64( + np.clip( + np.round(x * (self.target_speeds.size - 1)), + 0, + self.target_speeds.size - 1, + ) + ) @classmethod def speed_to_index_default(cls, speed: float) -> int: @@ -286,16 +354,30 @@ def speed_to_index_default(cls, speed: float) -> int: :param speed: an input speed [m/s] :return: the index of the closest speed allowed [] """ - x = (speed - cls.DEFAULT_TARGET_SPEEDS[0]) / (cls.DEFAULT_TARGET_SPEEDS[-1] - cls.DEFAULT_TARGET_SPEEDS[0]) - return np.int64(np.clip( - np.round(x * (cls.DEFAULT_TARGET_SPEEDS.size - 1)), 0, cls.DEFAULT_TARGET_SPEEDS.size - 1)) + x = (speed - cls.DEFAULT_TARGET_SPEEDS[0]) / ( + cls.DEFAULT_TARGET_SPEEDS[-1] - cls.DEFAULT_TARGET_SPEEDS[0] + ) + return np.int64( + np.clip( + np.round(x * (cls.DEFAULT_TARGET_SPEEDS.size - 1)), + 0, + cls.DEFAULT_TARGET_SPEEDS.size - 1, + ) + ) @classmethod def get_speed_index(cls, vehicle: Vehicle) -> int: - return getattr(vehicle, "speed_index", cls.speed_to_index_default(vehicle.speed)) - - def predict_trajectory(self, actions: List, action_duration: float, trajectory_timestep: float, dt: float) \ - -> List[ControlledVehicle]: + return getattr( + vehicle, "speed_index", cls.speed_to_index_default(vehicle.speed) + ) + + def predict_trajectory( + self, + actions: List, + action_duration: float, + trajectory_timestep: float, + dt: float, + ) -> List[ControlledVehicle]: """ Predict the future trajectory of the vehicle given a sequence of actions. diff --git a/highway_env/vehicle/dynamics.py b/highway_env/vehicle/dynamics.py index 5840542bf..16a2e2a3d 100644 --- a/highway_env/vehicle/dynamics.py +++ b/highway_env/vehicle/dynamics.py @@ -1,7 +1,7 @@ -from typing import Tuple, Callable +from typing import Callable, Tuple -import numpy as np import matplotlib.pyplot as plt +import numpy as np from highway_env.road.road import Road from highway_env.utils import Vector @@ -30,20 +30,25 @@ def rk4(func: Callable, state: np.ndarray, dt: float = 0.01, t: float = 0, **kwa class BicycleVehicle(Vehicle): """ A dynamical bicycle model, with tire friction and slipping. - + See Chapter 2 of Lateral Vehicle Dynamics. Vehicle Dynamics and Control. Rajamani, R. (2011) """ + MASS: float = 1 # [kg] LENGTH_A: float = Vehicle.LENGTH / 2 # [m] LENGTH_B: float = Vehicle.LENGTH / 2 # [m] - INERTIA_Z: float = 1/12 * MASS * (Vehicle.LENGTH ** 2 + Vehicle.WIDTH ** 2) # [kg.m2] + INERTIA_Z: float = ( + 1 / 12 * MASS * (Vehicle.LENGTH**2 + Vehicle.WIDTH**2) + ) # [kg.m2] FRICTION_FRONT: float = 15.0 * MASS # [N] FRICTION_REAR: float = 15.0 * MASS # [N] MAX_ANGULAR_SPEED: float = 2 * np.pi # [rad/s] MAX_SPEED: float = 15 # [m/s] - def __init__(self, road: Road, position: Vector, heading: float = 0, speed: float = 0) -> None: + def __init__( + self, road: Road, position: Vector, heading: float = 0, speed: float = 0 + ) -> None: super().__init__(road, position, heading, speed) self.lateral_speed = 0 self.yaw_rate = 0 @@ -52,12 +57,16 @@ def __init__(self, road: Road, position: Vector, heading: float = 0, speed: floa @property def state(self) -> np.ndarray: - return np.array([[self.position[0]], - [self.position[1]], - [self.heading], - [self.speed], - [self.lateral_speed], - [self.yaw_rate]]) + return np.array( + [ + [self.position[0]], + [self.position[1]], + [self.heading], + [self.speed], + [self.lateral_speed], + [self.yaw_rate], + ] + ) @property def derivative(self): @@ -75,42 +84,61 @@ def derivative_func(self, time: float, state: np.ndarray, **kwargs) -> np.ndarra delta_r = 0 theta_vf = np.arctan2(lateral_speed + self.LENGTH_A * yaw_rate, speed) # (2.27) theta_vr = np.arctan2(lateral_speed - self.LENGTH_B * yaw_rate, speed) # (2.28) - f_yf = 2*self.FRICTION_FRONT * (delta_f - theta_vf) # (2.25) - f_yr = 2*self.FRICTION_REAR * (delta_r - theta_vr) # (2.26) + f_yf = 2 * self.FRICTION_FRONT * (delta_f - theta_vf) # (2.25) + f_yr = 2 * self.FRICTION_REAR * (delta_r - theta_vr) # (2.26) if abs(speed) < 1: # Low speed dynamics: damping of lateral speed and yaw rate - f_yf = - self.MASS * lateral_speed - self.INERTIA_Z/self.LENGTH_A * yaw_rate - f_yr = - self.MASS * lateral_speed + self.INERTIA_Z/self.LENGTH_A * yaw_rate - d_lateral_speed = 1/self.MASS * (f_yf + f_yr) - yaw_rate * speed # (2.21) - d_yaw_rate = 1/self.INERTIA_Z * (self.LENGTH_A * f_yf - self.LENGTH_B * f_yr) # (2.22) + f_yf = ( + -self.MASS * lateral_speed - self.INERTIA_Z / self.LENGTH_A * yaw_rate + ) + f_yr = ( + -self.MASS * lateral_speed + self.INERTIA_Z / self.LENGTH_A * yaw_rate + ) + d_lateral_speed = 1 / self.MASS * (f_yf + f_yr) - yaw_rate * speed # (2.21) + d_yaw_rate = ( + 1 / self.INERTIA_Z * (self.LENGTH_A * f_yf - self.LENGTH_B * f_yr) + ) # (2.22) c, s = np.cos(heading), np.sin(heading) R = np.array(((c, -s), (s, c))) speed = R @ np.array([speed, lateral_speed]) - return np.array([[speed[0]], - [speed[1]], - [yaw_rate], - [self.action['acceleration']], - [d_lateral_speed], - [d_yaw_rate]]) + return np.array( + [ + [speed[0]], + [speed[1]], + [yaw_rate], + [self.action["acceleration"]], + [d_lateral_speed], + [d_yaw_rate], + ] + ) @property def derivative_linear(self) -> np.ndarray: """ Linearized lateral dynamics. - + This model is based on the following assumptions: - the vehicle is moving with a constant longitudinal speed - the steering input to front tires and the corresponding slip angles are small - + See https://pdfs.semanticscholar.org/bb9c/d2892e9327ec1ee647c30c320f2089b290c1.pdf, Chapter 3. """ x = np.array([[self.lateral_speed], [self.yaw_rate]]) - u = np.array([[self.action['steering']]]) + u = np.array([[self.action["steering"]]]) self.A_lat, self.B_lat = self.lateral_lpv_dynamics() dx = self.A_lat @ x + self.B_lat @ u c, s = np.cos(self.heading), np.sin(self.heading) R = np.array(((c, -s), (s, c))) speed = R @ np.array([self.speed, self.lateral_speed]) - return np.array([[speed[0]], [speed[1]], [self.yaw_rate], [self.action['acceleration']], dx[0], dx[1]]) + return np.array( + [ + [speed[0]], + [speed[1]], + [self.yaw_rate], + [self.action["acceleration"]], + dx[0], + dx[1], + ] + ) def step(self, dt: float) -> None: self.clip_actions() @@ -126,8 +154,12 @@ def step(self, dt: float) -> None: def clip_actions(self) -> None: super().clip_actions() # Required because of the linearisation - self.action["steering"] = np.clip(self.action["steering"], -np.pi/2, np.pi/2) - self.yaw_rate = np.clip(self.yaw_rate, -self.MAX_ANGULAR_SPEED, self.MAX_ANGULAR_SPEED) + self.action["steering"] = np.clip( + self.action["steering"], -np.pi / 2, np.pi / 2 + ) + self.yaw_rate = np.clip( + self.yaw_rate, -self.MAX_ANGULAR_SPEED, self.MAX_ANGULAR_SPEED + ) def lateral_lpv_structure(self) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: """ @@ -135,29 +167,43 @@ def lateral_lpv_structure(self) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: :return: lateral dynamics A0, phi, B such that dx = (A0 + theta^T phi)x + B u """ - B = np.array([ - [2*self.FRICTION_FRONT / self.MASS], - [self.FRICTION_FRONT * self.LENGTH_A / self.INERTIA_Z] - ]) + B = np.array( + [ + [2 * self.FRICTION_FRONT / self.MASS], + [self.FRICTION_FRONT * self.LENGTH_A / self.INERTIA_Z], + ] + ) speed_body_x = self.speed - A0 = np.array([ - [0, -speed_body_x], - [0, 0] - ]) + A0 = np.array([[0, -speed_body_x], [0, 0]]) if abs(speed_body_x) < 1: - return A0, np.zeros((2, 2, 2)), B*0 + return A0, np.zeros((2, 2, 2)), B * 0 - phi = np.array([ + phi = np.array( [ - [-2 / (self.MASS*speed_body_x), -2*self.LENGTH_A / (self.MASS*speed_body_x)], - [-2*self.LENGTH_A / (self.INERTIA_Z*speed_body_x), -2*self.LENGTH_A**2 / (self.INERTIA_Z*speed_body_x)] - ], [ - [-2 / (self.MASS*speed_body_x), 2*self.LENGTH_B / (self.MASS*speed_body_x)], - [2*self.LENGTH_B / (self.INERTIA_Z*speed_body_x), -2*self.LENGTH_B**2 / (self.INERTIA_Z*speed_body_x)] - ], - ]) + [ + [ + -2 / (self.MASS * speed_body_x), + -2 * self.LENGTH_A / (self.MASS * speed_body_x), + ], + [ + -2 * self.LENGTH_A / (self.INERTIA_Z * speed_body_x), + -2 * self.LENGTH_A**2 / (self.INERTIA_Z * speed_body_x), + ], + ], + [ + [ + -2 / (self.MASS * speed_body_x), + 2 * self.LENGTH_B / (self.MASS * speed_body_x), + ], + [ + 2 * self.LENGTH_B / (self.INERTIA_Z * speed_body_x), + -2 * self.LENGTH_B**2 / (self.INERTIA_Z * speed_body_x), + ], + ], + ] + ) return A0, phi, B def lateral_lpv_dynamics(self) -> Tuple[np.ndarray, np.ndarray]: @@ -182,13 +228,19 @@ def full_lateral_lpv_structure(self) -> Tuple[np.ndarray, np.ndarray, np.ndarray A_lat, phi_lat, B_lat = self.lateral_lpv_structure() speed_body_x = self.speed - A_top = np.array([ - [0, speed_body_x, 1, 0], - [0, 0, 0, 1] - ]) + A_top = np.array([[0, speed_body_x, 1, 0], [0, 0, 0, 1]]) A0 = np.concatenate((A_top, np.concatenate((np.zeros((2, 2)), A_lat), axis=1))) - phi = np.array([np.concatenate((np.zeros((2, 4)), np.concatenate((np.zeros((2, 2)), phi_i), axis=1))) - for phi_i in phi_lat]) + phi = np.array( + [ + np.concatenate( + ( + np.zeros((2, 4)), + np.concatenate((np.zeros((2, 2)), phi_i), axis=1), + ) + ) + for phi_i in phi_lat + ] + ) B = np.concatenate((np.zeros((2, 1)), B_lat)) return A0, phi, B @@ -208,23 +260,39 @@ def full_lateral_lpv_dynamics(self) -> Tuple[np.ndarray, np.ndarray]: def simulate(dt: float = 0.1) -> None: import control + time = np.arange(0, 20, dt) vehicle = BicycleVehicle(road=None, position=[0, 5], speed=8.3) xx, uu = [], [] from highway_env.interval import LPV + A, B = vehicle.full_lateral_lpv_dynamics() K = -np.asarray(control.place(A, B, -np.arange(1, 5))) - lpv = LPV(x0=vehicle.state[[1, 2, 4, 5]].squeeze(), a0=A, da=[np.zeros(A.shape)], b=B, - d=[[0], [0], [0], [1]], omega_i=[[0], [0]], u=None, k=K, center=None, x_i=None) + lpv = LPV( + x0=vehicle.state[[1, 2, 4, 5]].squeeze(), + a0=A, + da=[np.zeros(A.shape)], + b=B, + d=[[0], [0], [0], [1]], + omega_i=[[0], [0]], + u=None, + k=K, + center=None, + x_i=None, + ) for t in time: # Act u = K @ vehicle.state[[1, 2, 4, 5]] - omega = 2*np.pi/20 - u_p = 0*np.array([[-20*omega*np.sin(omega*t) * dt]]) + omega = 2 * np.pi / 20 + u_p = 0 * np.array([[-20 * omega * np.sin(omega * t) * dt]]) u += u_p # Record - xx.append(np.array([vehicle.position[0], vehicle.position[1], vehicle.heading])[:, np.newaxis]) + xx.append( + np.array([vehicle.position[0], vehicle.position[1], vehicle.heading])[ + :, np.newaxis + ] + ) uu.append(u) # Interval lpv.set_control(u, state=vehicle.state[[1, 2, 4, 5]]) @@ -244,13 +312,29 @@ def plot(time: np.ndarray, xx: np.ndarray, uu: np.ndarray) -> None: dir_x, dir_y = np.cos(xx[:, 2, 0] + uu[:, 0, 0]), np.sin(xx[:, 2, 0] + uu[:, 0, 0]) _, ax = plt.subplots(1, 1) ax.plot(pos_x, pos_y, linewidth=0.5) - dir_scale = 1/5 - ax.quiver(pos_x[::20]-0.5/dir_scale*psi_x[::20], - pos_y[::20]-0.5/dir_scale*psi_y[::20], - psi_x[::20], psi_y[::20], - angles='xy', scale_units='xy', scale=dir_scale, width=0.005, headwidth=1) - ax.quiver(pos_x[::20]+0.5/dir_scale*psi_x[::20], pos_y[::20]+0.5/dir_scale*psi_y[::20], dir_x[::20], dir_y[::20], - angles='xy', scale_units='xy', scale=0.25, width=0.005, color='r') + dir_scale = 1 / 5 + ax.quiver( + pos_x[::20] - 0.5 / dir_scale * psi_x[::20], + pos_y[::20] - 0.5 / dir_scale * psi_y[::20], + psi_x[::20], + psi_y[::20], + angles="xy", + scale_units="xy", + scale=dir_scale, + width=0.005, + headwidth=1, + ) + ax.quiver( + pos_x[::20] + 0.5 / dir_scale * psi_x[::20], + pos_y[::20] + 0.5 / dir_scale * psi_y[::20], + dir_x[::20], + dir_y[::20], + angles="xy", + scale_units="xy", + scale=0.25, + width=0.005, + color="r", + ) ax.axis("equal") ax.grid() plt.show() @@ -261,5 +345,5 @@ def main() -> None: simulate() -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/highway_env/vehicle/graphics.py b/highway_env/vehicle/graphics.py index 693bc23be..c6e3fa0cc 100644 --- a/highway_env/vehicle/graphics.py +++ b/highway_env/vehicle/graphics.py @@ -1,14 +1,14 @@ import itertools -from typing import List, Tuple, TYPE_CHECKING +from typing import TYPE_CHECKING, List, Tuple import numpy as np import pygame from highway_env.utils import Vector +from highway_env.vehicle.behavior import IDMVehicle, LinearVehicle +from highway_env.vehicle.controller import MDPVehicle from highway_env.vehicle.dynamics import BicycleVehicle from highway_env.vehicle.kinematics import Vehicle -from highway_env.vehicle.controller import ControlledVehicle, MDPVehicle -from highway_env.vehicle.behavior import IDMVehicle, LinearVehicle if TYPE_CHECKING: from highway_env.road.graphics import WorldSurface @@ -25,11 +25,15 @@ class VehicleGraphics(object): EGO_COLOR = GREEN @classmethod - def display(cls, vehicle: Vehicle, surface: "WorldSurface", - transparent: bool = False, - offscreen: bool = False, - label: bool = False, - draw_roof: bool = False) -> None: + def display( + cls, + vehicle: Vehicle, + surface: "WorldSurface", + transparent: bool = False, + offscreen: bool = False, + label: bool = False, + draw_roof: bool = False, + ) -> None: """ Display a vehicle on a pygame surface. @@ -51,44 +55,74 @@ def display(cls, vehicle: Vehicle, surface: "WorldSurface", # Vehicle rectangle length = v.LENGTH + 2 * tire_length - vehicle_surface = pygame.Surface((surface.pix(length), surface.pix(length)), - flags=pygame.SRCALPHA) # per-pixel alpha - rect = (surface.pix(tire_length), - surface.pix(length / 2 - v.WIDTH / 2), - surface.pix(v.LENGTH), - surface.pix(v.WIDTH)) - rect_headlight_left = (surface.pix(tire_length+v.LENGTH-headlight_length), - surface.pix(length / 2 - (1.4*v.WIDTH) / 3), - surface.pix(headlight_length), - surface.pix(headlight_width)) - rect_headlight_right = (surface.pix(tire_length+v.LENGTH-headlight_length), - surface.pix(length / 2 + (0.6*v.WIDTH) / 5), - surface.pix(headlight_length), - surface.pix(headlight_width)) + vehicle_surface = pygame.Surface( + (surface.pix(length), surface.pix(length)), flags=pygame.SRCALPHA + ) # per-pixel alpha + rect = ( + surface.pix(tire_length), + surface.pix(length / 2 - v.WIDTH / 2), + surface.pix(v.LENGTH), + surface.pix(v.WIDTH), + ) + rect_headlight_left = ( + surface.pix(tire_length + v.LENGTH - headlight_length), + surface.pix(length / 2 - (1.4 * v.WIDTH) / 3), + surface.pix(headlight_length), + surface.pix(headlight_width), + ) + rect_headlight_right = ( + surface.pix(tire_length + v.LENGTH - headlight_length), + surface.pix(length / 2 + (0.6 * v.WIDTH) / 5), + surface.pix(headlight_length), + surface.pix(headlight_width), + ) color = cls.get_color(v, transparent) pygame.draw.rect(vehicle_surface, color, rect, 0) pygame.draw.rect(vehicle_surface, cls.lighten(color), rect_headlight_left, 0) pygame.draw.rect(vehicle_surface, cls.lighten(color), rect_headlight_right, 0) if draw_roof: - rect_roof = (surface.pix(v.LENGTH/2 - tire_length/2), - surface.pix(0.999*length/ 2 - 0.38625*v.WIDTH), - surface.pix(roof_length), - surface.pix(roof_width)) + rect_roof = ( + surface.pix(v.LENGTH / 2 - tire_length / 2), + surface.pix(0.999 * length / 2 - 0.38625 * v.WIDTH), + surface.pix(roof_length), + surface.pix(roof_width), + ) pygame.draw.rect(vehicle_surface, cls.darken(color), rect_roof, 0) pygame.draw.rect(vehicle_surface, cls.BLACK, rect, 1) # Tires if type(vehicle) in [Vehicle, BicycleVehicle]: - tire_positions = [[surface.pix(tire_length), surface.pix(length / 2 - v.WIDTH / 2)], - [surface.pix(tire_length), surface.pix(length / 2 + v.WIDTH / 2)], - [surface.pix(length - tire_length), surface.pix(length / 2 - v.WIDTH / 2)], - [surface.pix(length - tire_length), surface.pix(length / 2 + v.WIDTH / 2)]] + tire_positions = [ + [surface.pix(tire_length), surface.pix(length / 2 - v.WIDTH / 2)], + [surface.pix(tire_length), surface.pix(length / 2 + v.WIDTH / 2)], + [ + surface.pix(length - tire_length), + surface.pix(length / 2 - v.WIDTH / 2), + ], + [ + surface.pix(length - tire_length), + surface.pix(length / 2 + v.WIDTH / 2), + ], + ] tire_angles = [0, 0, v.action["steering"], v.action["steering"]] for tire_position, tire_angle in zip(tire_positions, tire_angles): - tire_surface = pygame.Surface((surface.pix(tire_length), surface.pix(tire_length)), pygame.SRCALPHA) - rect = (0, surface.pix(tire_length/2-tire_width/2), surface.pix(tire_length), surface.pix(tire_width)) + tire_surface = pygame.Surface( + (surface.pix(tire_length), surface.pix(tire_length)), + pygame.SRCALPHA, + ) + rect = ( + 0, + surface.pix(tire_length / 2 - tire_width / 2), + surface.pix(tire_length), + surface.pix(tire_width), + ) pygame.draw.rect(tire_surface, cls.BLACK, rect, 0) - cls.blit_rotate(vehicle_surface, tire_surface, tire_position, np.rad2deg(-tire_angle)) + cls.blit_rotate( + vehicle_surface, + tire_surface, + tire_position, + np.rad2deg(-tire_angle), + ) # Centered rotation h = v.heading if abs(v.heading) > 2 * np.pi / 180 else 0 @@ -107,15 +141,27 @@ def display(cls, vehicle: Vehicle, surface: "WorldSurface", surface.blit(text, position) @staticmethod - def blit_rotate(surf: pygame.SurfaceType, image: pygame.SurfaceType, pos: Vector, angle: float, - origin_pos: Vector = None, show_rect: bool = False) -> None: + def blit_rotate( + surf: pygame.SurfaceType, + image: pygame.SurfaceType, + pos: Vector, + angle: float, + origin_pos: Vector = None, + show_rect: bool = False, + ) -> None: """Many thanks to https://stackoverflow.com/a/54714144.""" # calculate the axis aligned bounding box of the rotated image w, h = image.get_size() box = [pygame.math.Vector2(p) for p in [(0, 0), (w, 0), (w, -h), (0, -h)]] box_rotate = [p.rotate(angle) for p in box] - min_box = (min(box_rotate, key=lambda p: p[0])[0], min(box_rotate, key=lambda p: p[1])[1]) - max_box = (max(box_rotate, key=lambda p: p[0])[0], max(box_rotate, key=lambda p: p[1])[1]) + min_box = ( + min(box_rotate, key=lambda p: p[0])[0], + min(box_rotate, key=lambda p: p[1])[1], + ) + max_box = ( + max(box_rotate, key=lambda p: p[0])[0], + max(box_rotate, key=lambda p: p[1])[1], + ) # calculate the translation of the pivot if origin_pos is None: @@ -125,7 +171,10 @@ def blit_rotate(surf: pygame.SurfaceType, image: pygame.SurfaceType, pos: Vector pivot_move = pivot_rotate - pivot # calculate the upper left origin of the rotated image - origin = (pos[0] - origin_pos[0] + min_box[0] - pivot_move[0], pos[1] - origin_pos[1] - max_box[1] + pivot_move[1]) + origin = ( + pos[0] - origin_pos[0] + min_box[0] - pivot_move[0], + pos[1] - origin_pos[1] - max_box[1] + pivot_move[1], + ) # get a rotated image rotated_image = pygame.transform.rotate(image, angle) # rotate and blit the image @@ -135,7 +184,9 @@ def blit_rotate(surf: pygame.SurfaceType, image: pygame.SurfaceType, pos: Vector pygame.draw.rect(surf, (255, 0, 0), (*origin, *rotated_image.get_size()), 2) @classmethod - def display_trajectory(cls, states: List[Vehicle], surface: "WorldSurface", offscreen: bool = False) -> None: + def display_trajectory( + cls, states: List[Vehicle], surface: "WorldSurface", offscreen: bool = False + ) -> None: """ Display the whole trajectory of a vehicle on a pygame surface. @@ -147,8 +198,15 @@ def display_trajectory(cls, states: List[Vehicle], surface: "WorldSurface", offs cls.display(vehicle, surface, transparent=True, offscreen=offscreen) @classmethod - def display_history(cls, vehicle: Vehicle, surface: "WorldSurface", frequency: float = 3, duration: float = 2, - simulation: int = 15, offscreen: bool = False) -> None: + def display_history( + cls, + vehicle: Vehicle, + surface: "WorldSurface", + frequency: float = 3, + duration: float = 2, + simulation: int = 15, + offscreen: bool = False, + ) -> None: """ Display the whole trajectory of a vehicle on a pygame surface. @@ -159,10 +217,12 @@ def display_history(cls, vehicle: Vehicle, surface: "WorldSurface", frequency: f :param simulation: simulation frequency :param offscreen: whether the rendering should be done offscreen or not """ - for v in itertools.islice(vehicle.history, - None, - int(simulation * duration), - int(simulation / frequency)): + for v in itertools.islice( + vehicle.history, + None, + int(simulation * duration), + int(simulation / frequency), + ): cls.display(v, surface, transparent=True, offscreen=offscreen) @classmethod diff --git a/highway_env/vehicle/kinematics.py b/highway_env/vehicle/kinematics.py index af7c31dfe..7a24f0b13 100644 --- a/highway_env/vehicle/kinematics.py +++ b/highway_env/vehicle/kinematics.py @@ -1,12 +1,12 @@ -from typing import Union, Optional, Tuple, List -import numpy as np import copy from collections import deque +from typing import List, Optional, Tuple, Union + +import numpy as np -from highway_env import utils -from highway_env.road.road import Road, LaneIndex -from highway_env.vehicle.objects import RoadObject, Obstacle, Landmark +from highway_env.road.road import Road from highway_env.utils import Vector +from highway_env.vehicle.objects import RoadObject class Vehicle(RoadObject): @@ -24,35 +24,39 @@ class Vehicle(RoadObject): """ Vehicle width [m] """ DEFAULT_INITIAL_SPEEDS = [23, 25] """ Range for random initial speeds [m/s] """ - MAX_SPEED = 40. + MAX_SPEED = 40.0 """ Maximum reachable speed [m/s] """ - MIN_SPEED = -40. + MIN_SPEED = -40.0 """ Minimum reachable speed [m/s] """ HISTORY_SIZE = 30 """ Length of the vehicle state history, for trajectory display""" - def __init__(self, - road: Road, - position: Vector, - heading: float = 0, - speed: float = 0, - predition_type: str = 'constant_steering'): + def __init__( + self, + road: Road, + position: Vector, + heading: float = 0, + speed: float = 0, + predition_type: str = "constant_steering", + ): super().__init__(road, position, heading, speed) self.prediction_type = predition_type - self.action = {'steering': 0, 'acceleration': 0} + self.action = {"steering": 0, "acceleration": 0} self.crashed = False self.impact = None self.log = [] self.history = deque(maxlen=self.HISTORY_SIZE) @classmethod - def create_random(cls, road: Road, - speed: float = None, - lane_from: Optional[str] = None, - lane_to: Optional[str] = None, - lane_id: Optional[int] = None, - spacing: float = 1) \ - -> "Vehicle": + def create_random( + cls, + road: Road, + speed: float = None, + lane_from: Optional[str] = None, + lane_to: Optional[str] = None, + lane_id: Optional[int] = None, + spacing: float = 1, + ) -> "Vehicle": """ Create a random vehicle on the road. @@ -69,17 +73,32 @@ def create_random(cls, road: Road, """ _from = lane_from or road.np_random.choice(list(road.network.graph.keys())) _to = lane_to or road.np_random.choice(list(road.network.graph[_from].keys())) - _id = lane_id if lane_id is not None else road.np_random.choice(len(road.network.graph[_from][_to])) + _id = ( + lane_id + if lane_id is not None + else road.np_random.choice(len(road.network.graph[_from][_to])) + ) lane = road.network.get_lane((_from, _to, _id)) if speed is None: if lane.speed_limit is not None: - speed = road.np_random.uniform(0.7*lane.speed_limit, 0.8*lane.speed_limit) + speed = road.np_random.uniform( + 0.7 * lane.speed_limit, 0.8 * lane.speed_limit + ) else: - speed = road.np_random.uniform(Vehicle.DEFAULT_INITIAL_SPEEDS[0], Vehicle.DEFAULT_INITIAL_SPEEDS[1]) - default_spacing = 12+1.0*speed - offset = spacing * default_spacing * np.exp(-5 / 40 * len(road.network.graph[_from][_to])) - x0 = np.max([lane.local_coordinates(v.position)[0] for v in road.vehicles]) \ - if len(road.vehicles) else 3*offset + speed = road.np_random.uniform( + Vehicle.DEFAULT_INITIAL_SPEEDS[0], Vehicle.DEFAULT_INITIAL_SPEEDS[1] + ) + default_spacing = 12 + 1.0 * speed + offset = ( + spacing + * default_spacing + * np.exp(-5 / 40 * len(road.network.graph[_from][_to])) + ) + x0 = ( + np.max([lane.local_coordinates(v.position)[0] for v in road.vehicles]) + if len(road.vehicles) + else 3 * offset + ) x0 += offset * road.np_random.uniform(0.9, 1.1) v = cls(road, lane.position(x0, 0), lane.heading_at(x0), speed) return v @@ -95,7 +114,7 @@ def create_from(cls, vehicle: "Vehicle") -> "Vehicle": :return: a new vehicle at the same dynamical state """ v = cls(vehicle.road, vehicle.position, vehicle.heading, vehicle.speed) - if hasattr(vehicle, 'color'): + if hasattr(vehicle, "color"): v.color = vehicle.color return v @@ -119,42 +138,51 @@ def step(self, dt: float) -> None: :param dt: timestep of integration of the model [s] """ self.clip_actions() - delta_f = self.action['steering'] + delta_f = self.action["steering"] beta = np.arctan(1 / 2 * np.tan(delta_f)) - v = self.speed * np.array([np.cos(self.heading + beta), - np.sin(self.heading + beta)]) + v = self.speed * np.array( + [np.cos(self.heading + beta), np.sin(self.heading + beta)] + ) self.position += v * dt if self.impact is not None: self.position += self.impact self.crashed = True self.impact = None self.heading += self.speed * np.sin(beta) / (self.LENGTH / 2) * dt - self.speed += self.action['acceleration'] * dt + self.speed += self.action["acceleration"] * dt self.on_state_update() def clip_actions(self) -> None: if self.crashed: - self.action['steering'] = 0 - self.action['acceleration'] = -1.0*self.speed - self.action['steering'] = float(self.action['steering']) - self.action['acceleration'] = float(self.action['acceleration']) + self.action["steering"] = 0 + self.action["acceleration"] = -1.0 * self.speed + self.action["steering"] = float(self.action["steering"]) + self.action["acceleration"] = float(self.action["acceleration"]) if self.speed > self.MAX_SPEED: - self.action['acceleration'] = min(self.action['acceleration'], 1.0 * (self.MAX_SPEED - self.speed)) + self.action["acceleration"] = min( + self.action["acceleration"], 1.0 * (self.MAX_SPEED - self.speed) + ) elif self.speed < self.MIN_SPEED: - self.action['acceleration'] = max(self.action['acceleration'], 1.0 * (self.MIN_SPEED - self.speed)) + self.action["acceleration"] = max( + self.action["acceleration"], 1.0 * (self.MIN_SPEED - self.speed) + ) def on_state_update(self) -> None: if self.road: - self.lane_index = self.road.network.get_closest_lane_index(self.position, self.heading) + self.lane_index = self.road.network.get_closest_lane_index( + self.position, self.heading + ) self.lane = self.road.network.get_lane(self.lane_index) if self.road.record_history: self.history.appendleft(self.create_from(self)) - def predict_trajectory_constant_speed(self, times: np.ndarray) -> Tuple[List[np.ndarray], List[float]]: - if self.prediction_type == 'zero_steering': - action = {'acceleration': 0.0, 'steering': 0.0} - elif self.prediction_type == 'constant_steering': - action = {'acceleration': 0.0, 'steering': self.action['steering']} + def predict_trajectory_constant_speed( + self, times: np.ndarray + ) -> Tuple[List[np.ndarray], List[float]]: + if self.prediction_type == "zero_steering": + action = {"acceleration": 0.0, "steering": 0.0} + elif self.prediction_type == "constant_steering": + action = {"acceleration": 0.0, "steering": self.action["steering"]} else: raise ValueError("Unknown predition type") @@ -178,7 +206,11 @@ def velocity(self) -> np.ndarray: def destination(self) -> np.ndarray: if getattr(self, "route", None): last_lane_index = self.route[-1] - last_lane_index = last_lane_index if last_lane_index[-1] is not None else (*last_lane_index[:-1], 0) + last_lane_index = ( + last_lane_index + if last_lane_index[-1] is not None + else (*last_lane_index[:-1], 0) + ) last_lane = self.road.network.get_lane(last_lane_index) return last_lane.position(last_lane.length, 0) else: @@ -187,7 +219,9 @@ def destination(self) -> np.ndarray: @property def destination_direction(self) -> np.ndarray: if (self.destination != self.position).any(): - return (self.destination - self.position) / np.linalg.norm(self.destination - self.position) + return (self.destination - self.position) / np.linalg.norm( + self.destination - self.position + ) else: return np.zeros((2,)) @@ -200,38 +234,47 @@ def lane_offset(self) -> np.ndarray: else: return np.zeros((3,)) - def to_dict(self, origin_vehicle: "Vehicle" = None, observe_intentions: bool = True) -> dict: + def to_dict( + self, origin_vehicle: "Vehicle" = None, observe_intentions: bool = True + ) -> dict: d = { - 'presence': 1, - 'x': self.position[0], - 'y': self.position[1], - 'vx': self.velocity[0], - 'vy': self.velocity[1], - 'heading': self.heading, - 'cos_h': self.direction[0], - 'sin_h': self.direction[1], - 'cos_d': self.destination_direction[0], - 'sin_d': self.destination_direction[1], - 'long_off': self.lane_offset[0], - 'lat_off': self.lane_offset[1], - 'ang_off': self.lane_offset[2], + "presence": 1, + "x": self.position[0], + "y": self.position[1], + "vx": self.velocity[0], + "vy": self.velocity[1], + "heading": self.heading, + "cos_h": self.direction[0], + "sin_h": self.direction[1], + "cos_d": self.destination_direction[0], + "sin_d": self.destination_direction[1], + "long_off": self.lane_offset[0], + "lat_off": self.lane_offset[1], + "ang_off": self.lane_offset[2], } if not observe_intentions: d["cos_d"] = d["sin_d"] = 0 if origin_vehicle: origin_dict = origin_vehicle.to_dict() - for key in ['x', 'y', 'vx', 'vy']: + for key in ["x", "y", "vx", "vy"]: d[key] -= origin_dict[key] return d def __str__(self): - return "{} #{}: {}".format(self.__class__.__name__, id(self) % 1000, self.position) + return "{} #{}: {}".format( + self.__class__.__name__, id(self) % 1000, self.position + ) def __repr__(self): return self.__str__() - def predict_trajectory(self, actions: List, action_duration: float, trajectory_timestep: float, dt: float) \ - -> List['Vehicle']: + def predict_trajectory( + self, + actions: List, + action_duration: float, + trajectory_timestep: float, + dt: float, + ) -> List["Vehicle"]: """ Predict the future trajectory of the vehicle given a sequence of actions. diff --git a/highway_env/vehicle/objects.py b/highway_env/vehicle/objects.py index 1eba65201..16655a85e 100644 --- a/highway_env/vehicle/objects.py +++ b/highway_env/vehicle/objects.py @@ -1,5 +1,6 @@ from abc import ABC -from typing import Sequence, Tuple, TYPE_CHECKING, Optional +from typing import TYPE_CHECKING, Optional, Sequence, Tuple + import numpy as np from highway_env import utils @@ -22,7 +23,13 @@ class RoadObject(ABC): LENGTH: float = 2 # Object length [m] WIDTH: float = 2 # Object width [m] - def __init__(self, road: 'Road', position: Sequence[float], heading: float = 0, speed: float = 0): + def __init__( + self, + road: "Road", + position: Sequence[float], + heading: float = 0, + speed: float = 0, + ): """ :param road: the road instance where the object is placed in :param position: cartesian position of object in the surface @@ -33,7 +40,11 @@ def __init__(self, road: 'Road', position: Sequence[float], heading: float = 0, self.position = np.array(position, dtype=np.float64) self.heading = heading self.speed = speed - self.lane_index = self.road.network.get_closest_lane_index(self.position, self.heading) if self.road else np.nan + self.lane_index = ( + self.road.network.get_closest_lane_index(self.position, self.heading) + if self.road + else np.nan + ) self.lane = self.road.network.get_lane(self.lane_index) if self.road else None # Enable collision with other collidables @@ -52,8 +63,13 @@ def __init__(self, road: 'Road', position: Sequence[float], heading: float = 0, self.impact = np.zeros(self.position.shape) @classmethod - def make_on_lane(cls, road: 'Road', lane_index: LaneIndex, longitudinal: float, speed: Optional[float] = None) \ - -> 'RoadObject': + def make_on_lane( + cls, + road: "Road", + lane_index: LaneIndex, + longitudinal: float, + speed: Optional[float] = None, + ) -> "RoadObject": """ Create a vehicle on a given lane at a longitudinal position. @@ -66,9 +82,11 @@ def make_on_lane(cls, road: 'Road', lane_index: LaneIndex, longitudinal: float, lane = road.network.get_lane(lane_index) if speed is None: speed = lane.speed_limit - return cls(road, lane.position(longitudinal, 0), lane.heading_at(longitudinal), speed) + return cls( + road, lane.position(longitudinal, 0), lane.heading_at(longitudinal), speed + ) - def handle_collisions(self, other: 'RoadObject', dt: float = 0) -> None: + def handle_collisions(self, other: "RoadObject", dt: float = 0) -> None: """ Check for collision with another vehicle. @@ -100,29 +118,40 @@ def handle_collisions(self, other: 'RoadObject', dt: float = 0) -> None: def _is_colliding(self, other, dt): # Fast spherical pre-check - if np.linalg.norm(other.position - self.position) > (self.diagonal + other.diagonal) / 2 + self.speed * dt: - return False, False, np.zeros(2,) + if ( + np.linalg.norm(other.position - self.position) + > (self.diagonal + other.diagonal) / 2 + self.speed * dt + ): + return ( + False, + False, + np.zeros( + 2, + ), + ) # Accurate rectangular check - return utils.are_polygons_intersecting(self.polygon(), other.polygon(), self.velocity * dt, other.velocity * dt) + return utils.are_polygons_intersecting( + self.polygon(), other.polygon(), self.velocity * dt, other.velocity * dt + ) # Just added for sake of compatibility def to_dict(self, origin_vehicle=None, observe_intentions=True): d = { - 'presence': 1, - 'x': self.position[0], - 'y': self.position[1], - 'vx': 0., - 'vy': 0., - 'cos_h': np.cos(self.heading), - 'sin_h': np.sin(self.heading), - 'cos_d': 0., - 'sin_d': 0. + "presence": 1, + "x": self.position[0], + "y": self.position[1], + "vx": 0.0, + "vy": 0.0, + "cos_h": np.cos(self.heading), + "sin_h": np.sin(self.heading), + "cos_d": 0.0, + "sin_d": 0.0, } if not observe_intentions: d["cos_d"] = d["sin_d"] = 0 if origin_vehicle: origin_dict = origin_vehicle.to_dict() - for key in ['x', 'y', 'vx', 'vy']: + for key in ["x", "y", "vx", "vy"]: d[key] -= origin_dict[key] return d @@ -135,21 +164,22 @@ def velocity(self) -> np.ndarray: return self.speed * self.direction def polygon(self) -> np.ndarray: - points = np.array([ - [-self.LENGTH / 2, -self.WIDTH / 2], - [-self.LENGTH / 2, +self.WIDTH / 2], - [+self.LENGTH / 2, +self.WIDTH / 2], - [+self.LENGTH / 2, -self.WIDTH / 2], - ]).T + points = np.array( + [ + [-self.LENGTH / 2, -self.WIDTH / 2], + [-self.LENGTH / 2, +self.WIDTH / 2], + [+self.LENGTH / 2, +self.WIDTH / 2], + [+self.LENGTH / 2, -self.WIDTH / 2], + ] + ).T c, s = np.cos(self.heading), np.sin(self.heading) - rotation = np.array([ - [c, -s], - [s, c] - ]) + rotation = np.array([[c, -s], [s, c]]) points = (rotation @ points).T + np.tile(self.position, (4, 1)) return np.vstack([points, points[0:1]]) - def lane_distance_to(self, other: 'RoadObject', lane: 'AbstractLane' = None) -> float: + def lane_distance_to( + self, other: "RoadObject", lane: "AbstractLane" = None + ) -> float: """ Compute the signed distance to another object along a lane. @@ -161,11 +191,14 @@ def lane_distance_to(self, other: 'RoadObject', lane: 'AbstractLane' = None) -> return np.nan if not lane: lane = self.lane - return lane.local_coordinates(other.position)[0] - lane.local_coordinates(self.position)[0] + return ( + lane.local_coordinates(other.position)[0] + - lane.local_coordinates(self.position)[0] + ) @property def on_road(self) -> bool: - """ Is the object on its current lane, or off-road? """ + """Is the object on its current lane, or off-road?""" return self.lane.on_lane(self.position) def front_distance_to(self, other: "RoadObject") -> float: @@ -182,7 +215,9 @@ class Obstacle(RoadObject): """Obstacles on the road.""" - def __init__(self, road, position: Sequence[float], heading: float = 0, speed: float = 0): + def __init__( + self, road, position: Sequence[float], heading: float = 0, speed: float = 0 + ): super().__init__(road, position, heading, speed) self.solid = True @@ -191,7 +226,8 @@ class Landmark(RoadObject): """Landmarks of certain areas on the road that must be reached.""" - def __init__(self, road, position: Sequence[float], heading: float = 0, speed: float = 0): + def __init__( + self, road, position: Sequence[float], heading: float = 0, speed: float = 0 + ): super().__init__(road, position, heading, speed) self.solid = False - diff --git a/highway_env/vehicle/uncertainty/estimation.py b/highway_env/vehicle/uncertainty/estimation.py index cccc0dc45..865271528 100644 --- a/highway_env/vehicle/uncertainty/estimation.py +++ b/highway_env/vehicle/uncertainty/estimation.py @@ -2,8 +2,8 @@ import numpy as np -from highway_env.road.road import Road, LaneIndex, Route -from highway_env.utils import confidence_polytope, is_consistent_dataset, Vector +from highway_env.road.road import LaneIndex, Road, Route +from highway_env.utils import Vector, confidence_polytope, is_consistent_dataset from highway_env.vehicle.behavior import LinearVehicle from highway_env.vehicle.uncertainty.prediction import IntervalVehicle, Polytope @@ -13,16 +13,23 @@ class RegressionVehicle(IntervalVehicle): """Estimator for the parameter of a LinearVehicle.""" def longitudinal_matrix_polytope(self) -> Polytope: - return self.polytope_from_estimation(self.data["longitudinal"], self.theta_a_i, self.longitudinal_structure) + return self.polytope_from_estimation( + self.data["longitudinal"], self.theta_a_i, self.longitudinal_structure + ) def lateral_matrix_polytope(self) -> Polytope: - return self.polytope_from_estimation(self.data["lateral"], self.theta_b_i, self.lateral_structure) + return self.polytope_from_estimation( + self.data["lateral"], self.theta_b_i, self.lateral_structure + ) - def polytope_from_estimation(self, data: dict, parameter_box: np.ndarray, structure: Callable[[], Polytope])\ - -> Polytope: + def polytope_from_estimation( + self, data: dict, parameter_box: np.ndarray, structure: Callable[[], Polytope] + ) -> Polytope: if not data: return self.parameter_box_to_polytope(parameter_box, structure) - theta_n_lambda, d_theta, _, _ = confidence_polytope(data, parameter_box=parameter_box) + theta_n_lambda, d_theta, _, _ = confidence_polytope( + data, parameter_box=parameter_box + ) a, phi = structure() a0 = a + np.tensordot(theta_n_lambda, phi, axes=[0, 0]) da = [np.tensordot(d_theta_k, phi, axes=[0, 0]) for d_theta_k in d_theta] @@ -30,18 +37,31 @@ def polytope_from_estimation(self, data: dict, parameter_box: np.ndarray, struct class MultipleModelVehicle(LinearVehicle): - def __init__(self, road: Road, - position: Vector, - heading: float = 0, - speed: float = 0, - target_lane_index: LaneIndex = None, - target_speed: float = None, - route: Route = None, - enable_lane_change: bool = True, - timer: bool = None, - data: dict = None) -> None: - super().__init__(road, position, heading, speed, target_lane_index, target_speed, route, - enable_lane_change, timer, data) + def __init__( + self, + road: Road, + position: Vector, + heading: float = 0, + speed: float = 0, + target_lane_index: LaneIndex = None, + target_speed: float = None, + route: Route = None, + enable_lane_change: bool = True, + timer: bool = None, + data: dict = None, + ) -> None: + super().__init__( + road, + position, + heading, + speed, + target_lane_index, + target_speed, + route, + enable_lane_change, + timer, + data, + ) if not self.data: self.data = [] @@ -67,13 +87,22 @@ def update_possible_routes(self) -> None: for route in self.get_routes_at_intersection(): # Candidates # Unknown lane -> first lane for i, lane_index in enumerate(route): - route[i] = lane_index if lane_index[2] is not None else (lane_index[0], lane_index[1], 0) + route[i] = ( + lane_index + if lane_index[2] is not None + else (lane_index[0], lane_index[1], 0) + ) # Is this route already considered, or a suffix of a route already considered ? for known_route, _ in self.data: if known_route == route: break - elif len(known_route) < len(route) and route[:len(known_route)] == known_route: - self.data = [(r, d) if r != known_route else (route, d) for r, d in self.data] + elif ( + len(known_route) < len(route) + and route[: len(known_route)] == known_route + ): + self.data = [ + (r, d) if r != known_route else (route, d) for r, d in self.data + ] break else: self.data.append((route.copy(), {})) # Add it @@ -86,7 +115,9 @@ def update_possible_routes(self) -> None: # Reject inconsistent hypotheses for route, data in self.data.copy(): if data: - if not is_consistent_dataset(data["lateral"], parameter_box=LinearVehicle.STEERING_RANGE): + if not is_consistent_dataset( + data["lateral"], parameter_box=LinearVehicle.STEERING_RANGE + ): self.data.remove((route, data)) def assume_model_is_valid(self, index: int) -> "LinearVehicle": @@ -98,7 +129,7 @@ def assume_model_is_valid(self, index: int) -> "LinearVehicle": """ if not self.data: return self.create_from(self) - index = min(index, len(self.data)-1) + index = min(index, len(self.data) - 1) route, data = self.data[index] vehicle = RegressionVehicle.create_from(self) vehicle.target_lane_index = route[0] diff --git a/highway_env/vehicle/uncertainty/prediction.py b/highway_env/vehicle/uncertainty/prediction.py index 1c7b372cc..2f158f17b 100644 --- a/highway_env/vehicle/uncertainty/prediction.py +++ b/highway_env/vehicle/uncertainty/prediction.py @@ -1,12 +1,21 @@ import copy -from typing import List, Tuple, Callable, Union, TYPE_CHECKING +from typing import TYPE_CHECKING, Callable, List, Tuple + import numpy as np from highway_env import utils -from highway_env.interval import polytope, vector_interval_section, integrator_interval, \ - interval_negative_part, intervals_diff, intervals_product, LPV, interval_absolute_to_local, \ - interval_local_to_absolute -from highway_env.road.road import Route, LaneIndex, Road +from highway_env.interval import ( + LPV, + integrator_interval, + interval_absolute_to_local, + interval_local_to_absolute, + interval_negative_part, + intervals_diff, + intervals_product, + polytope, + vector_interval_section, +) +from highway_env.road.road import LaneIndex, Road, Route from highway_env.utils import Vector from highway_env.vehicle.behavior import LinearVehicle from highway_env.vehicle.controller import MDPVehicle @@ -28,34 +37,42 @@ class IntervalVehicle(LinearVehicle): are only used for storage of the bounds. """ - def __init__(self, - road: Road, - position: Vector, - heading: float = 0, - speed: float = 0, - target_lane_index: LaneIndex = None, - target_speed: float = None, - route: Route = None, - enable_lane_change: bool = True, - timer: float = None, - theta_a_i: List[List[float]] = None, - theta_b_i: List[List[float]] = None, - data: dict = None) -> None: + def __init__( + self, + road: Road, + position: Vector, + heading: float = 0, + speed: float = 0, + target_lane_index: LaneIndex = None, + target_speed: float = None, + route: Route = None, + enable_lane_change: bool = True, + timer: float = None, + theta_a_i: List[List[float]] = None, + theta_b_i: List[List[float]] = None, + data: dict = None, + ) -> None: """ :param theta_a_i: The interval of possible acceleration parameters :param theta_b_i: The interval of possible steering parameters """ - super().__init__(road, - position, - heading, - speed, - target_lane_index, - target_speed, - route, - enable_lane_change, - timer) - self.theta_a_i = theta_a_i if theta_a_i is not None else LinearVehicle.ACCELERATION_RANGE - self.theta_b_i = theta_b_i if theta_b_i is not None else LinearVehicle.STEERING_RANGE + super().__init__( + road, + position, + heading, + speed, + target_lane_index, + target_speed, + route, + enable_lane_change, + timer, + ) + self.theta_a_i = ( + theta_a_i if theta_a_i is not None else LinearVehicle.ACCELERATION_RANGE + ) + self.theta_b_i = ( + theta_b_i if theta_b_i is not None else LinearVehicle.STEERING_RANGE + ) self.data = data self.interval = VehicleInterval(self) self.trajectory = [] @@ -65,17 +82,19 @@ def __init__(self, @classmethod def create_from(cls, vehicle: LinearVehicle) -> "IntervalVehicle": - v = cls(vehicle.road, - vehicle.position, - heading=vehicle.heading, - speed=vehicle.speed, - target_lane_index=getattr(vehicle, 'target_lane_index', None), - target_speed=getattr(vehicle, 'target_speed', None), - route=getattr(vehicle, 'route', None), - timer=getattr(vehicle, 'timer', None), - theta_a_i=getattr(vehicle, 'theta_a_i', None), - theta_b_i=getattr(vehicle, 'theta_b_i', None), - data=getattr(vehicle, "data", None)) + v = cls( + vehicle.road, + vehicle.position, + heading=vehicle.heading, + speed=vehicle.speed, + target_lane_index=getattr(vehicle, "target_lane_index", None), + target_speed=getattr(vehicle, "target_speed", None), + route=getattr(vehicle, "route", None), + timer=getattr(vehicle, "timer", None), + theta_a_i=getattr(vehicle, "theta_a_i", None), + theta_b_i=getattr(vehicle, "theta_b_i", None), + data=getattr(vehicle, "data", None), + ) return v def step(self, dt: float, mode: str = "partial") -> None: @@ -109,9 +128,12 @@ def observer_step(self, dt: float) -> None: phi_a_i[:, 0] = [0, 0] if front_interval: phi_a_i[:, 1] = interval_negative_part( - intervals_diff(front_interval.speed, v_i)) + intervals_diff(front_interval.speed, v_i) + ) # Lane distance interval - lane_psi = self.lane.heading_at(self.lane.local_coordinates(self.position)[0]) + lane_psi = self.lane.heading_at( + self.lane.local_coordinates(self.position)[0] + ) lane_direction = [np.cos(lane_psi), np.sin(lane_psi)] diff_i = intervals_diff(front_interval.position, position_i) d_i = vector_interval_section(diff_i, lane_direction) @@ -124,14 +146,16 @@ def observer_step(self, dt: float) -> None: lanes = self.get_followed_lanes() for lane_index in lanes: lane = self.road.network.get_lane(lane_index) - longitudinal_pursuit = lane.local_coordinates(self.position)[0] + self.speed * self.TAU_PURSUIT + longitudinal_pursuit = ( + lane.local_coordinates(self.position)[0] + self.speed * self.TAU_PURSUIT + ) lane_psi = lane.heading_at(longitudinal_pursuit) _, lateral_i = interval_absolute_to_local(position_i, lane) lateral_i = -np.flip(lateral_i) - i_v_i = 1/np.flip(v_i, 0) - phi_b_i_lane = np.transpose(np.array([ - [0, 0], - intervals_product(lateral_i, i_v_i)])) + i_v_i = 1 / np.flip(v_i, 0) + phi_b_i_lane = np.transpose( + np.array([[0, 0], intervals_product(lateral_i, i_v_i)]) + ) # Union of candidate feature intervals if phi_b_i is None: phi_b_i = phi_b_i_lane @@ -148,7 +172,9 @@ def observer_step(self, dt: float) -> None: if keep_stability: dv_i = integrator_interval(v_i - self.target_speed, self.theta_a_i[:, 0]) else: - dv_i = intervals_product(self.theta_a_i[:, 0], self.target_speed - np.flip(v_i, 0)) + dv_i = intervals_product( + self.theta_a_i[:, 0], self.target_speed - np.flip(v_i, 0) + ) dv_i += a_i dv_i = np.clip(dv_i, -self.ACC_MAX, self.ACC_MAX) keep_stability = True @@ -156,14 +182,20 @@ def observer_step(self, dt: float) -> None: delta_psi = list(map(utils.wrap_to_pi, psi_i - lane_psi)) d_psi_i = integrator_interval(delta_psi, self.theta_b_i[:, 0]) else: - d_psi_i = intervals_product(self.theta_b_i[:, 0], lane_psi - np.flip(psi_i, 0)) + d_psi_i = intervals_product( + self.theta_b_i[:, 0], lane_psi - np.flip(psi_i, 0) + ) d_psi_i += b_i # Position interval - cos_i = [-1 if psi_i[0] <= np.pi <= psi_i[1] else min(map(np.cos, psi_i)), - 1 if psi_i[0] <= 0 <= psi_i[1] else max(map(np.cos, psi_i))] - sin_i = [-1 if psi_i[0] <= -np.pi/2 <= psi_i[1] else min(map(np.sin, psi_i)), - 1 if psi_i[0] <= np.pi/2 <= psi_i[1] else max(map(np.sin, psi_i))] + cos_i = [ + -1 if psi_i[0] <= np.pi <= psi_i[1] else min(map(np.cos, psi_i)), + 1 if psi_i[0] <= 0 <= psi_i[1] else max(map(np.cos, psi_i)), + ] + sin_i = [ + -1 if psi_i[0] <= -np.pi / 2 <= psi_i[1] else min(map(np.sin, psi_i)), + 1 if psi_i[0] <= np.pi / 2 <= psi_i[1] else max(map(np.sin, psi_i)), + ] dx_i = intervals_product(v_i, cos_i) dy_i = intervals_product(v_i, sin_i) @@ -192,22 +224,31 @@ def predictor_step(self, dt: float) -> None: if self.target_lane_index != self.previous_target_lane_index: position_i = self.interval.position target_lane = self.road.network.get_lane(self.target_lane_index) - previous_target_lane = self.road.network.get_lane(self.previous_target_lane_index) + previous_target_lane = self.road.network.get_lane( + self.previous_target_lane_index + ) longi_i, lat_i = interval_absolute_to_local(position_i, target_lane) - psi_i = self.interval.heading + \ - target_lane.heading_at(longi_i.mean()) - previous_target_lane.heading_at(longi_i.mean()) + psi_i = ( + self.interval.heading + + target_lane.heading_at(longi_i.mean()) + - previous_target_lane.heading_at(longi_i.mean()) + ) x_i_local_unrotated = np.transpose([lat_i, psi_i]) - new_x_i_t = self.lateral_lpv.change_coordinates(x_i_local_unrotated, back=False, interval=True) + new_x_i_t = self.lateral_lpv.change_coordinates( + x_i_local_unrotated, back=False, interval=True + ) delta = new_x_i_t.mean(axis=0) - self.lateral_lpv.x_i_t.mean(axis=0) self.lateral_lpv.x_i_t += delta - x_i_local_unrotated = self.longitudinal_lpv.change_coordinates(self.longitudinal_lpv.x_i_t, - back=True, - interval=True) + x_i_local_unrotated = self.longitudinal_lpv.change_coordinates( + self.longitudinal_lpv.x_i_t, back=True, interval=True + ) x_i_local_unrotated[:, 0] = longi_i - new_x_i_t = self.longitudinal_lpv.change_coordinates(x_i_local_unrotated, - back=False, - interval=True) - self.longitudinal_lpv.x_i_t += new_x_i_t.mean(axis=0) - self.longitudinal_lpv.x_i_t.mean(axis=0) + new_x_i_t = self.longitudinal_lpv.change_coordinates( + x_i_local_unrotated, back=False, interval=True + ) + self.longitudinal_lpv.x_i_t += new_x_i_t.mean( + axis=0 + ) - self.longitudinal_lpv.x_i_t.mean(axis=0) self.previous_target_lane_index = self.target_lane_index # Step @@ -215,12 +256,18 @@ def predictor_step(self, dt: float) -> None: self.lateral_lpv.step(dt) # Backward coordinates change - x_i_long = self.longitudinal_lpv.change_coordinates(self.longitudinal_lpv.x_i_t, back=True, interval=True) - x_i_lat = self.lateral_lpv.change_coordinates(self.lateral_lpv.x_i_t, back=True, interval=True) + x_i_long = self.longitudinal_lpv.change_coordinates( + self.longitudinal_lpv.x_i_t, back=True, interval=True + ) + x_i_lat = self.lateral_lpv.change_coordinates( + self.lateral_lpv.x_i_t, back=True, interval=True + ) # Conversion from rectified to true coordinates target_lane = self.road.network.get_lane(self.target_lane_index) - position_i = interval_local_to_absolute(x_i_long[:, 0], x_i_lat[:, 0], target_lane) + position_i = interval_local_to_absolute( + x_i_long[:, 0], x_i_lat[:, 0], target_lane + ) self.interval.position = position_i self.interval.speed = x_i_long[:, 2] self.interval.heading = x_i_lat[:, 1] @@ -239,16 +286,20 @@ def predictor_init(self) -> None: # LPV specification if front_interval: - f_longi_i, _ = interval_absolute_to_local(front_interval.position, target_lane) + f_longi_i, _ = interval_absolute_to_local( + front_interval.position, target_lane + ) f_pos = f_longi_i[0] f_vel = front_interval.speed[0] else: f_pos, f_vel = 0, 0 x0 = [longi_i[0], f_pos, v_i[0], f_vel] - center = [-self.DISTANCE_WANTED - self.target_speed * self.TIME_WANTED, - 0, - self.target_speed, - self.target_speed] + center = [ + -self.DISTANCE_WANTED - self.target_speed * self.TIME_WANTED, + 0, + self.target_speed, + self.target_speed, + ] noise = 1 b = np.eye(4) d = np.array([[1], [0], [0], [0]]) @@ -271,13 +322,19 @@ def predictor_init(self) -> None: self.lateral_lpv = LPV(x0, a0, da, b, d, omega_i, u, center=center) def longitudinal_matrix_polytope(self) -> Polytope: - return IntervalVehicle.parameter_box_to_polytope(self.theta_a_i, self.longitudinal_structure) + return IntervalVehicle.parameter_box_to_polytope( + self.theta_a_i, self.longitudinal_structure + ) def lateral_matrix_polytope(self) -> Polytope: - return IntervalVehicle.parameter_box_to_polytope(self.theta_b_i, self.lateral_structure) + return IntervalVehicle.parameter_box_to_polytope( + self.theta_b_i, self.lateral_structure + ) @staticmethod - def parameter_box_to_polytope(parameter_box: np.ndarray, structure: Callable) -> Polytope: + def parameter_box_to_polytope( + parameter_box: np.ndarray, structure: Callable + ) -> Polytope: a, phi = structure() a_theta = lambda params: a + np.tensordot(phi, params, axes=[0, 0]) return polytope(a_theta, parameter_box) @@ -297,7 +354,9 @@ def get_front_interval(self) -> "VehicleInterval": front_interval = None return front_interval - def get_followed_lanes(self, lane_change_model: str = "model", squeeze: bool = True) -> List[LaneIndex]: + def get_followed_lanes( + self, lane_change_model: str = "model", squeeze: bool = True + ) -> List[LaneIndex]: """ Get the list of lanes that could be followed by this vehicle. @@ -311,12 +370,19 @@ def get_followed_lanes(self, lane_change_model: str = "model", squeeze: bool = T if lane_change_model == "model": lanes = [self.target_lane_index] elif lane_change_model == "all": - lanes = self.road.network.side_lanes(self.target_lane_index) + [self.target_lane_index] + lanes = self.road.network.side_lanes(self.target_lane_index) + [ + self.target_lane_index + ] elif lane_change_model == "right": lanes = [self.target_lane_index] _from, _to, _id = self.target_lane_index - if _id < len(self.road.network.graph[_from][_to]) - 1 \ - and self.road.network.get_lane((_from, _to, _id + 1)).is_reachable_from(self.position): + if _id < len( + self.road.network.graph[_from][_to] + ) - 1 and self.road.network.get_lane( + (_from, _to, _id + 1) + ).is_reachable_from( + self.position + ): lanes += [(_from, _to, _id + 1)] elif not squeeze: lanes += [self.target_lane_index] # Right lane is also current lane @@ -337,33 +403,49 @@ def partial_observer_step(self, dt: float, alpha: float = 0) -> None: o = self.interval v_minus = IntervalVehicle.create_from(self) v_minus.interval = copy.deepcopy(self.interval) - v_minus.interval.position[1, :] = (1 - alpha) * o.position[0, :] + alpha * o.position[1, :] + v_minus.interval.position[1, :] = (1 - alpha) * o.position[ + 0, : + ] + alpha * o.position[1, :] v_minus.interval.speed[1] = (1 - alpha) * o.speed[0] + alpha * o.speed[1] v_minus.interval.heading[1] = (1 - alpha) * o.heading[0] + alpha * o.heading[1] v_plus = IntervalVehicle.create_from(self) v_plus.interval = copy.deepcopy(self.interval) - v_plus.interval.position[0, :] = alpha * o.position[0, :] + (1 - alpha) * o.position[1, :] + v_plus.interval.position[0, :] = ( + alpha * o.position[0, :] + (1 - alpha) * o.position[1, :] + ) v_plus.interval.speed[0] = alpha * o.speed[0] + (1 - alpha) * o.speed[1] v_plus.interval.heading[0] = alpha * o.heading[0] + (1 - alpha) * o.heading[1] # 2. Propagate their observer dynamics x_i_-(t+dt) and x_i_+(t+dt) v_minus.road = copy.copy(v_minus.road) - v_minus.road.vehicles = [v if v is not self else v_minus for v in v_minus.road.vehicles] + v_minus.road.vehicles = [ + v if v is not self else v_minus for v in v_minus.road.vehicles + ] v_plus.road = copy.copy(v_plus.road) - v_plus.road.vehicles = [v if v is not self else v_plus for v in v_plus.road.vehicles] + v_plus.road.vehicles = [ + v if v is not self else v_plus for v in v_plus.road.vehicles + ] v_minus.observer_step(dt) v_plus.observer_step(dt) # 3. Merge the resulting intervals together to x_i(t+dt). - self.interval.position = np.array([v_minus.interval.position[0], v_plus.interval.position[1]]) - self.interval.speed = np.array([v_minus.interval.speed[0], v_plus.interval.speed[1]]) - self.interval.heading = np.array([min(v_minus.interval.heading[0], v_plus.interval.heading[0]), - max(v_minus.interval.heading[1], v_plus.interval.heading[1])]) + self.interval.position = np.array( + [v_minus.interval.position[0], v_plus.interval.position[1]] + ) + self.interval.speed = np.array( + [v_minus.interval.speed[0], v_plus.interval.speed[1]] + ) + self.interval.heading = np.array( + [ + min(v_minus.interval.heading[0], v_plus.interval.heading[0]), + max(v_minus.interval.heading[1], v_plus.interval.heading[1]), + ] + ) def store_trajectories(self) -> None: """Store the current model, min and max states to a trajectory list.""" self.trajectory.append(LinearVehicle.create_from(self)) self.interval_trajectory.append(copy.deepcopy(self.interval)) - def handle_collisions(self, other: 'RoadObject', dt: float) -> None: + def handle_collisions(self, other: "RoadObject", dt: float) -> None: """ Worst-case collision check. @@ -381,18 +463,24 @@ def handle_collisions(self, other: 'RoadObject', dt: float) -> None: return # Fast rectangular pre-check - if not utils.point_in_rectangle(other.position, - self.interval.position[0] - self.LENGTH, - self.interval.position[1] + self.LENGTH): + if not utils.point_in_rectangle( + other.position, + self.interval.position[0] - self.LENGTH, + self.interval.position[1] + self.LENGTH, + ): return # Projection of other vehicle to uncertainty rectangle. This is the possible position of this vehicle which is # the most likely to collide with other vehicle - projection = np.minimum(np.maximum(other.position, self.interval.position[0]), - self.interval.position[1]) + projection = np.minimum( + np.maximum(other.position, self.interval.position[0]), + self.interval.position[1], + ) # Accurate rectangular check - if utils.rotated_rectangles_intersect((projection, self.LENGTH, self.WIDTH, self.heading), - (other.position, 0.9*other.LENGTH, 0.9*other.WIDTH, other.heading)): + if utils.rotated_rectangles_intersect( + (projection, self.LENGTH, self.WIDTH, self.heading), + (other.position, 0.9 * other.LENGTH, 0.9 * other.WIDTH, other.heading), + ): self.speed = other.speed = min(self.speed, other.speed) self.crashed = other.crashed = True diff --git a/scripts/sb3_highway_dqn.py b/scripts/sb3_highway_dqn.py index ae43a0a39..a46aba5b3 100644 --- a/scripts/sb3_highway_dqn.py +++ b/scripts/sb3_highway_dqn.py @@ -4,27 +4,29 @@ import highway_env - TRAIN = True -if __name__ == '__main__': +if __name__ == "__main__": # Create the environment env = gym.make("highway-fast-v0", render_mode="rgb_array") obs, info = env.reset() # Create the model - model = DQN('MlpPolicy', env, - policy_kwargs=dict(net_arch=[256, 256]), - learning_rate=5e-4, - buffer_size=15000, - learning_starts=200, - batch_size=32, - gamma=0.8, - train_freq=1, - gradient_steps=1, - target_update_interval=50, - verbose=1, - tensorboard_log="highway_dqn/") + model = DQN( + "MlpPolicy", + env, + policy_kwargs=dict(net_arch=[256, 256]), + learning_rate=5e-4, + buffer_size=15000, + learning_starts=200, + batch_size=32, + gamma=0.8, + train_freq=1, + gradient_steps=1, + target_update_interval=50, + verbose=1, + tensorboard_log="highway_dqn/", + ) # Train the model if TRAIN: @@ -34,7 +36,9 @@ # Run the trained model and record video model = DQN.load("highway_dqn/model", env=env) - env = RecordVideo(env, video_folder="highway_dqn/videos", episode_trigger=lambda e: True) + env = RecordVideo( + env, video_folder="highway_dqn/videos", episode_trigger=lambda e: True + ) env.unwrapped.set_record_video_wrapper(env) env.configure({"simulation_frequency": 15}) # Higher FPS for rendering diff --git a/scripts/sb3_highway_dqn_cnn.py b/scripts/sb3_highway_dqn_cnn.py index 8b840a7f4..c9f5cdb1a 100644 --- a/scripts/sb3_highway_dqn_cnn.py +++ b/scripts/sb3_highway_dqn_cnn.py @@ -1,21 +1,23 @@ import gymnasium as gym from stable_baselines3 import DQN -from stable_baselines3.common.vec_env import VecVideoRecorder, DummyVecEnv +from stable_baselines3.common.vec_env import DummyVecEnv, VecVideoRecorder import highway_env def train_env(): - env = gym.make('highway-fast-v0') - env.configure({ - "observation": { - "type": "GrayscaleObservation", - "observation_shape": (128, 64), - "stack_size": 4, - "weights": [0.2989, 0.5870, 0.1140], # weights for RGB conversion - "scaling": 1.75, - }, - }) + env = gym.make("highway-fast-v0") + env.configure( + { + "observation": { + "type": "GrayscaleObservation", + "observation_shape": (128, 64), + "stack_size": 4, + "weights": [0.2989, 0.5870, 0.1140], # weights for RGB conversion + "scaling": 1.75, + }, + } + ) env.reset() return env @@ -27,20 +29,23 @@ def test_env(): return env -if __name__ == '__main__': +if __name__ == "__main__": # Train - model = DQN('CnnPolicy', DummyVecEnv([train_env]), - learning_rate=5e-4, - buffer_size=15000, - learning_starts=200, - batch_size=32, - gamma=0.8, - train_freq=1, - gradient_steps=1, - target_update_interval=50, - exploration_fraction=0.7, - verbose=1, - tensorboard_log="highway_cnn/") + model = DQN( + "CnnPolicy", + DummyVecEnv([train_env]), + learning_rate=5e-4, + buffer_size=15000, + learning_starts=200, + batch_size=32, + gamma=0.8, + train_freq=1, + gradient_steps=1, + target_update_interval=50, + exploration_fraction=0.7, + verbose=1, + tensorboard_log="highway_cnn/", + ) model.learn(total_timesteps=int(1e5)) model.save("highway_cnn/model") @@ -49,9 +54,13 @@ def test_env(): env = DummyVecEnv([test_env]) video_length = 2 * env.envs[0].config["duration"] - env = VecVideoRecorder(env, "highway_cnn/videos/", - record_video_trigger=lambda x: x == 0, video_length=video_length, - name_prefix="dqn-agent") + env = VecVideoRecorder( + env, + "highway_cnn/videos/", + record_video_trigger=lambda x: x == 0, + video_length=video_length, + name_prefix="dqn-agent", + ) obs, info = env.reset() for _ in range(video_length + 1): action, _ = model.predict(obs) diff --git a/scripts/sb3_highway_ppo.py b/scripts/sb3_highway_ppo.py index 87ffa058e..a881937a4 100644 --- a/scripts/sb3_highway_ppo.py +++ b/scripts/sb3_highway_ppo.py @@ -1,16 +1,9 @@ import gymnasium as gym -import torch as th from stable_baselines3 import PPO -from torch.distributions import Categorical -import torch -import torch.nn as nn -import numpy as np -from torch.nn import functional as F from stable_baselines3.common.env_util import make_vec_env -from stable_baselines3.common.torch_layers import BaseFeaturesExtractor from stable_baselines3.common.vec_env import SubprocVecEnv -import highway_env +import highway_env # ================================== # Main script @@ -22,16 +15,18 @@ n_cpu = 6 batch_size = 64 env = make_vec_env("highway-fast-v0", n_envs=n_cpu, vec_env_cls=SubprocVecEnv) - model = PPO("MlpPolicy", - env, - policy_kwargs=dict(net_arch=[dict(pi=[256, 256], vf=[256, 256])]), - n_steps=batch_size * 12 // n_cpu, - batch_size=batch_size, - n_epochs=10, - learning_rate=5e-4, - gamma=0.8, - verbose=2, - tensorboard_log="highway_ppo/") + model = PPO( + "MlpPolicy", + env, + policy_kwargs=dict(net_arch=[dict(pi=[256, 256], vf=[256, 256])]), + n_steps=batch_size * 12 // n_cpu, + batch_size=batch_size, + n_epochs=10, + learning_rate=5e-4, + gamma=0.8, + verbose=2, + tensorboard_log="highway_ppo/", + ) # Train the agent model.learn(total_timesteps=int(2e4)) # Save the agent diff --git a/scripts/sb3_highway_ppo_transformer.py b/scripts/sb3_highway_ppo_transformer.py index fa007371a..aa3702400 100644 --- a/scripts/sb3_highway_ppo_transformer.py +++ b/scripts/sb3_highway_ppo_transformer.py @@ -1,24 +1,27 @@ import functools + import gymnasium as gym +import numpy as np import pygame import seaborn as sns -import torch as th -from highway_env.utils import lmap -from stable_baselines3 import PPO -from torch.distributions import Categorical import torch +import torch as th import torch.nn as nn -import numpy as np -from torch.nn import functional as F +from stable_baselines3 import PPO from stable_baselines3.common.env_util import make_vec_env from stable_baselines3.common.torch_layers import BaseFeaturesExtractor from stable_baselines3.common.vec_env import SubprocVecEnv +from torch.distributions import Categorical +from torch.nn import functional as F + import highway_env +from highway_env.utils import lmap # ================================== # Policy Architecture # ================================== + def activation_factory(activation_type): if activation_type == "RELU": return F.relu @@ -29,6 +32,7 @@ def activation_factory(activation_type): else: raise ValueError("Unknown activation_type: {}".format(activation_type)) + class BaseModule(torch.nn.Module): """ Base torch.nn.Module implementing basic features: @@ -42,29 +46,31 @@ def __init__(self, activation_type="RELU", reset_type="XAVIER"): self.reset_type = reset_type def _init_weights(self, m): - if hasattr(m, 'weight'): + if hasattr(m, "weight"): if self.reset_type == "XAVIER": torch.nn.init.xavier_uniform_(m.weight.data) elif self.reset_type == "ZEROS": - torch.nn.init.constant_(m.weight.data, 0.) + torch.nn.init.constant_(m.weight.data, 0.0) else: raise ValueError("Unknown reset type") - if hasattr(m, 'bias') and m.bias is not None: - torch.nn.init.constant_(m.bias.data, 0.) + if hasattr(m, "bias") and m.bias is not None: + torch.nn.init.constant_(m.bias.data, 0.0) def reset(self): self.apply(self._init_weights) class MultiLayerPerceptron(BaseModule): - def __init__(self, - in_size=None, - layer_sizes=None, - reshape=True, - out_size=None, - activation="RELU", - is_policy=False, - **kwargs): + def __init__( + self, + in_size=None, + layer_sizes=None, + reshape=True, + out_size=None, + activation="RELU", + is_policy=False, + **kwargs + ): super().__init__(**kwargs) self.reshape = reshape self.layer_sizes = layer_sizes or [64, 64] @@ -73,8 +79,7 @@ def __init__(self, self.is_policy = is_policy self.softmax = nn.Softmax(dim=-1) sizes = [in_size] + self.layer_sizes - layers_list = [nn.Linear(sizes[i], sizes[i + 1]) - for i in range(len(sizes) - 1)] + layers_list = [nn.Linear(sizes[i], sizes[i + 1]) for i in range(len(sizes) - 1)] self.layers = nn.ModuleList(layers_list) if out_size: self.predict = nn.Linear(sizes[-1], out_size) @@ -104,78 +109,69 @@ def action_scores(self, x): class EgoAttention(BaseModule): - def __init__(self, - feature_size=64, - heads=4, - dropout_factor=0): + def __init__(self, feature_size=64, heads=4, dropout_factor=0): super().__init__() self.feature_size = feature_size self.heads = heads self.dropout_factor = dropout_factor self.features_per_head = int(self.feature_size / self.heads) - self.value_all = nn.Linear(self.feature_size, - self.feature_size, - bias=False) - self.key_all = nn.Linear(self.feature_size, - self.feature_size, - bias=False) - self.query_ego = nn.Linear(self.feature_size, - self.feature_size, - bias=False) - self.attention_combine = nn.Linear(self.feature_size, - self.feature_size, - bias=False) + self.value_all = nn.Linear(self.feature_size, self.feature_size, bias=False) + self.key_all = nn.Linear(self.feature_size, self.feature_size, bias=False) + self.query_ego = nn.Linear(self.feature_size, self.feature_size, bias=False) + self.attention_combine = nn.Linear( + self.feature_size, self.feature_size, bias=False + ) @classmethod def default_config(cls): - return { - } + return {} def forward(self, ego, others, mask=None): batch_size = others.shape[0] n_entities = others.shape[1] + 1 - input_all = torch.cat((ego.view(batch_size, 1, - self.feature_size), others), dim=1) + input_all = torch.cat( + (ego.view(batch_size, 1, self.feature_size), others), dim=1 + ) # Dimensions: Batch, entity, head, feature_per_head - key_all = self.key_all(input_all).view(batch_size, - n_entities, - self.heads, - self.features_per_head) - value_all = self.value_all(input_all).view(batch_size, - n_entities, - self.heads, - self.features_per_head) - query_ego = self.query_ego(ego).view(batch_size, 1, - self.heads, - self.features_per_head) + key_all = self.key_all(input_all).view( + batch_size, n_entities, self.heads, self.features_per_head + ) + value_all = self.value_all(input_all).view( + batch_size, n_entities, self.heads, self.features_per_head + ) + query_ego = self.query_ego(ego).view( + batch_size, 1, self.heads, self.features_per_head + ) # Dimensions: Batch, head, entity, feature_per_head key_all = key_all.permute(0, 2, 1, 3) value_all = value_all.permute(0, 2, 1, 3) query_ego = query_ego.permute(0, 2, 1, 3) if mask is not None: - mask = mask.view((batch_size, 1, 1, - n_entities)).repeat((1, self.heads, 1, 1)) - value, attention_matrix = attention(query_ego, - key_all, - value_all, - mask, - nn.Dropout(self.dropout_factor)) - result = (self.attention_combine( - value.reshape((batch_size, - self.feature_size))) + ego.squeeze(1))/2 + mask = mask.view((batch_size, 1, 1, n_entities)).repeat( + (1, self.heads, 1, 1) + ) + value, attention_matrix = attention( + query_ego, key_all, value_all, mask, nn.Dropout(self.dropout_factor) + ) + result = ( + self.attention_combine(value.reshape((batch_size, self.feature_size))) + + ego.squeeze(1) + ) / 2 return result, attention_matrix class EgoAttentionNetwork(BaseModule): - def __init__(self, - in_size=None, - out_size=None, - presence_feature_idx=0, - embedding_layer_kwargs=None, - attention_layer_kwargs=None, - **kwargs): + def __init__( + self, + in_size=None, + out_size=None, + presence_feature_idx=0, + embedding_layer_kwargs=None, + attention_layer_kwargs=None, + **kwargs + ): super().__init__(**kwargs) self.out_size = out_size self.presence_feature_idx = presence_feature_idx @@ -200,7 +196,7 @@ def split_input(self, x, mask=None): others = x[:, 1:, :] if mask is None: aux = self.presence_feature_idx - mask = x[:, :, aux:aux + 1] < 0.5 + mask = x[:, :, aux : aux + 1] < 0.5 return ego, others, mask def forward_attention(self, x): @@ -246,7 +242,7 @@ def attention(query, key, value, mask=None, dropout=None): attention_network_kwargs = dict( - in_size=5*15, + in_size=5 * 15, embedding_layer_kwargs={"in_size": 7, "layer_sizes": [64, 64], "reshape": False}, attention_layer_kwargs={"feature_size": 64, "heads": 2}, ) @@ -260,7 +256,10 @@ class CustomExtractor(BaseFeaturesExtractor): """ def __init__(self, observation_space: gym.spaces.Box, **kwargs): - super().__init__(observation_space, features_dim=kwargs["attention_layer_kwargs"]["feature_size"]) + super().__init__( + observation_space, + features_dim=kwargs["attention_layer_kwargs"]["feature_size"], + ) self.extractor = EgoAttentionNetwork(**kwargs) def forward(self, observations: th.Tensor) -> th.Tensor: @@ -271,6 +270,7 @@ def forward(self, observations: th.Tensor) -> th.Tensor: # Environment configuration # ================================== + def make_configure_env(**kwargs): env = gym.make(kwargs["id"]) env.configure(kwargs["config"]) @@ -279,27 +279,19 @@ def make_configure_env(**kwargs): env_kwargs = { - 'id': 'highway-v0', - 'config': { + "id": "highway-v0", + "config": { "lanes_count": 3, "vehicles_count": 15, "observation": { "type": "Kinematics", "vehicles_count": 10, - "features": [ - "presence", - "x", - "y", - "vx", - "vy", - "cos_h", - "sin_h" - ], - "absolute": False + "features": ["presence", "x", "y", "vx", "vy", "cos_h", "sin_h"], + "absolute": False, }, "policy_frequency": 2, "duration": 40, - } + }, } @@ -307,28 +299,40 @@ def make_configure_env(**kwargs): # Display attention matrix # ================================== -def display_vehicles_attention(agent_surface, sim_surface, env, model, min_attention=0.01): - v_attention = compute_vehicles_attention(env, model) - for head in range(list(v_attention.values())[0].shape[0]): - attention_surface = pygame.Surface(sim_surface.get_size(), pygame.SRCALPHA) - for vehicle, attention in v_attention.items(): - if attention[head] < min_attention: - continue - width = attention[head] * 5 - desat = np.clip(lmap(attention[head], (0, 0.5), (0.7, 1)), 0.7, 1) - colors = sns.color_palette("dark", desat=desat) - color = np.array(colors[(2*head) % (len(colors) - 1)]) * 255 - color = (*color, np.clip(lmap(attention[head], (0, 0.5), (100, 200)), 100, 200)) - if vehicle is env.vehicle: - pygame.draw.circle(attention_surface, color, - sim_surface.vec2pix(env.vehicle.position), - max(sim_surface.pix(width / 2), 1)) - else: - pygame.draw.line(attention_surface, color, - sim_surface.vec2pix(env.vehicle.position), - sim_surface.vec2pix(vehicle.position), - max(sim_surface.pix(width), 1)) - sim_surface.blit(attention_surface, (0, 0)) + +def display_vehicles_attention( + agent_surface, sim_surface, env, model, min_attention=0.01 +): + v_attention = compute_vehicles_attention(env, model) + for head in range(list(v_attention.values())[0].shape[0]): + attention_surface = pygame.Surface(sim_surface.get_size(), pygame.SRCALPHA) + for vehicle, attention in v_attention.items(): + if attention[head] < min_attention: + continue + width = attention[head] * 5 + desat = np.clip(lmap(attention[head], (0, 0.5), (0.7, 1)), 0.7, 1) + colors = sns.color_palette("dark", desat=desat) + color = np.array(colors[(2 * head) % (len(colors) - 1)]) * 255 + color = ( + *color, + np.clip(lmap(attention[head], (0, 0.5), (100, 200)), 100, 200), + ) + if vehicle is env.vehicle: + pygame.draw.circle( + attention_surface, + color, + sim_surface.vec2pix(env.vehicle.position), + max(sim_surface.pix(width / 2), 1), + ) + else: + pygame.draw.line( + attention_surface, + color, + sim_surface.vec2pix(env.vehicle.position), + sim_surface.vec2pix(vehicle.position), + max(sim_surface.pix(width), 1), + ) + sim_surface.blit(attention_surface, (0, 0)) def compute_vehicles_attention(env, model): @@ -353,7 +357,10 @@ def compute_vehicles_attention(env, model): v_position = np.array([v_position["x"], v_position["y"]]) if not obs_type.absolute and v_index > 0: v_position += env.unwrapped.vehicle.position - vehicle = min(env.unwrapped.road.vehicles, key=lambda v: np.linalg.norm(v.position - v_position)) + vehicle = min( + env.unwrapped.road.vehicles, + key=lambda v: np.linalg.norm(v.position - v_position), + ) v_attention[vehicle] = attention[:, v_index] return v_attention @@ -370,23 +377,34 @@ def compute_vehicles_attention(env, model): features_extractor_class=CustomExtractor, features_extractor_kwargs=attention_network_kwargs, ) - env = make_vec_env(make_configure_env, n_envs=n_cpu, seed=0, vec_env_cls=SubprocVecEnv, env_kwargs=env_kwargs) - model = PPO("MlpPolicy", env, - n_steps=512 // n_cpu, - batch_size=64, - learning_rate=2e-3, - policy_kwargs=policy_kwargs, - verbose=2, - tensorboard_log="highway_attention_ppo/") + env = make_vec_env( + make_configure_env, + n_envs=n_cpu, + seed=0, + vec_env_cls=SubprocVecEnv, + env_kwargs=env_kwargs, + ) + model = PPO( + "MlpPolicy", + env, + n_steps=512 // n_cpu, + batch_size=64, + learning_rate=2e-3, + policy_kwargs=policy_kwargs, + verbose=2, + tensorboard_log="highway_attention_ppo/", + ) # Train the agent - model.learn(total_timesteps=200*1000) + model.learn(total_timesteps=200 * 1000) # Save the agent model.save("highway_attention_ppo/model") model = PPO.load("highway_attention_ppo/model") env = make_configure_env(**env_kwargs) env.render() - env.viewer.set_agent_display(functools.partial(display_vehicles_attention, env=env, model=model)) + env.viewer.set_agent_display( + functools.partial(display_vehicles_attention, env=env, model=model) + ) for _ in range(5): obs, info = env.reset() done = truncated = False diff --git a/scripts/sb3_racetracks_ppo.py b/scripts/sb3_racetracks_ppo.py index 64ae5c4ee..88a06f462 100644 --- a/scripts/sb3_racetracks_ppo.py +++ b/scripts/sb3_racetracks_ppo.py @@ -1,30 +1,29 @@ -import numpy as np import gymnasium as gym from gymnasium.wrappers import RecordVideo -from stable_baselines3 import DQN, DDPG, PPO +from stable_baselines3 import PPO from stable_baselines3.common.env_util import make_vec_env -from stable_baselines3.common.noise import NormalActionNoise from stable_baselines3.common.vec_env import SubprocVecEnv import highway_env - TRAIN = True -if __name__ == '__main__': +if __name__ == "__main__": n_cpu = 6 batch_size = 64 env = make_vec_env("racetrack-v0", n_envs=n_cpu, vec_env_cls=SubprocVecEnv) - model = PPO("MlpPolicy", - env, - policy_kwargs=dict(net_arch=[dict(pi=[256, 256], vf=[256, 256])]), - n_steps=batch_size * 12 // n_cpu, - batch_size=batch_size, - n_epochs=10, - learning_rate=5e-4, - gamma=0.9, - verbose=2, - tensorboard_log="racetrack_ppo/") + model = PPO( + "MlpPolicy", + env, + policy_kwargs=dict(net_arch=[dict(pi=[256, 256], vf=[256, 256])]), + n_steps=batch_size * 12 // n_cpu, + batch_size=batch_size, + n_epochs=10, + learning_rate=5e-4, + gamma=0.9, + verbose=2, + tensorboard_log="racetrack_ppo/", + ) # Train the model if TRAIN: model.learn(total_timesteps=int(1e5)) @@ -35,7 +34,9 @@ model = PPO.load("racetrack_ppo/model", env=env) env = gym.make("racetrack-v0") - env = RecordVideo(env, video_folder="racetrack_ppo/videos", episode_trigger=lambda e: True) + env = RecordVideo( + env, video_folder="racetrack_ppo/videos", episode_trigger=lambda e: True + ) env.unwrapped.set_record_video_wrapper(env) for video in range(10): diff --git a/scripts/utils.py b/scripts/utils.py index 830197bdb..8b5af6f3f 100644 --- a/scripts/utils.py +++ b/scripts/utils.py @@ -1,16 +1,18 @@ -from IPython import display as ipythondisplay -from pyvirtualdisplay import Display -from gymnasium.wrappers import RecordVideo -from pathlib import Path import base64 +from pathlib import Path +from gymnasium.wrappers import RecordVideo +from IPython import display as ipythondisplay +from pyvirtualdisplay import Display display = Display(visible=0, size=(1400, 900)) display.start() def record_videos(env, video_folder="videos"): - wrapped = RecordVideo(env, video_folder=video_folder, episode_trigger=lambda e: True) + wrapped = RecordVideo( + env, video_folder=video_folder, episode_trigger=lambda e: True + ) # Capture intermediate frames env.unwrapped.set_record_video_wrapper(wrapped) @@ -22,8 +24,12 @@ def show_videos(path="videos"): html = [] for mp4 in Path(path).glob("*.mp4"): video_b64 = base64.b64encode(mp4.read_bytes()) - html.append(''''''.format(mp4, video_b64.decode('ascii'))) + """.format( + mp4, video_b64.decode("ascii") + ) + ) ipythondisplay.display(ipythondisplay.HTML(data="
".join(html))) diff --git a/setup.py b/setup.py index eef624788..098261bee 100644 --- a/setup.py +++ b/setup.py @@ -5,4 +5,4 @@ from setuptools import setup if __name__ == "__main__": - setup() \ No newline at end of file + setup() diff --git a/tests/envs/test_actions.py b/tests/envs/test_actions.py index 96c23e756..8cad31f2b 100644 --- a/tests/envs/test_actions.py +++ b/tests/envs/test_actions.py @@ -1,5 +1,6 @@ import gymnasium as gym import pytest + import highway_env highway_env.register_highway_envs() diff --git a/tests/envs/test_env_preprocessors.py b/tests/envs/test_env_preprocessors.py index d69b8e7ee..9db589196 100644 --- a/tests/envs/test_env_preprocessors.py +++ b/tests/envs/test_env_preprocessors.py @@ -1,10 +1,12 @@ import gymnasium as gym + import highway_env highway_env.register_highway_envs() + def test_preprocessors(): - env = gym.make('highway-v0') + env = gym.make("highway-v0") env = env.simplify() env = env.change_vehicles("highway_env.vehicle.behavior.IDMVehicle") env = env.set_preferred_lane(0) @@ -21,4 +23,3 @@ def test_preprocessors(): assert env.observation_space.contains(obs) assert 0 <= reward <= 1 - diff --git a/tests/envs/test_gym.py b/tests/envs/test_gym.py index 85e1a5e62..71cbb91e4 100644 --- a/tests/envs/test_gym.py +++ b/tests/envs/test_gym.py @@ -1,5 +1,6 @@ import gymnasium as gym import pytest + import highway_env from highway_env.envs.highway_env import HighwayEnv diff --git a/tests/envs/test_time.py b/tests/envs/test_time.py index 7f817fb37..d2981890e 100644 --- a/tests/envs/test_time.py +++ b/tests/envs/test_time.py @@ -1,6 +1,7 @@ -import pytest import timeit + import gymnasium as gym + import highway_env highway_env.register_highway_envs() @@ -9,6 +10,7 @@ def wrapper(func, *args, **kwargs): def wrapped(): return func(*args, **kwargs) + return wrapped @@ -25,7 +27,7 @@ def test_running_time(repeat=1): for env_name, steps in [ ("highway-v0", 10), ("highway-fast-v0", 10), - ("parking-v0", 20) + ("parking-v0", 20), ]: env_time = wrapper(time_env, env_name, steps) time_spent = timeit.timeit(env_time, number=repeat) / repeat diff --git a/tests/graphics/test_render.py b/tests/graphics/test_render.py index fe8cc6776..8e8848a3c 100644 --- a/tests/graphics/test_render.py +++ b/tests/graphics/test_render.py @@ -1,6 +1,7 @@ import gymnasium as gym import numpy as np import pytest + import highway_env highway_env.register_highway_envs() @@ -15,22 +16,35 @@ def test_render(env_spec): img = env.render() env.close() assert isinstance(img, np.ndarray) - assert img.shape == (env.config["screen_height"], env.config["screen_width"], 3) # (H,W,C) + assert img.shape == ( + env.config["screen_height"], + env.config["screen_width"], + 3, + ) # (H,W,C) @pytest.mark.parametrize("env_spec", envs) def test_obs_grayscale(env_spec, stack_size=4): env = gym.make(env_spec) - env.configure({ - "offscreen_rendering": True, - "observation": { - "type": "GrayscaleObservation", - "observation_shape": (env.config["screen_width"], env.config["screen_height"]), - "stack_size": stack_size, - "weights": [0.2989, 0.5870, 0.1140], + env.configure( + { + "offscreen_rendering": True, + "observation": { + "type": "GrayscaleObservation", + "observation_shape": ( + env.config["screen_width"], + env.config["screen_height"], + ), + "stack_size": stack_size, + "weights": [0.2989, 0.5870, 0.1140], + }, } - }) + ) obs, info = env.reset() env.close() assert isinstance(obs, np.ndarray) - assert obs.shape == (stack_size, env.config["screen_width"], env.config["screen_height"]) + assert obs.shape == ( + stack_size, + env.config["screen_width"], + env.config["screen_height"], + ) diff --git a/tests/road/test_road.py b/tests/road/test_road.py index aeb29627b..ba5d45399 100644 --- a/tests/road/test_road.py +++ b/tests/road/test_road.py @@ -1,7 +1,7 @@ import numpy as np import pytest -from highway_env.road.lane import StraightLane, CircularLane, PolyLane +from highway_env.road.lane import CircularLane, PolyLane, StraightLane from highway_env.road.road import Road, RoadNetwork from highway_env.vehicle.controller import ControlledVehicle @@ -28,10 +28,10 @@ def test_network(net): assert v.lane_index == (0, 1, 0) # Lane changes - dt = 1/15 + dt = 1 / 15 lane_index = v.target_lane_index lane_changes = 0 - for _ in range(int(20/dt)): + for _ in range(int(20 / dt)): road.act() road.step(dt) if lane_index != v.target_lane_index: diff --git a/tests/test_utils.py b/tests/test_utils.py index fbf18c4e6..7467f4a8a 100644 --- a/tests/test_utils.py +++ b/tests/test_utils.py @@ -4,9 +4,11 @@ def test_rotated_rectangles_intersect(): - assert rotated_rectangles_intersect(([12.86076812, 28.60182391], 5.0, 2.0, -0.4675779906495494), - ([9.67753944, 28.90585412], 5.0, 2.0, -0.3417019364473201)) + assert rotated_rectangles_intersect( + ([12.86076812, 28.60182391], 5.0, 2.0, -0.4675779906495494), + ([9.67753944, 28.90585412], 5.0, 2.0, -0.3417019364473201), + ) assert rotated_rectangles_intersect(([0, 0], 2, 1, 0), ([0, 1], 2, 1, 0)) assert not rotated_rectangles_intersect(([0, 0], 2, 1, 0), ([0, 2.1], 2, 1, 0)) assert not rotated_rectangles_intersect(([0, 0], 2, 1, 0), ([1, 1.1], 2, 1, 0)) - assert rotated_rectangles_intersect(([0, 0], 2, 1, np.pi/4), ([1, 1.1], 2, 1, 0)) + assert rotated_rectangles_intersect(([0, 0], 2, 1, np.pi / 4), ([1, 1.1], 2, 1, 0)) diff --git a/tests/vehicle/test_behavior.py b/tests/vehicle/test_behavior.py index 526ebf93e..4ae93f94b 100644 --- a/tests/vehicle/test_behavior.py +++ b/tests/vehicle/test_behavior.py @@ -1,8 +1,8 @@ import pytest -from highway_env.vehicle.objects import Obstacle from highway_env.road.road import Road, RoadNetwork from highway_env.vehicle.behavior import IDMVehicle, LinearVehicle +from highway_env.vehicle.objects import Obstacle FPS = 15 vehicle_types = [IDMVehicle, LinearVehicle] @@ -17,9 +17,11 @@ def test_stop_before_obstacle(vehicle_type): road.objects.append(obstacle) for _ in range(10 * FPS): road.act() - road.step(dt=1/FPS) + road.step(dt=1 / FPS) assert not vehicle.crashed - assert vehicle.position[0] == pytest.approx(obstacle.position[0] - vehicle_type.DISTANCE_WANTED, abs=1) + assert vehicle.position[0] == pytest.approx( + obstacle.position[0] - vehicle_type.DISTANCE_WANTED, abs=1 + ) assert vehicle.position[1] == pytest.approx(0) assert vehicle.speed == pytest.approx(0, abs=1) assert vehicle.heading == pytest.approx(0) diff --git a/tests/vehicle/test_control.py b/tests/vehicle/test_control.py index 7dc033f60..68856a040 100644 --- a/tests/vehicle/test_control.py +++ b/tests/vehicle/test_control.py @@ -10,7 +10,7 @@ def test_step(): v = ControlledVehicle(road=None, position=[0, 0], speed=20, heading=0) for _ in range(2 * FPS): - v.step(dt=1/FPS) + v.step(dt=1 / FPS) assert v.position[0] == pytest.approx(40) assert v.position[1] == pytest.approx(0) assert v.speed == pytest.approx(20) @@ -19,23 +19,35 @@ def test_step(): 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), speed=20, heading=0) - v.act('LANE_RIGHT') + v = ControlledVehicle( + road=road, + position=road.network.get_lane(("0", "1", 0)).position(0, 0), + speed=20, + heading=0, + ) + v.act("LANE_RIGHT") for _ in range(3 * FPS): v.act() - v.step(dt=1/FPS) + v.step(dt=1 / FPS) assert v.speed == pytest.approx(20) - assert v.position[1] == pytest.approx(StraightLane.DEFAULT_WIDTH, abs=StraightLane.DEFAULT_WIDTH/4) + assert v.position[1] == pytest.approx( + StraightLane.DEFAULT_WIDTH, abs=StraightLane.DEFAULT_WIDTH / 4 + ) assert v.lane_index[2] == 1 def test_speed_control(): road = Road(RoadNetwork.straight_road_network(1)) - v = ControlledVehicle(road=road, position=road.network.get_lane(("0", "1", 0)).position(0, 0), speed=20, heading=0) - v.act('FASTER') + v = ControlledVehicle( + road=road, + position=road.network.get_lane(("0", "1", 0)).position(0, 0), + speed=20, + heading=0, + ) + v.act("FASTER") for _ in range(int(3 * v.TAU_ACC * FPS)): v.act() - v.step(dt=1/FPS) + v.step(dt=1 / FPS) assert v.speed == pytest.approx(20 + v.DELTA_SPEED, abs=0.5) assert v.position[1] == pytest.approx(0) assert v.lane_index[2] == 0 diff --git a/tests/vehicle/test_dynamics.py b/tests/vehicle/test_dynamics.py index 66975f5c7..18682c5ec 100644 --- a/tests/vehicle/test_dynamics.py +++ b/tests/vehicle/test_dynamics.py @@ -2,15 +2,15 @@ from highway_env.road.road import Road, RoadNetwork from highway_env.vehicle.kinematics import Vehicle -from highway_env.vehicle.objects import Obstacle, Landmark +from highway_env.vehicle.objects import Landmark, Obstacle FPS = 15 def test_step(): v = Vehicle(road=None, position=[0, 0], speed=20, heading=0) - for _ in range(2*FPS): - v.step(dt=1/FPS) + 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.speed == pytest.approx(20) @@ -19,14 +19,14 @@ def test_step(): def test_act(): v = Vehicle(road=None, position=[0, 0], speed=20, heading=0) - v.act({'acceleration': 1, 'steering': 0}) + v.act({"acceleration": 1, "steering": 0}) for _ in range(1 * FPS): - v.step(dt=1/FPS) + v.step(dt=1 / FPS) assert v.speed == pytest.approx(21) - v.act({'acceleration': 0, 'steering': 0.5}) + v.act({"acceleration": 0, "steering": 0.5}) for _ in range(1 * FPS): - v.step(dt=1/FPS) + v.step(dt=1 / FPS) assert v.speed == pytest.approx(21) assert v.position[1] > 0 @@ -34,8 +34,8 @@ def test_act(): def test_brake(): v = Vehicle(road=None, position=[0, 0], speed=20, heading=0) for _ in range(10 * FPS): - v.act({'acceleration': min(max(-1 * v.speed, -6), 6), 'steering': 0}) - v.step(dt=1/FPS) + v.act({"acceleration": min(max(-1 * v.speed, -6), 6), "steering": 0}) + v.step(dt=1 / FPS) assert v.speed == pytest.approx(0, abs=0.01) diff --git a/tests/vehicle/test_uncertainty.py b/tests/vehicle/test_uncertainty.py index 6e0ae0cd7..cd1848d4a 100644 --- a/tests/vehicle/test_uncertainty.py +++ b/tests/vehicle/test_uncertainty.py @@ -8,7 +8,7 @@ def test_partial(): road = Road(RoadNetwork.straight_road_network()) v = IntervalVehicle(road, position=[0, 0], speed=20, heading=0) for _ in range(2 * FPS): - v.step(dt=1/FPS, mode="partial") + v.step(dt=1 / FPS, mode="partial") assert v.interval.position[0, 0] <= v.position[0] <= v.interval.position[1, 0] assert v.interval.position[0, 1] <= v.position[1] <= v.interval.position[1, 1] assert v.interval.heading[0] <= v.heading <= v.interval.heading[1] @@ -18,7 +18,7 @@ def test_predictor(): road = Road(RoadNetwork.straight_road_network()) v = IntervalVehicle(road, position=[0, 0], speed=20, heading=0) for _ in range(2 * FPS): - v.step(dt=1/FPS, mode="predictor") + v.step(dt=1 / FPS, mode="predictor") assert v.interval.position[0, 0] <= v.position[0] <= v.interval.position[1, 0] assert v.interval.position[0, 1] <= v.position[1] <= v.interval.position[1, 1] assert v.interval.heading[0] <= v.heading <= v.interval.heading[1] From c43f08b15c89e1de256eab4c81fe6a7ce02f9f7a Mon Sep 17 00:00:00 2001 From: snow-fox Date: Sat, 18 Nov 2023 13:44:31 +0000 Subject: [PATCH 6/9] add pre-commit workflow --- .github/workflows/pre-commit.yml | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) create mode 100644 .github/workflows/pre-commit.yml diff --git a/.github/workflows/pre-commit.yml b/.github/workflows/pre-commit.yml new file mode 100644 index 000000000..80ce02af6 --- /dev/null +++ b/.github/workflows/pre-commit.yml @@ -0,0 +1,21 @@ +# https://pre-commit.com +# This GitHub Action assumes that the repo contains a valid .pre-commit-config.yaml file. +name: pre-commit +on: + pull_request: + push: + branches: [main] + +permissions: + contents: read # to fetch code (actions/checkout) + +jobs: + pre-commit: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + - uses: actions/setup-python@v4 + - run: python -m pip install pre-commit + - run: python -m pre_commit --version + - run: python -m pre_commit install + - run: python -m pre_commit run --all-files From 8d768b6cd22de41301df8488f2c1744da9e95321 Mon Sep 17 00:00:00 2001 From: snow-fox Date: Sat, 18 Nov 2023 13:44:45 +0000 Subject: [PATCH 7/9] work on master --- .github/workflows/pre-commit.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/pre-commit.yml b/.github/workflows/pre-commit.yml index 80ce02af6..9060a3791 100644 --- a/.github/workflows/pre-commit.yml +++ b/.github/workflows/pre-commit.yml @@ -4,7 +4,7 @@ name: pre-commit on: pull_request: push: - branches: [main] + branches: [master] permissions: contents: read # to fetch code (actions/checkout) From f1cd8cbf235d4761208f2cbda5f951ca1be7415e Mon Sep 17 00:00:00 2001 From: snow-fox Date: Sat, 18 Nov 2023 13:54:09 +0000 Subject: [PATCH 8/9] fix remaining errors --- .pre-commit-config.yaml | 2 +- docs/conf.py | 2 +- highway_env/__init__.py | 14 +++++++++++++- highway_env/road/regulation.py | 2 +- scripts/sb3_highway_dqn.py | 2 +- scripts/sb3_highway_dqn_cnn.py | 2 +- scripts/sb3_highway_ppo.py | 2 +- scripts/sb3_highway_ppo_transformer.py | 2 +- scripts/sb3_racetracks_ppo.py | 2 +- setup.cfg | 3 ++- setup.py | 18 +++++++++++++++++- 11 files changed, 40 insertions(+), 11 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index e9d84ff8a..cc95bb980 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -6,7 +6,7 @@ repos: hooks: - id: flake8 args: - - '--per-file-ignores=**/__init__.py:F401,F403' + - '--per-file-ignores=**/__init__.py:F401,F403,E402' - --ignore=E203,W503,E741,E731 - --max-complexity=30 - --max-line-length=456 diff --git a/docs/conf.py b/docs/conf.py index 7e3561ab1..efc6e3a74 100644 --- a/docs/conf.py +++ b/docs/conf.py @@ -18,7 +18,7 @@ import os from typing import Any, Dict -import highway_env +import highway_env # noqa: F401 project = "highway-env" copyright = "2023 Farama Foundation" diff --git a/highway_env/__init__.py b/highway_env/__init__.py index 3f1c9ff09..7241ff142 100644 --- a/highway_env/__init__.py +++ b/highway_env/__init__.py @@ -1,11 +1,23 @@ -# Hide pygame support prompt import os +import sys + +__version__ = "1.8.2" + +try: + from farama_notifications import notifications + if "highway_env" in notifications and __version__ in notifications["gymnasium"]: + print(notifications["highway_env"][__version__], file=sys.stderr) +except Exception: # nosec + pass + +# Hide pygame support prompt os.environ["PYGAME_HIDE_SUPPORT_PROMPT"] = "1" from gymnasium.envs.registration import register + def register_highway_envs(): """Import the envs module so that envs register themselves.""" diff --git a/highway_env/road/regulation.py b/highway_env/road/regulation.py index d0ed4328a..1a335a38d 100644 --- a/highway_env/road/regulation.py +++ b/highway_env/road/regulation.py @@ -5,8 +5,8 @@ from highway_env import utils from highway_env.road.road import Road, RoadNetwork from highway_env.vehicle.controller import ControlledVehicle, MDPVehicle -from highway_env.vehicle.objects import Obstacle from highway_env.vehicle.kinematics import Vehicle +from highway_env.vehicle.objects import Obstacle class RegulatedRoad(Road): diff --git a/scripts/sb3_highway_dqn.py b/scripts/sb3_highway_dqn.py index a46aba5b3..8aa964320 100644 --- a/scripts/sb3_highway_dqn.py +++ b/scripts/sb3_highway_dqn.py @@ -2,7 +2,7 @@ from gymnasium.wrappers import RecordVideo from stable_baselines3 import DQN -import highway_env +import highway_env # noqa: F401 TRAIN = True diff --git a/scripts/sb3_highway_dqn_cnn.py b/scripts/sb3_highway_dqn_cnn.py index c9f5cdb1a..057fa71b1 100644 --- a/scripts/sb3_highway_dqn_cnn.py +++ b/scripts/sb3_highway_dqn_cnn.py @@ -2,7 +2,7 @@ from stable_baselines3 import DQN from stable_baselines3.common.vec_env import DummyVecEnv, VecVideoRecorder -import highway_env +import highway_env # noqa: F401 def train_env(): diff --git a/scripts/sb3_highway_ppo.py b/scripts/sb3_highway_ppo.py index a881937a4..372c271c8 100644 --- a/scripts/sb3_highway_ppo.py +++ b/scripts/sb3_highway_ppo.py @@ -3,7 +3,7 @@ from stable_baselines3.common.env_util import make_vec_env from stable_baselines3.common.vec_env import SubprocVecEnv -import highway_env +import highway_env # noqa: F401 # ================================== # Main script diff --git a/scripts/sb3_highway_ppo_transformer.py b/scripts/sb3_highway_ppo_transformer.py index aa3702400..aa234de3b 100644 --- a/scripts/sb3_highway_ppo_transformer.py +++ b/scripts/sb3_highway_ppo_transformer.py @@ -14,7 +14,7 @@ from torch.distributions import Categorical from torch.nn import functional as F -import highway_env +import highway_env # noqa: F401 from highway_env.utils import lmap # ================================== diff --git a/scripts/sb3_racetracks_ppo.py b/scripts/sb3_racetracks_ppo.py index 88a06f462..029528fe9 100644 --- a/scripts/sb3_racetracks_ppo.py +++ b/scripts/sb3_racetracks_ppo.py @@ -4,7 +4,7 @@ from stable_baselines3.common.env_util import make_vec_env from stable_baselines3.common.vec_env import SubprocVecEnv -import highway_env +import highway_env # noqa: F401 TRAIN = True diff --git a/setup.cfg b/setup.cfg index 876252256..6417bab87 100644 --- a/setup.cfg +++ b/setup.cfg @@ -1,6 +1,6 @@ [metadata] name=highway-env -version=1.8.2 +version=attr: my_package.__version__ author=Edouard Leurent author_email=eleurent@gmail.com description=An environment for simulated highway driving tasks. @@ -18,6 +18,7 @@ classifiers= setup_requires= pytest-runner install_requires= + farama-notifications gymnasium>=0.27 numpy pygame>=2.0.2 diff --git a/setup.py b/setup.py index 098261bee..e5e5d6041 100644 --- a/setup.py +++ b/setup.py @@ -2,7 +2,23 @@ # Unfortunately it is still required py the pip editable mode `pip install -e` # See https://stackoverflow.com/a/60885212 +import pathlib + from setuptools import setup +CWD = pathlib.Path(__file__).absolute().parent + + +def get_version(): + """Gets the gymnasium version.""" + path = CWD / "highway_env" / "__init__.py" + content = path.read_text() + + for line in content.splitlines(): + if line.startswith("__version__"): + return line.strip().split()[-1].strip().strip('"') + raise RuntimeError("bad version data in __init__.py") + + if __name__ == "__main__": - setup() + setup(version=get_version()) From c23822b012e952ebc0bf1b53c404ebe2c8e96c21 Mon Sep 17 00:00:00 2001 From: snow-fox Date: Sat, 18 Nov 2023 13:55:24 +0000 Subject: [PATCH 9/9] add funding.yaml --- .github/FUNDING.yml | 1 + 1 file changed, 1 insertion(+) create mode 100644 .github/FUNDING.yml diff --git a/.github/FUNDING.yml b/.github/FUNDING.yml new file mode 100644 index 000000000..c5d88ac82 --- /dev/null +++ b/.github/FUNDING.yml @@ -0,0 +1 @@ +github: Farama-Foundation