diff --git a/examples/waypoint_follow.py b/examples/waypoint_follow.py index 1cda9966..596ac28b 100644 --- a/examples/waypoint_follow.py +++ b/examples/waypoint_follow.py @@ -310,7 +310,7 @@ def main(): "model": "st", "observation_config": {"type": "kinematic_state"}, "params": {"mu": 1.0}, - "reset_config": {"type": "random_static"}, + "reset_config": {"type": "rl_random_static"}, }, render_mode="human", ) diff --git a/gym/f110_gym/envs/f110_env.py b/gym/f110_gym/envs/f110_env.py index 5cb1ad41..bb14d753 100644 --- a/gym/f110_gym/envs/f110_env.py +++ b/gym/f110_gym/envs/f110_env.py @@ -226,8 +226,8 @@ def default_config(cls) -> dict: "integrator": "rk4", "model": "st", "control_input": ["speed", "steering_angle"], - "observation_config": {"type": "original"}, - "reset_config": {"type": "grid_static"}, + "observation_config": {"type": None}, + "reset_config": {"type": None}, } def configure(self, config: dict) -> None: diff --git a/gym/f110_gym/envs/observation.py b/gym/f110_gym/envs/observation.py index 855a5838..263f8260 100644 --- a/gym/f110_gym/envs/observation.py +++ b/gym/f110_gym/envs/observation.py @@ -1,6 +1,4 @@ -""" -Author: Luigi Berducci -""" +from __future__ import annotations from abc import abstractmethod from typing import List @@ -265,7 +263,9 @@ def observe(self): return obs -def observation_factory(env, type: str, **kwargs) -> Observation: +def observation_factory(env, type: str | None, **kwargs) -> Observation: + type = type or "original" + if type == "original": return OriginalObservation(env) elif type == "features": diff --git a/gym/f110_gym/envs/reset/__init__.py b/gym/f110_gym/envs/reset/__init__.py index c6e05ba8..d5f56ea1 100644 --- a/gym/f110_gym/envs/reset/__init__.py +++ b/gym/f110_gym/envs/reset/__init__.py @@ -1,20 +1,21 @@ +from __future__ import annotations from f110_gym.envs.reset.masked_reset import GridResetFn, AllTrackResetFn from f110_gym.envs.reset.reset_fn import ResetFn from f110_gym.envs.track import Track -def make_reset_fn(type: str, track: Track, num_agents: int, **kwargs) -> ResetFn: - if type == "grid_static": - return GridResetFn(track=track, num_agents=num_agents, shuffle=False, **kwargs) - elif type == "grid_random": - return GridResetFn(track=track, num_agents=num_agents, shuffle=True, **kwargs) - elif type == "random_static": - return AllTrackResetFn( - track=track, num_agents=num_agents, shuffle=False, **kwargs - ) - elif type == "random_random": - return AllTrackResetFn( - track=track, num_agents=num_agents, shuffle=True, **kwargs - ) - else: - raise ValueError(f"invalid reset type {type}") +def make_reset_fn(type: str | None, track: Track, num_agents: int, **kwargs) -> ResetFn: + type = type or "rl_grid_static" + + try: + refline_token, reset_token, shuffle_token = type.split("_") + + refline = {"cl": track.centerline, "rl": track.raceline}[refline_token] + reset_fn = {"grid": GridResetFn, "random": AllTrackResetFn}[reset_token] + shuffle = {"static": False, "random": True}[shuffle_token] + options = {"cl": {"move_laterally": True}, "rl": {"move_laterally": False}}[refline_token] + + except Exception as ex: + raise ValueError(f"Invalid reset function type: {type}. Expected format: __") from ex + + return reset_fn(reference_line=refline, num_agents=num_agents, shuffle=shuffle, **options, **kwargs) diff --git a/gym/f110_gym/envs/reset/masked_reset.py b/gym/f110_gym/envs/reset/masked_reset.py index cac879cd..1f4e6983 100644 --- a/gym/f110_gym/envs/reset/masked_reset.py +++ b/gym/f110_gym/envs/reset/masked_reset.py @@ -4,7 +4,7 @@ from f110_gym.envs.reset.reset_fn import ResetFn from f110_gym.envs.reset.utils import sample_around_waypoint -from f110_gym.envs.track import Track +from f110_gym.envs.track import Track, Raceline class MaskedResetFn(ResetFn): @@ -14,23 +14,24 @@ def get_mask(self) -> np.ndarray: def __init__( self, - track: Track, + reference_line: Raceline, num_agents: int, move_laterally: bool, min_dist: float, max_dist: float, ): - self.track = track + self.reference_line = reference_line self.n_agents = num_agents self.min_dist = min_dist self.max_dist = max_dist self.move_laterally = move_laterally self.mask = self.get_mask() + self.reference_line = reference_line def sample(self) -> np.ndarray: waypoint_id = np.random.choice(np.where(self.mask)[0]) poses = sample_around_waypoint( - track=self.track, + reference_line=self.reference_line, waypoint_id=waypoint_id, n_agents=self.n_agents, min_dist=self.min_dist, @@ -43,9 +44,10 @@ def sample(self) -> np.ndarray: class GridResetFn(MaskedResetFn): def __init__( self, - track: Track, + reference_line: Raceline, num_agents: int, move_laterally: bool = True, + use_centerline: bool = True, shuffle: bool = True, start_width: float = 1.0, min_dist: float = 1.5, @@ -55,7 +57,7 @@ def __init__( self.shuffle = shuffle super().__init__( - track=track, + reference_line=reference_line, num_agents=num_agents, move_laterally=move_laterally, min_dist=min_dist, @@ -64,10 +66,10 @@ def __init__( def get_mask(self) -> np.ndarray: # approximate the nr waypoints in the starting line - step_size = self.track.centerline.length / self.track.centerline.n + step_size = self.reference_line.length / self.reference_line.n n_wps = int(self.start_width / step_size) - mask = np.zeros(self.track.centerline.n) + mask = np.zeros(self.reference_line.n) mask[:n_wps] = 1 return mask.astype(bool) @@ -83,7 +85,7 @@ def sample(self) -> np.ndarray: class AllTrackResetFn(MaskedResetFn): def __init__( self, - track: Track, + reference_line: Raceline, num_agents: int, move_laterally: bool = True, shuffle: bool = True, @@ -91,7 +93,7 @@ def __init__( max_dist: float = 2.5, ): super().__init__( - track=track, + reference_line=reference_line, num_agents=num_agents, move_laterally=move_laterally, min_dist=min_dist, @@ -100,7 +102,7 @@ def __init__( self.shuffle = shuffle def get_mask(self) -> np.ndarray: - return np.ones(self.track.centerline.n).astype(bool) + return np.ones(self.reference_line.n).astype(bool) def sample(self) -> np.ndarray: poses = super().sample() diff --git a/gym/f110_gym/envs/reset/utils.py b/gym/f110_gym/envs/reset/utils.py index 40ce9a73..f345276c 100644 --- a/gym/f110_gym/envs/reset/utils.py +++ b/gym/f110_gym/envs/reset/utils.py @@ -2,11 +2,11 @@ import numpy as np -from f110_gym.envs.track import Track +from f110_gym.envs.track import Track, Raceline def sample_around_waypoint( - track: Track, + reference_line: Raceline, waypoint_id: int, n_agents: int, min_dist: float, @@ -26,22 +26,22 @@ def sample_around_waypoint( - move_laterally: if True, the agents are sampled on the left/right of the track centerline """ current_wp_id = waypoint_id - n_waypoints = track.centerline.n + n_waypoints = reference_line.n poses = [] rnd_sign = ( - np.random.choice([-1, 1]) if move_laterally else 1 + np.random.choice([-1.0, 1.0]) if move_laterally else 0.0 ) # random sign to sample lateral position (left/right) for i in range(n_agents): # compute pose from current wp_id wp = [ - track.centerline.xs[current_wp_id], - track.centerline.ys[current_wp_id], + reference_line.xs[current_wp_id], + reference_line.ys[current_wp_id], ] next_wp_id = (current_wp_id + 1) % n_waypoints next_wp = [ - track.centerline.xs[next_wp_id], - track.centerline.ys[next_wp_id], + reference_line.xs[next_wp_id], + reference_line.ys[next_wp_id], ] theta = np.arctan2(next_wp[1] - wp[1], next_wp[0] - wp[0]) @@ -65,8 +65,8 @@ def sample_around_waypoint( if pnt_id > n_waypoints - 1: pnt_id = 0 # increment distance - x_diff = track.centerline.xs[pnt_id] - track.centerline.xs[pnt_id - 1] - y_diff = track.centerline.ys[pnt_id] - track.centerline.ys[pnt_id - 1] + x_diff = reference_line.xs[pnt_id] - reference_line.xs[pnt_id - 1] + y_diff = reference_line.ys[pnt_id] - reference_line.ys[pnt_id - 1] dist = dist + np.linalg.norm( [y_diff, x_diff] ) # approx distance by summing linear segments diff --git a/gym/f110_gym/test/benchmark_renderer.py b/gym/f110_gym/test/benchmark_renderer.py deleted file mode 100644 index 4cfc4277..00000000 --- a/gym/f110_gym/test/benchmark_renderer.py +++ /dev/null @@ -1,222 +0,0 @@ -import numpy as np - -from f110_gym.envs import F110Env -from f110_gym.envs.utils import deep_update - - -def pretty_print(dict: dict, col_width=15): - keys = list(dict.keys()) - columns = ["key"] + [str(k) for k in dict[keys[0]]] - - # opening line - for _ in columns: - print("|" + "-" * col_width, end="") - print("|") - # header - for col in columns: - padding = max(0, col_width - len(col)) - print("|" + col[:col_width] + " " * padding, end="") - print("|") - # separator line - for _ in columns: - print("|" + "-" * col_width, end="") - print("|") - - # table - for key in keys: - padding = max(0, col_width - len(str(key))) - print("|" + str(key)[:col_width] + " " * padding, end="") - for col in columns[1:]: - padding = max(0, col_width - len(str(dict[key][col]))) - print("|" + str(dict[key][col])[:col_width] + " " * padding, end="") - print("|") - - # footer - for col in columns: - print("|" + "-" * col_width, end="") - print("|") - - -class BenchmarkRenderer: - @staticmethod - def _make_env(config={}, render_mode=None) -> F110Env: - import gymnasium as gym - import f110_gym - - base_config = { - "map": "Spielberg", - "num_agents": 1, - "timestep": 0.01, - "integrator": "rk4", - "control_input": ["speed", "steering_angle"], - "model": "st", - "observation_config": {"type": "kinematic_state"}, - "params": {"mu": 1.0}, - } - config = deep_update(base_config, config) - - env = gym.make( - "f110_gym:f110-v0", - config=config, - render_mode=render_mode, - ) - - return env - - def benchmark_single_agent_rendering(self): - import time - - sim_time = 15.0 # seconds - results = {} - - for render_mode in [None, "human", "human_fast", "rgb_array", "rgb_array_list"]: - env = self._make_env(render_mode=render_mode) - env.reset() - frame = env.render() - - print( - f"Running simulation of {sim_time}s for render mode: {render_mode}..." - ) - - max_steps = int(sim_time / env.timestep) - t0 = time.time() - for _ in range(max_steps): - action = env.action_space.sample() - env.step(action) - frame = env.render() - tf = time.time() - env.close() - - results[render_mode] = { - "sim_time": sim_time, - "elapsed_time": tf - t0, - "fps": max_steps / (tf - t0), - } - - pretty_print(results) - - def benchmark_n_agents_human_rendering(self): - """ - This is meant to benchmark the human rendering mode, for increasing nr of agents. - """ - import time - - sim_time = 15.0 # seconds - render_mode = "human" - - results = {} - - for num_agents in [1, 2, 3, 4, 5, 10]: - env = self._make_env( - config={"num_agents": num_agents}, render_mode=render_mode - ) - env.reset() - frame = env.render() - - print( - f"Running simulation of {num_agents} agents for render mode: {render_mode}..." - ) - - max_steps = int(sim_time / env.timestep) - t0 = time.time() - for _ in range(max_steps): - action = env.action_space.sample() - env.step(action) - frame = env.render() - tf = time.time() - env.close() - - results[num_agents] = { - "sim_time": sim_time, - "elapsed_time": tf - t0, - "fps": max_steps / (tf - t0), - } - - pretty_print(results) - - def benchmark_callbacks_human_rendering(self): - import time - - sim_time = 15.0 # seconds - render_mode = "human" - - results = {} - - class GoStraightPlanner: - def __init__(self, env, agent_id: str = "agent_0"): - self.waypoints = np.stack( - [env.track.raceline.xs, env.track.raceline.ys] - ).T - self.pos = None - self.agent_id = agent_id - - def plan(self, obs): - state = obs[self.agent_id] - self.pos = np.array([state["pose_x"], state["pose_y"]]) - return np.array([0.0, 2.5]) - - def render_waypoints(self, e): - e.render_closed_lines(points=self.waypoints, size=1) - - def render_position(self, e): - if self.pos is not None: - points = self.pos[None] - e.render_points(points, size=1) - - for render_config in [[False, False], [True, False], [True, True]]: - env = self._make_env(render_mode=render_mode) - planner = GoStraightPlanner(env) - - show_path, show_point = render_config - config_str = f"show_path={show_path}, show_point={show_point}" - - if show_path: - env.add_render_callback(callback_func=planner.render_waypoints) - - if show_point: - env.add_render_callback(callback_func=planner.render_position) - - rnd_idx = np.random.randint(0, len(env.track.raceline.xs)) - obs, _ = env.reset( - options={ - "poses": np.array( - [ - [ - env.track.raceline.xs[rnd_idx], - env.track.raceline.ys[rnd_idx], - env.track.raceline.yaws[rnd_idx], - ] - ] - ) - } - ) - frame = env.render() - - print( - f"Running simulation of {config_str} for render mode: {render_mode}..." - ) - - max_steps = int(sim_time / env.timestep) - t0 = time.time() - for _ in range(max_steps): - action = planner.plan(obs=obs) - obs, _, _, _, _ = env.step(np.array([action])) - frame = env.render() - tf = time.time() - env.close() - - results[config_str] = { - "sim_time": sim_time, - "elapsed_time": tf - t0, - "fps": max_steps / (tf - t0), - } - - pretty_print(results) - - -if __name__ == "__main__": - benchmark = BenchmarkRenderer() - - benchmark.benchmark_single_agent_rendering() - benchmark.benchmark_n_agents_human_rendering() - benchmark.benchmark_callbacks_human_rendering() diff --git a/gym/f110_gym/test/test_scan_sim.py b/gym/f110_gym/test/test_scan_sim.py deleted file mode 100644 index e69de29b..00000000 diff --git a/tests/test_f110_env.py b/tests/test_f110_env.py index 545b9ea3..053a2b20 100644 --- a/tests/test_f110_env.py +++ b/tests/test_f110_env.py @@ -217,7 +217,7 @@ def test_auto_reset_options_in_synch_vec_env(self): config = { "num_agents": num_agents, "observation_config": {"type": "kinematic_state"}, - "reset_config": {"type": "random_random"}, + "reset_config": {"type": "rl_random_random"}, } vec_env = gym.vector.make( "f110_gym:f110-v0", asynchronous=False, config=config, num_envs=num_envs diff --git a/gym/f110_gym/test/test_renderer.py b/tests/test_renderer.py similarity index 100% rename from gym/f110_gym/test/test_renderer.py rename to tests/test_renderer.py