diff --git a/docs/src/conf.py b/docs/src/conf.py index 93d2ef20..77d484e4 100644 --- a/docs/src/conf.py +++ b/docs/src/conf.py @@ -65,4 +65,6 @@ # "**": ["search-field.html", "sidebar-nav-bs.html", "sidebar-ethical-ads.html"] # } html_last_updated_fmt = "%b %d, %Y" -html_show_sourcelink = True \ No newline at end of file +html_show_sourcelink = True + +copybutton_prompt_text = ">>> " \ No newline at end of file diff --git a/gym/f110_gym/envs/collision_models.py b/gym/f110_gym/envs/collision_models.py index 3f7ac235..bd6cec8d 100644 --- a/gym/f110_gym/envs/collision_models.py +++ b/gym/f110_gym/envs/collision_models.py @@ -4,14 +4,17 @@ @njit(cache=True) def perpendicular(pt): - """ - Return a 2-vector's perpendicular vector + """Return a 2-vector's perpendicular vector - Args: - pt (np.ndarray, (2,)): input vector + Parameters + ---------- + pt : np.ndarray + input vector - Returns: - pt (np.ndarray, (2,)): perpendicular vector + Returns + ------- + np.ndarray + perpendicular vector """ temp = pt[0] pt[0] = pt[1] @@ -21,14 +24,21 @@ def perpendicular(pt): @njit(cache=True) def tripleProduct(a, b, c): - """ - Return triple product of three vectors - - Args: - a, b, c (np.ndarray, (2,)): input vectors - - Returns: - (np.ndarray, (2,)): triple product + """Return triple product of three vectors + + Parameters + ---------- + a : np.ndarray + input vector + b : np.ndarray + input vector + c : np.ndarray + input vector + + Returns + ------- + np.ndarray + triple product """ ac = a.dot(c) bc = b.dot(c) @@ -37,44 +47,57 @@ def tripleProduct(a, b, c): @njit(cache=True) def avgPoint(vertices): - """ - Return the average point of multiple vertices + """Return the average point of multiple vertices - Args: - vertices (np.ndarray, (n, 2)): the vertices we want to find avg on + Parameters + ---------- + vertices : np.ndarray + the vertices we want to find avg on - Returns: - avg (np.ndarray, (2,)): average point of the vertices + Returns + ------- + np.ndarray + average point of the vertices """ return np.sum(vertices, axis=0) / vertices.shape[0] @njit(cache=True) def indexOfFurthestPoint(vertices, d): - """ - Return the index of the vertex furthest away along a direction in the list of vertices - - Args: - vertices (np.ndarray, (n, 2)): the vertices we want to find avg on - - Returns: - idx (int): index of the furthest point + """Return the index of the vertex furthest away along a direction in the list of vertices + + Parameters + ---------- + vertices : np.ndarray + the vertices we want to find index on + d : np.ndarray + direction + + Returns + ------- + int + index of the furthest point """ return np.argmax(vertices.dot(d)) @njit(cache=True) def support(vertices1, vertices2, d): - """ - Minkowski sum support function for GJK - - Args: - vertices1 (np.ndarray, (n, 2)): vertices of the first body - vertices2 (np.ndarray, (n, 2)): vertices of the second body - d (np.ndarray, (2, )): direction to find the support along - - Returns: - support (np.ndarray, (n, 2)): Minkowski sum + """Minkowski sum support function for GJK + + Parameters + ---------- + vertices1 : np.ndarray + vertices of the first body + vertices2 : np.ndarray + vertices of the second body + d : np.ndarray + direction to find the support along + + Returns + ------- + np.ndarray + Minkowski sum """ i = indexOfFurthestPoint(vertices1, d) j = indexOfFurthestPoint(vertices2, -d) @@ -83,15 +106,19 @@ def support(vertices1, vertices2, d): @njit(cache=True) def collision(vertices1, vertices2): - """ - GJK test to see whether two bodies overlap - - Args: - vertices1 (np.ndarray, (n, 2)): vertices of the first body - vertices2 (np.ndarray, (n, 2)): vertices of the second body - - Returns: - overlap (boolean): True if two bodies collide + """GJK test to see whether two bodies overlap + + Parameters + ---------- + vertices1 : np.ndarray + vertices of the first body + vertices2 : np.ndarray + vertices of the second body + + Returns + ------- + boolean + True if two bodies collide """ index = 0 simplex = np.empty((3, 2)) @@ -155,15 +182,19 @@ def collision(vertices1, vertices2): @njit(cache=True) def collision_multiple(vertices): - """ - Check pair-wise collisions for all provided vertices - - Args: - vertices (np.ndarray (num_bodies, 4, 2)): all vertices for checking pair-wise collision - - Returns: - collisions (np.ndarray (num_vertices, )): whether each body is in collision - collision_idx (np.ndarray (num_vertices, )): which index of other body is each index's body is in collision, -1 if not in collision + """Check pair-wise collisions for all provided vertices + + Parameters + ---------- + vertices : np.ndarray + all vertices for checking pair-wise collision + + Returns + ------- + collisions : np.ndarray + whether each body is in collision + collision_idx : np.ndarray + which index of other body is each index's body is in collision, -1 if not in collision """ collisions = np.zeros((vertices.shape[0],)) collision_idx = -1 * np.ones((vertices.shape[0],)) @@ -184,21 +215,19 @@ def collision_multiple(vertices): return collisions, collision_idx -""" -Utility functions for getting vertices by pose and shape -""" - - @njit(cache=True) def get_trmtx(pose): - """ - Get transformation matrix of vehicle frame -> global frame + """Get transformation matrix of vehicle frame -> global frame - Args: - pose (np.ndarray (3, )): current pose of the vehicle + Parameters + ---------- + pose : np.ndarray + current pose of the vehicle - return: - H (np.ndarray (4, 4)): transformation matrix + Returns + ------- + np.ndarray + transformation matrix """ x = pose[0] y = pose[1] @@ -218,16 +247,21 @@ def get_trmtx(pose): @njit(cache=True) def get_vertices(pose, length, width): - """ - Utility function to return vertices of the car body given pose and size - - Args: - pose (np.ndarray, (3, )): current world coordinate pose of the vehicle - length (float): car length - width (float): car width - - Returns: - vertices (np.ndarray, (4, 2)): corner vertices of the vehicle body + """Utility function to return vertices of the car body given pose and size + + Parameters + ---------- + pose : np.ndarray + current world coordinate pose of the vehicle + length : float + car length + width : float + car width + + Returns + ------- + np.ndarray + corner vertices of the vehicle body """ H = get_trmtx(pose) rl = H.dot(np.asarray([[-length / 2], [width / 2], [0.0], [1.0]])).flatten() diff --git a/gym/f110_gym/envs/dynamic_models.py b/gym/f110_gym/envs/dynamic_models.py index 112126ec..57ac1b42 100644 --- a/gym/f110_gym/envs/dynamic_models.py +++ b/gym/f110_gym/envs/dynamic_models.py @@ -6,11 +6,37 @@ class DynamicModel(Enum): + """Enum for specifying dynamic models + + KS: 1 (kinematic single track) + + ST: 2 (single track) + + """ + KS = 1 # Kinematic Single Track ST = 2 # Single Track @staticmethod def from_string(model: str): + """Set dynamic models from string. + + Parameters + ---------- + model : str + dynamic model type + + Returns + ------- + int + dynamic model type + + + Raises + ------ + ValueError + Unknown model type + """ if model == "ks": warnings.warn( "Chosen model is KS. This is different from previous versions of the gym." @@ -22,6 +48,23 @@ def from_string(model: str): raise ValueError(f"Unknown model type {model}") def get_initial_state(self, pose=None): + """Set initial dynamic state based on model + + Parameters + ---------- + pose : np.ndarray, optional + initial pose, by default None + + Returns + ------- + np.ndarray + initial state + + Raises + ------ + ValueError + Unknown model type + """ # initialize zero state if self == DynamicModel.KS: # state is [x, y, steer_angle, vel, yaw_angle] @@ -41,6 +84,18 @@ def get_initial_state(self, pose=None): @property def f_dynamics(self): + """property, returns dynamics function + + Returns + ------- + Callable + dynamic function + + Raises + ------ + ValueError + Unknown model type + """ if self == DynamicModel.KS: return vehicle_dynamics_ks elif self == DynamicModel.ST: @@ -51,16 +106,21 @@ def f_dynamics(self): @njit(cache=True) def upper_accel_limit(vel, a_max, v_switch): - """ - Upper acceleration limit, adjusts the acceleration based on constraints - - Args: - vel (float): current velocity of the vehicle - a_max (float): maximum allowed acceleration, symmetrical - v_switch (float): switching velocity (velocity at which the acceleration is no longer able to create wheel spin) - - Returns: - positive_accel_limit (float): adjusted acceleration + """Upper acceleration limit, adjusts the acceleration based on constraints + + Parameters + ---------- + vel : float + current velocity of the vehicle + a_max : float + maximum allowed acceleration, symmetrical + v_switch : float + switching velocity (velocity at which the acceleration is no longer able to create wheel spin) + + Returns + ------- + float + adjusted acceleration """ if vel > v_switch: pos_limit = a_max * (v_switch / vel) @@ -72,21 +132,28 @@ def upper_accel_limit(vel, a_max, v_switch): @njit(cache=True) def accl_constraints(vel, a_long_d, v_switch, a_max, v_min, v_max): + """Acceleration constraints, adjusts the acceleration based on constraints + + Parameters + ---------- + vel : float + current velocity of the vehicle + a_long_d : float + unconstrained desired acceleration in the direction of travel. + v_switch : float + switching velocity (velocity at which the acceleration is no longer able to create wheel spin) + a_max : float + maximum allowed acceleration, symmetrical + v_min : float + minimum allowed velocity + v_max : float + maximum allowed velocity + + Returns + ------- + float + adjusted acceleration """ - Acceleration constraints, adjusts the acceleration based on constraints - - Args: - vel (float): current velocity of the vehicle - a_long_d (float): unconstrained desired acceleration in the direction of travel. - v_switch (float): switching velocity (velocity at which the acceleration is no longer able to create wheel spin) - a_max (float): maximum allowed acceleration, symmetrical - v_min (float): minimum allowed velocity - v_max (float): maximum allowed velocity - - Returns: - accl (float): adjusted acceleration - """ - uac = upper_accel_limit(vel, a_max, v_switch) if (vel <= v_min and a_long_d <= 0) or (vel >= v_max and a_long_d >= 0): @@ -105,21 +172,28 @@ def accl_constraints(vel, a_long_d, v_switch, a_max, v_min, v_max): def steering_constraint( steering_angle, steering_velocity, s_min, s_max, sv_min, sv_max ): + """Steering constraints, adjusts the steering velocity based on constraints + + Parameters + ---------- + steering_angle : float + current steering_angle of the vehicle + steering_velocity : float + unconstraint desired steering_velocity + s_min : float + minimum steering angle + s_max : float + maximum steering angle + sv_min : float + minimum steering velocity + sv_max : float + maximum steering velocity + + Returns + ------- + float + adjusted steering velocity """ - Steering constraints, adjusts the steering velocity based on constraints - - Args: - steering_angle (float): current steering_angle of the vehicle - steering_velocity (float): unconstraint desired steering_velocity - s_min (float): minimum steering angle - s_max (float): maximum steering angle - sv_min (float): minimum steering velocity - sv_max (float): maximum steering velocity - - Returns: - steering_velocity (float): adjusted steering velocity - """ - # constraint steering velocity if (steering_angle <= s_min and steering_velocity <= 0) or ( steering_angle >= s_max and steering_velocity >= 0 @@ -154,39 +228,59 @@ def vehicle_dynamics_ks( v_min, v_max, ): - """ - Single Track Kinematic Vehicle Dynamics. + """Kinematic Single Track Vehicle Dynamics. Follows https://gitlab.lrz.de/tum-cps/commonroad-vehicle-models/-/blob/master/vehicleModels_commonRoad.pdf, section 5 - Args: - x (numpy.ndarray (5, )): vehicle state vector (x0, x1, x2, x3, x4) - x0: x position in global coordinates - x1: y position in global coordinates - x2: steering angle of front wheels - x3: velocity in x direction - x4: yaw angle - u (numpy.ndarray (2, )): control input vector (u1, u2) - u1: steering angle velocity of front wheels - u2: longitudinal acceleration - mu (float): friction coefficient - C_Sf (float): cornering stiffness of front wheels - C_Sr (float): cornering stiffness of rear wheels - lf (float): distance from center of gravity to front axle - lr (float): distance from center of gravity to rear axle - h (float): height of center of gravity - m (float): mass of vehicle - I (float): moment of inertia of vehicle, about Z axis - s_min (float): minimum steering angle - s_max (float): maximum steering angle - sv_min (float): minimum steering velocity - sv_max (float): maximum steering velocity - v_switch (float): velocity above which the acceleration is no longer able to create wheel slip - a_max (float): maximum allowed acceleration - v_min (float): minimum allowed velocity - v_max (float): maximum allowed velocity - - Returns: - f (numpy.ndarray): right hand side of differential equations + Parameters + ---------- + x : np.ndarray + vehicle state vector (x0, x1, x2, x3, x4) + x0: x position in global coordinates + x1: y position in global coordinates + x2: steering angle of front wheels + x3: velocity in x direction + x4: yaw angle + u_init : np.ndarray + control input vector (u1, u2) + u1: steering angle velocity of front wheels + u2: longitudinal acceleration + mu : float + friction coefficient + C_Sf : float + cornering stiffness of front wheels + C_Sr : float + cornering stiffness of rear wheels + lf : float + distance from center of gravity to front axle + lr : float + distance from center of gravity to rear axle + h : float + height of center of gravity + m : float + mass of vehicle + I : float + moment of inertia of vehicle, about Z axis + s_min : float + minimum steering angle + s_max : float + maximum steering angle + sv_min : float + minimum steering velocity + sv_max : float + maximum steering velocity + v_switch : float + velocity above which the acceleration is no longer able to create wheel slip + a_max : float + maximum allowed acceleration + v_min : float + minimum allowed velocity + v_max : float + maximum allowed velocity + + Returns + ------- + np.ndarray + right hand side of differential equations """ # Controls X = x[0] @@ -245,41 +339,61 @@ def vehicle_dynamics_st( v_min, v_max, ): - """ - Single Track Vehicle Dynamics. + """Single Track Vehicle Dynamics. From https://gitlab.lrz.de/tum-cps/commonroad-vehicle-models/-/blob/master/vehicleModels_commonRoad.pdf, section 7 - Args: - x (numpy.ndarray (7, )): vehicle state vector (x0, x1, x2, x3, x4, x5, x6) - x0: x position in global coordinates - x1: y position in global coordinates - x2: steering angle of front wheels - x3: velocity in x direction - x4:yaw angle - x5: yaw rate - x6: slip angle at vehicle center - u (numpy.ndarray (2, )): control input vector (u1, u2) - u1: steering angle velocity of front wheels - u2: longitudinal acceleration - mu (float): friction coefficient - C_Sf (float): cornering stiffness of front wheels - C_Sr (float): cornering stiffness of rear wheels - lf (float): distance from center of gravity to front axle - lr (float): distance from center of gravity to rear axle - h (float): height of center of gravity - m (float): mass of vehicle - I (float): moment of inertia of vehicle, about Z axis - s_min (float): minimum steering angle - s_max (float): maximum steering angle - sv_min (float): minimum steering velocity - sv_max (float): maximum steering velocity - v_switch (float): velocity above which the acceleration is no longer able to create wheel spin - a_max (float): maximum allowed acceleration - v_min (float): minimum allowed velocity - v_max (float): maximum allowed velocity - - Returns: - f (numpy.ndarray): right hand side of differential equations + Parameters + ---------- + x : np.ndarray + vehicle state vector (x0, x1, x2, x3, x4, x5, x6) + x0: x position in global coordinates + x1: y position in global coordinates + x2: steering angle of front wheels + x3: velocity in x direction + x4:yaw angle + x5: yaw rate + x6: slip angle at vehicle center + u_init : np.ndarray + control input vector (u1, u2) + u1: steering angle velocity of front wheels + u2: longitudinal acceleration + mu : float + friction coefficient + C_Sf : float + cornering stiffness of front wheels + C_Sr : float + cornering stiffness of rear wheels + lf : float + distance from center of gravity to front axle + lr : float + distance from center of gravity to rear axle + h : float + height of center of gravity + m : float + mass of vehicle + I : float + moment of inertia of vehicle, about Z axis + s_min : float + minimum steering angle + s_max : float + maximum steering angle + sv_min : float + minimum steering velocity + sv_max : float + maximum steering velocity + v_switch : float + velocity above which the acceleration is no longer able to create wheel slip + a_max : float + maximum allowed acceleration + v_min : float + minimum allowed velocity + v_max : float + maximum allowed velocity + + Returns + ------- + np.ndarray + right hand side of differential equations """ # States X = x[0] @@ -374,6 +488,22 @@ def vehicle_dynamics_st( @njit(cache=True) def pid_steer(steer, current_steer, max_sv): + """PID control for steering angle to steering velocity + + Parameters + ---------- + steer : float + requested steering angle + current_steer : float + current steering angle + max_sv : float + maximum steering velocity + + Returns + ------- + float + steering velocity + """ # steering steer_diff = steer - current_steer if np.fabs(steer_diff) > 1e-4: @@ -386,16 +516,25 @@ def pid_steer(steer, current_steer, max_sv): @njit(cache=True) def pid_accl(speed, current_speed, max_a, max_v, min_v): - """ - Basic controller for speed/steer -> accl./steer vel. - - Args: - speed (float): desired input speed - steer (float): desired input steering angle - - Returns: - accl (float): desired input acceleration - sv (float): desired input steering velocity + """PID control for speed to acceleration + + Parameters + ---------- + speed : float + requested speed + current_speed : float + current speed + max_a : float + maximum acceleration + max_v : float + maximum velocity + min_v : float + minimum velocity + + Returns + ------- + float + acceleration """ # accl vel_diff = speed - current_speed diff --git a/gym/f110_gym/envs/f110_env.py b/gym/f110_gym/envs/f110_env.py index 17af4c06..5008e125 100644 --- a/gym/f110_gym/envs/f110_env.py +++ b/gym/f110_gym/envs/f110_env.py @@ -1,8 +1,7 @@ # gym imports import gymnasium as gym -from f110_gym.envs.action import (CarAction, - from_single_to_multi_action_space) +from f110_gym.envs.action import CarAction, from_single_to_multi_action_space from f110_gym.envs.integrator import IntegratorType from f110_gym.envs.rendering import make_renderer @@ -15,64 +14,50 @@ from f110_gym.envs.track import Track from f110_gym.envs.utils import deep_update - # others import numpy as np -""" - OpenAI gym environment for F1TENTH - - Env should be initialized by calling gym.make('f110_gym:f110-v0', **kwargs) - - Args: - kwargs: - seed (int, default=12345): seed for random state and reproducibility - map (str, default='vegas'): name of the map used for the environment. - - params (dict, default={'mu': 1.0489, 'C_Sf':, 'C_Sr':, 'lf': 0.15875, 'lr': 0.17145, 'h': 0.074, 'm': 3.74, 'I': 0.04712, 's_min': -0.4189, 's_max': 0.4189, 'sv_min': -3.2, 'sv_max': 3.2, 'v_switch':7.319, 'a_max': 9.51, 'v_min':-5.0, 'v_max': 20.0, 'width': 0.31, 'length': 0.58}): dictionary of vehicle parameters. - mu: surface friction coefficient - C_Sf: Cornering stiffness coefficient, front - C_Sr: Cornering stiffness coefficient, rear - lf: Distance from center of gravity to front axle - lr: Distance from center of gravity to rear axle - h: Height of center of gravity - m: Total mass of the vehicle - I: Moment of inertial of the entire vehicle about the z axis - s_min: Minimum steering angle constraint - s_max: Maximum steering angle constraint - sv_min: Minimum steering velocity constraint - sv_max: Maximum steering velocity constraint - v_switch: Switching velocity (velocity at which the acceleration is no longer able to create wheel spin) - a_max: Maximum longitudinal acceleration - v_min: Minimum longitudinal velocity - v_max: Maximum longitudinal velocity - width: width of the vehicle in meters - length: length of the vehicle in meters - - num_agents (int, default=2): number of agents in the environment - - timestep (float, default=0.01): physics timestep - - ego_idx (int, default=0): ego's index in list of agents - """ + class F110Env(gym.Env): """ - OpenAI gym environment for F1TENTH + Gymnasium environment for F1TENTH + + Parameters + ---------- + config : dict, optional + parameter dict, by default None + render_mode : _type_, optional + rendering mode, by default None + + Examples + -------- + Using default params: + + >>> import gymnasium as gym + >>> env = gym.make("f1tenth_gym:f1tenth-v0") + >>> obs, info = env.reset() + >>> while not done: + >>> action = env.action_space.sample() + >>> obs, step_reward, done, truncated, info = env.step(action) + >>> env.close() + + Using custom params: + + >>> import gymnasium as gym + >>> env = gym.make("f1tenth_gym:f1tenth-v0") + >>> new_conf = {"params": {"mu": 1.5}} + >>> env.configure(config=new_conf) + + Using different maps: + + >>> import gymnasium as gym + >>> env = gym.make("f1tenth_gym:f1tenth-v0", config={"map": "Shanghai",}) """ # NOTE: change matadata with default rendering-modes, add definition of render_fps metadata = {"render_modes": ["human", "human_fast", "rgb_array"], "render_fps": 100} def __init__(self, config: dict = None, render_mode=None, **kwargs): - """_summary_ - - Parameters - ---------- - config : dict, optional - _description_, by default None - render_mode : _type_, optional - _description_, by default None - """ super().__init__() # Configuration @@ -175,7 +160,41 @@ def __init__(self, config: dict = None, render_mode=None, **kwargs): @classmethod def default_config(cls) -> dict: - """Default environment configuration. + """Default environment configuration: + + >>> { + >>> "seed": 12345, + >>> "map": "Spielberg", + >>> "params": { + >>> "mu": 1.0489, + >>> "C_Sf": 4.718, + >>> "C_Sr": 5.4562, + >>> "lf": 0.15875, + >>> "lr": 0.17145, + >>> "h": 0.074, + >>> "m": 3.74, + >>> "I": 0.04712, + >>> "s_min": -0.4189, + >>> "s_max": 0.4189, + >>> "sv_min": -3.2, + >>> "sv_max": 3.2, + >>> "v_switch": 7.319, + >>> "a_max": 9.51, + >>> "v_min": -5.0, + >>> "v_max": 20.0, + >>> "width": 0.31, + >>> "length": 0.58, + >>> }, + >>> "num_agents": 2, + >>> "timestep": 0.01, + >>> "ego_idx": 0, + >>> "integrator": "rk4", + >>> "model": "st", + >>> "control_input": ["speed", "steering_angle"], + >>> "observation_config": {"type": None}, + >>> "reset_config": {"type": None}, + >>> } + Can be overloaded in environment implementations, or by calling configure(). @@ -242,7 +261,9 @@ def configure(self, config: dict) -> None: if hasattr(self, "action_space"): # if some parameters changed, recompute action space - self.action_type = CarAction(self.config["control_input"], params=self.params) + self.action_type = CarAction( + self.config["control_input"], params=self.params + ) self.action_space = from_single_to_multi_action_space( self.action_type.space, self.num_agents ) @@ -300,19 +321,24 @@ def _update_state(self): self.collisions = self.sim.collisions def step(self, action): - """ - Step function for the gym env + """Step function for the gym env - Args: - action (np.ndarray(num_agents, 2)) + Parameters + ---------- + action : np.ndarray + control input for all agents - Returns: - obs (dict): observation of the current step - reward (float, default=self.timestep): step reward, currently is physics timestep - done (bool): if the simulation is done - info (dict): auxillary information dictionary + Returns + ------- + obs : dict + observation of the current step + reward : float + step reward, by default self.timestep + done : bool + if the simulation is done + info : dict + auxillary information dictionary """ - # call simulation step self.sim.step(action) @@ -347,18 +373,25 @@ def step(self, action): return obs, reward, done, truncated, info def reset(self, seed=None, options=None): - """ - Reset the gym environment by given poses + """Reset the gym environment by given poses - Args: - seed: random seed for the reset - options: dictionary of options for the reset containing initial poses of the agents + Parameters + ---------- + seed : int, optional + random seed for the reset, by default None + options : dict, optional + dictionary of options for the reset containing initial poses of the agents, by default None - Returns: - obs (dict): observation of the current step - reward (float, default=self.timestep): step reward, currently is physics timestep - done (bool): if the simulation is done - info (dict): auxillary information dictionary + Returns + ------- + obs : dict + observation of the current step + reward : float + step reward, by default self.timestep + done : bool + if the simulation is done + info : dict + auxillary information dictionary """ if seed is not None: np.random.seed(seed=self.seed) @@ -409,52 +442,46 @@ def reset(self, seed=None, options=None): return obs, info def update_map(self, map_name: str): - """ - Updates the map used by simulation - - Args: - map_name (str): name of the map + """Updates the map used by simulation - Returns: - None + Parameters + ---------- + map_name : str + name of the map """ self.sim.set_map(map_name) self.track = Track.from_track_name(map_name) def update_params(self, params, index=-1): - """ - Updates the parameters used by simulation for vehicles - - Args: - params (dict): dictionary of parameters - index (int, default=-1): if >= 0 then only update a specific agent's params + """Updates the parameters used by simulation for vehicles - Returns: - None + Parameters + ---------- + params : dict + dictionary of parameters + index : int, optional + if >= 0 then only update a specific agent's params, by default -1 """ self.sim.update_params(params, agent_idx=index) def add_render_callback(self, callback_func): - """ - Add extra drawing function to call during rendering. + """Add extra drawing function to call during rendering. - Args: - callback_func (function (EnvRenderer) -> None): custom function to called during render() + Parameters + ---------- + callback_func : Callable(EnvRenderer) -> None + custom function to called during render() """ - self.renderer.add_renderer_callback(callback_func) def render(self, mode="human"): - """ - Renders the environment with pyglet. Use mouse scroll in the window to zoom in/out, use mouse click drag to pan. Shows the agents, the map, current fps (bottom left corner), and the race information near as text. + """Renders the environment with pygame - Args: - mode (str, default='human'): rendering mode, currently supports: - 'human': slowed down rendering such that the env is rendered in a way that sim time elapsed is close to real time elapsed - 'human_fast': render as fast as possible + Parameters + ---------- + mode : str, optional + rendering mode, by default "human" - Returns: - None """ # NOTE: separate render (manage render-mode) from render_frame (actual rendering with pyglet) diff --git a/gym/f110_gym/envs/integrator.py b/gym/f110_gym/envs/integrator.py index bfc796be..a597a010 100644 --- a/gym/f110_gym/envs/integrator.py +++ b/gym/f110_gym/envs/integrator.py @@ -3,11 +3,35 @@ class IntegratorType(Enum): + """Integrator enum + + RK4: 1 + + Euler: 2 + """ + RK4 = 1 Euler = 2 @staticmethod def from_string(integrator: str): + """Set integrator by string + + Parameters + ---------- + integrator : str + integrator type + + Returns + ------- + RK4Integrator | EulerIntegrator + integrator object + + Raises + ------ + ValueError + Unknown integrator type + """ if integrator == "rk4": return RK4Integrator() elif integrator == "euler": @@ -17,24 +41,74 @@ def from_string(integrator: str): class Integrator: + """Integrator abstract class""" + def __init__(self) -> None: self._integrator_type = None @abstractmethod def integrate(self, f, x, u, dt, params): + """Integrate dynamics + + Parameters + ---------- + f : np.ndarray + RHS of ODE + x : np.ndarray + state + u : np.ndarray + control input + dt : float + sampling time + params : dict + parameter dictionary + + Raises + ------ + NotImplementedError + """ raise NotImplementedError("integrate method not implemented") @property def type(self) -> str: + """property, integrator type + + Returns + ------- + str + type + """ return self._integrator_type class RK4Integrator(Integrator): + """Runge Kutta fourth order integrator""" + def __init__(self) -> None: super().__init__() self._integrator_type = "rk4" def integrate(self, f, x, u, dt, params): + """Integrate dynamics + + Parameters + ---------- + f : np.ndarray + RHS of ODE + x : np.ndarray + state + u : np.ndarray + control input + dt : float + sampling time + params : dict + parameter dictionary + + Returns + ------- + np.ndarray: + integrated state + """ k1 = f( x, u, @@ -131,11 +205,33 @@ def integrate(self, f, x, u, dt, params): class EulerIntegrator(Integrator): + """Euler integrator""" + def __init__(self) -> None: super().__init__() self._integrator_type = "euler" def integrate(self, f, x, u, dt, params): + """Integrate dynamics + + Parameters + ---------- + f : np.ndarray + RHS of ODE + x : np.ndarray + state + u : np.ndarray + control input + dt : float + sampling time + params : dict + parameter dictionary + + Returns + ------- + np.ndarray: + integrated state + """ dstate = f( x, u, diff --git a/gym/f110_gym/envs/laser_models.py b/gym/f110_gym/envs/laser_models.py index 7cd0420b..92ddcebd 100644 --- a/gym/f110_gym/envs/laser_models.py +++ b/gym/f110_gym/envs/laser_models.py @@ -8,16 +8,20 @@ def get_dt(bitmap, resolution): - """ - Distance transformation, returns the distance matrix from the input bitmap. + """Distance transformation, returns the distance matrix from the input bitmap. Uses scipy.ndimage, cannot be JITted. - Args: - bitmap (numpy.ndarray, (n, m)): input binary bitmap of the environment, where 0 is obstacles, and 255 (or anything > 0) is freespace - resolution (float): resolution of the input bitmap (m/cell) - - Returns: - dt (numpy.ndarray, (n, m)): output distance matrix, where each cell has the corresponding distance (in meters) to the closest obstacle + Parameters + ---------- + bitmap : np.ndarray + input binary bitmap of the environment, where 0 is obstacles, and 255 (or anything > 0) is freespace + resolution : float + resolution of the input bitmap (m/cell) + + Returns + ------- + np.ndarray + output distance matrix, where each cell has the corresponding distance (in meters) to the closest obstacle """ dt = resolution * edt(bitmap) return dt @@ -25,18 +29,35 @@ def get_dt(bitmap, resolution): @njit(cache=True) def xy_2_rc(x, y, orig_x, orig_y, orig_c, orig_s, height, width, resolution): - """ - Translate (x, y) coordinate into (r, c) in the matrix - - Args: - x (float): coordinate in x (m) - y (float): coordinate in y (m) - orig_x (float): x coordinate of the map origin (m) - orig_y (float): y coordinate of the map origin (m) - - Returns: - r (int): row number in the transform matrix of the given point - c (int): column number in the transform matrix of the given point + """Translate (x, y) coordinate into (r, c) in the matrix + + Parameters + ---------- + x : float + coordinate in x (m) + y : float + coordinate in y (m) + orig_x : float + x coordinate of the map origin (m) + orig_y : float + y coordinate of the map origin (m) + orig_c : float + cosine of the map origin rotation + orig_s : float + sine of the map origin rotation + height : int + height of map (pixel) + width : int + width of map (pixel) + resolution : float + resolution of the input bitmap (m/cell) + + Returns + ------- + r : int + row index + c : int + column index """ # translation x_trans = x - orig_x @@ -66,17 +87,35 @@ def xy_2_rc(x, y, orig_x, orig_y, orig_c, orig_s, height, width, resolution): def distance_transform( x, y, orig_x, orig_y, orig_c, orig_s, height, width, resolution, dt ): - """ - Look up corresponding distance in the distance matrix - - Args: - x (float): x coordinate of the lookup point - y (float): y coordinate of the lookup point - orig_x (float): x coordinate of the map origin (m) - orig_y (float): y coordinate of the map origin (m) - - Returns: - distance (float): corresponding shortest distance to obstacle in meters + """Look up corresponding distance in the distance transform matrix + + Parameters + ---------- + x : _type_ + x coordinate of the lookup point + y : _type_ + y coordinate of the lookup point + orig_x : _type_ + x coordinate of the map origin (m) + orig_y : _type_ + y coordinate of the map origin (m) + orig_c : _type_ + cosine of the map origin rotation + orig_s : _type_ + sine of the map origin rotation + height : _type_ + map height + width : _type_ + map width + resolution : _type_ + resolution of the input bitmap (m/cell) + dt : _type_ + distance transform matrix + + Returns + ------- + float + distance to closest obstacle from the look up point """ r, c = xy_2_rc(x, y, orig_x, orig_y, orig_c, orig_s, height, width, resolution) distance = dt[r, c] @@ -101,21 +140,46 @@ def trace_ray( dt, max_range, ): + """Find the length of a specific ray at a specific scan angle theta. + + Parameters + ---------- + x : float + current x coordinate of the ego (scan) frame + y : float + current y coordinate of the ego (scan) frame + theta_index : int + current index of the scan beam in the scan range + sines : np.ndarray + pre-calculated sines of the angle array + cosines : np.ndarray + pre-calculated cosines of the angle array + eps : float + tolerance to stop ray tracing + orig_x : _type_ + x coordinate of the map origin (m) + orig_y : _type_ + y coordinate of the map origin (m) + orig_c : _type_ + cosine of the map origin rotation + orig_s : _type_ + sine of the map origin rotation + height : _type_ + map height + width : _type_ + map width + resolution : _type_ + resolution of the input bitmap (m/cell) + dt : _type_ + distance transform matrix + max_range : float + maximum range for laser scan rays + + Returns + ------- + float + ray distance """ - Find the length of a specific ray at a specific scan angle theta - Purely math calculation and loops, should be JITted. - - Args: - x (float): current x coordinate of the ego (scan) frame - y (float): current y coordinate of the ego (scan) frame - theta_index(int): current index of the scan beam in the scan range - sines (numpy.ndarray (n, )): pre-calculated sines of the angle array - cosines (numpy.ndarray (n, )): pre-calculated cosines ... - - Returns: - total_distance (float): the distance to first obstacle on the current scan beam - """ - # int casting, and index precal trigs theta_index_ = int(theta_index) s = sines[theta_index_] @@ -166,18 +230,49 @@ def get_scan( dt, max_range, ): - """ - Perform the scan for each discretized angle of each beam of the laser, loop heavy, should be JITted - - Args: - pose (numpy.ndarray(3, )): current pose of the scan frame in the map - theta_dis (int): number of steps to discretize the angles between 0 and 2pi for look up - fov (float): field of view of the laser scan - num_beams (int): number of beams in the scan - theta_index_increment (float): increment between angle indices after discretization - - Returns: - scan (numpy.ndarray(n, )): resulting laser scan at the pose, n=num_beams + """Perform the scan for each discretized angle of each beam of the laser + + Parameters + ---------- + pose : np.ndarray + current pose of the scan frame in the map + theta_dis : int + number of steps to discretize the angles between 0 and 2pi for look up + fov : float + field of view of the laser scan + num_beams : int + number of beams in the scan + theta_index_increment : float + increment between angle indices after discretization + sines : np.ndarray + pre-calculated sines of the angle array + cosines : np.ndarray + pre-calculated cosines of the angle array + eps : float + tolerance to stop ray tracing + orig_x : _type_ + x coordinate of the map origin (m) + orig_y : _type_ + y coordinate of the map origin (m) + orig_c : _type_ + cosine of the map origin rotation + orig_s : _type_ + sine of the map origin rotation + height : _type_ + map height + width : _type_ + map width + resolution : _type_ + resolution of the input bitmap (m/cell) + dt : _type_ + distance transform matrix + max_range : float + maximum range for laser scan rays + + Returns + ------- + np.ndarray + resulting laser scan at the pose """ # empty scan array init scan = np.empty((num_beams,)) @@ -223,20 +318,27 @@ def get_scan( @njit(cache=True, error_model="numpy") def check_ttc_jit(scan, vel, scan_angles, cosines, side_distances, ttc_thresh): - """ - Checks the iTTC of each beam in a scan for collision with environment - - Args: - scan (np.ndarray(num_beams, )): current scan to check - vel (float): current velocity - scan_angles (np.ndarray(num_beams, )): precomped angles of each beam - cosines (np.ndarray(num_beams, )): precomped cosines of the scan angles - side_distances (np.ndarray(num_beams, )): precomped distances at each beam from the laser to the sides of the car - ttc_thresh (float): threshold for iTTC for collision - - Returns: - in_collision (bool): whether vehicle is in collision with environment - collision_angle (float): at which angle the collision happened + """Checks the iTTC of each beam in a scan for collision with environment + + Parameters + ---------- + scan : np.ndarray + current scan to check + vel : float + current velocity + scan_angles : np.ndarray + precomped angles of each beam + cosines : np.ndarray + precomped cosines of the scan angles + side_distances : np.ndarray + precomped distances at each beam from the laser to the sides of the car + ttc_thresh : float + threshold for iTTC for collision + + Returns + ------- + bool + if scan indicates collision """ in_collision = False if vel != 0.0: @@ -255,28 +357,40 @@ def check_ttc_jit(scan, vel, scan_angles, cosines, side_distances, ttc_thresh): @njit(cache=True) def cross(v1, v2): - """ - Cross product of two 2-vectors - - Args: - v1, v2 (np.ndarray(2, )): input vectors - - Returns: - crossproduct (float): cross product + """Cross product of two 2-vectors + + Parameters + ---------- + v1 : np.ndarray + input vector 1 + v2 : np.ndarray + input vector 2 + + Returns + ------- + float + cross product """ return v1[0] * v2[1] - v1[1] * v2[0] @njit(cache=True) def are_collinear(pt_a, pt_b, pt_c): - """ - Checks if three points are collinear in 2D - - Args: - pt_a, pt_b, pt_c (np.ndarray(2, )): points to check in 2D - - Returns: - col (bool): whether three points are collinear + """Checks if three points are collinear in 2D + + Parameters + ---------- + pt_a : np.ndarray + point to check in 2D + pt_b : np.ndarray + point to check in 2D + pt_c : np.ndarray + point to check in 2D + + Returns + ------- + bool + whether three points are collinear """ tol = 1e-8 ba = pt_b - pt_a @@ -287,16 +401,23 @@ def are_collinear(pt_a, pt_b, pt_c): @njit(cache=True) def get_range(pose, beam_theta, va, vb): - """ - Get the distance at a beam angle to the vector formed by two of the four vertices of a vehicle - - Args: - pose (np.ndarray(3, )): pose of the scanning vehicle - beam_theta (float): angle of the current beam (world frame) - va, vb (np.ndarray(2, )): the two vertices forming an edge - - Returns: - distance (float): smallest distance at beam theta from scanning pose to edge + """Get the distance at a beam angle to the vector formed by two of the four vertices of a vehicle + + Parameters + ---------- + pose : np.ndarray + pose of the scanning vehicle + beam_theta : float + angle of the current beam (world frame) + va : np.ndarray + the two vertices forming an edge + vb : np.ndarray + the two vertices forming an edge + + Returns + ------- + float + smallest distance at beam theta from scanning pose to edge """ o = pose[0:2] v1 = o - va @@ -321,13 +442,23 @@ def get_range(pose, beam_theta, va, vb): @njit(cache=True) def get_blocked_view_indices(pose, vertices, scan_angles): - """ - Get the indices of the start and end of blocked fov in scans by another vehicle - - Args: - pose (np.ndarray(3, )): pose of the scanning vehicle - vertices (np.ndarray(4, 2)): four vertices of a vehicle pose - scan_angles (np.ndarray(num_beams, )): corresponding beam angles + """Get the indices of the start and end of blocked fov in scans by another vehicle + + Parameters + ---------- + pose : np.ndarray + pose of the scanning vehicle + vertices : np.ndarray + four vertices of a vehicle pose + scan_angles : np.ndarray + corresponding beam angles + + Returns + ------- + min_ind : int + smaller index of blocked portion of laser scan array + max_ind : int + larger index of blocked portion of laser scan array """ # find four vectors formed by pose and 4 vertices: vecs = vertices - pose[:2] @@ -359,17 +490,23 @@ def get_blocked_view_indices(pose, vertices, scan_angles): @njit(cache=True) def ray_cast(pose, scan, scan_angles, vertices): - """ - Modify a scan by ray casting onto another agent's four vertices - - Args: - pose (np.ndarray(3, )): pose of the vehicle performing scan - scan (np.ndarray(num_beams, )): original scan to modify - scan_angles (np.ndarray(num_beams, )): corresponding beam angles - vertices (np.ndarray(4, 2)): four vertices of a vehicle pose - - Returns: - new_scan (np.ndarray(num_beams, )): modified scan + """Modify a scan by ray casting onto another agent's four vertices + + Parameters + ---------- + pose : np.ndarray + pose of the vehicle performing scan + scan : np.ndarray + original scan to modify + scan_angles : np.ndarray + corresponding beam angles + vertices : np.ndarray + four vertices of a vehicle pose + + Returns + ------- + np.ndarray + modified scan """ # pad vertices so loops around looped_vertices = np.empty((5, 2)) @@ -394,15 +531,20 @@ def ray_cast(pose, scan, scan_angles, vertices): class ScanSimulator2D(object): - """ - 2D LIDAR scan simulator class - - Init params: - num_beams (int): number of beams in the scan - fov (float): field of view of the laser scan - eps (float, default=0.0001): ray tracing iteration termination condition - theta_dis (int, default=2000): number of steps to discretize the angles between 0 and 2pi for look up - max_range (float, default=30.0): maximum range of the laser + """2D LIDAR scan simulator class + + Parameters + ---------- + num_beams : int + number of beams in the scan + fov : float + field of view of the laser scan + eps : float, optional + ray tracing iteration termination condition, by default 0.0001 + theta_dis : int, optional + number of steps to discretize the angles between 0 and 2pi for look up, by default 2000 + max_range : float, optional + maximum range of the laser, by default 30.0 """ def __init__(self, num_beams, fov, eps=0.0001, theta_dis=2000, max_range=30.0): @@ -432,14 +574,17 @@ def __init__(self, num_beams, fov, eps=0.0001, theta_dis=2000, max_range=30.0): self.cosines = np.cos(theta_arr) def set_map(self, map: str | Track): - """ - Set the bitmap of the scan simulator by path + """Set the bitmap of the scan simulator by path - Args: - map (str | Track): path to the map file, or Track object + Parameters + ---------- + map : str | Track + path to the map file, or Track object - Returns: - flag (bool): if image reading and loading is successful + Returns + ------- + bool + if image reading and loading is successful """ if isinstance(map, str): self.track = Track.from_track_name(map) @@ -466,21 +611,27 @@ def set_map(self, map: str | Track): return True def scan(self, pose, rng, std_dev=0.01): + """Perform simulated 2D scan by pose on the given map + + Parameters + ---------- + pose : np.ndarray + pose of the scan frame (x, y, theta) + rng : np.random.Generator + random number generator to use for whitenoise in scan + std_dev : float, optional + standard deviation of the generated whitenoise in the scan, by default 0.01 + + Returns + ------- + np.ndarray + data array of the laserscan + + Raises + ------ + ValueError + Map is not set for scan simulator. """ - Perform simulated 2D scan by pose on the given map - - Args: - pose (numpy.ndarray (3, )): pose of the scan frame (x, y, theta) - rng (numpy.random.Generator): random number generator to use for whitenoise in scan, or None - std_dev (float, default=0.01): standard deviation of the generated whitenoise in the scan - - Returns: - scan (numpy.ndarray (n, )): data array of the laserscan, n=num_beams - - Raises: - ValueError: when scan is called before a map is set - """ - if self.map_height is None: raise ValueError("Map is not set for scan simulator.") @@ -511,12 +662,20 @@ def scan(self, pose, rng, std_dev=0.01): return scan def get_increment(self): + """Return angle increment between scans + + Returns + ------- + float + angle increment + """ return self.angle_increment """ Unit test for the 2D scan simulator class Author: Hongrui Zheng +TODO: do we still need these? Test cases: 1, 2: Comparison between generated scan array of the new simulator and the legacy C++ simulator, generated data used, MSE is used as the metric diff --git a/gym/f110_gym/envs/observation.py b/gym/f110_gym/envs/observation.py index 263f8260..ae3a706a 100644 --- a/gym/f110_gym/envs/observation.py +++ b/gym/f110_gym/envs/observation.py @@ -7,31 +7,62 @@ class Observation: - """ - Abstract class for observations. Each observation must implement the space and observe methods. + """Observation abstract class - :param env: The environment. - :param vehicle_id: The id of the observer vehicle. - :param kwargs: Additional arguments. + Parameters + ---------- + env : gym.Env + Gymnasium environment """ - def __init__(self, env): self.env = env @abstractmethod def space(self): + """Observation space + + Raises + ------ + NotImplementedError + """ raise NotImplementedError() @abstractmethod def observe(self): + """Observe + + Raises + ------ + NotImplementedError + """ raise NotImplementedError() class OriginalObservation(Observation): + """Original Observation class + + Parameters + ---------- + env : gym.Env + Gymnasium environment + """ def __init__(self, env): super().__init__(env) def space(self): + """Return observation space + + Original Observations is a dict with keys: + + >>> [ego_idx, scans, poses_x, poses_y, poses_theta, linear_vels_x, linear_vels_y, ang_vels_z, collisions, lap_times, lap_counts] + + Each value is a list of observations corresponding to the agents. + + Returns + ------- + gym.spaces.Space + observation space + """ num_agents = self.env.num_agents scan_size = self.env.sim.agents[0].scan_simulator.num_beams scan_range = ( @@ -99,6 +130,13 @@ def space(self): return obs_space def observe(self): + """Observe function + + Returns + ------- + dict + Current observation + """ # state indices xi, yi, deltai, vxi, yawi, yaw_ratei, slipi = range( 7 @@ -152,11 +190,31 @@ def observe(self): class FeaturesObservation(Observation): + """Feature Observation class + + Parameters + ---------- + env : gym.Env + Gymnasium environment + """ def __init__(self, env, features: List[str]): super().__init__(env) self.features = features def space(self): + """Return observation space + + Feature Observations is a dict with agent_ids as keys + + And each value is a dict with keys: + + >>> [scan, pose_x, pose_y, pose_theta, linear_vel_x, linear_vel_y, ang_vel_z, delta, beta, collision, lap_time, lap_count] + + Returns + ------- + gym.spaces.Space + observation space + """ scan_size = self.env.sim.agents[0].scan_simulator.num_beams scan_range = self.env.sim.agents[0].scan_simulator.max_range large_num = 1e30 # large number to avoid unbounded obs space (ie., low=-inf or high=inf) @@ -209,6 +267,13 @@ def space(self): return obs_space def observe(self): + """Observe function + + Returns + ------- + dict + Current observation + """ # state indices xi, yi, deltai, vxi, yawi, yaw_ratei, slipi = range( 7 @@ -264,6 +329,25 @@ def observe(self): def observation_factory(env, type: str | None, **kwargs) -> Observation: + """Decides which observation type and keys to include based on type string + + Parameters + ---------- + env : gym.Env + Gymnasium environment + type : str | None + Observation type + + Returns + ------- + Observation + Observation type + + Raises + ------ + ValueError + Invalid observation type + """ type = type or "original" if type == "original": diff --git a/gym/f110_gym/envs/utils.py b/gym/f110_gym/envs/utils.py index 4bed0ca9..cd82e3aa 100644 --- a/gym/f110_gym/envs/utils.py +++ b/gym/f110_gym/envs/utils.py @@ -6,9 +6,17 @@ def deep_update( mapping: Dict[KeyType, Any], *updating_mappings: Dict[KeyType, Any] ) -> Dict[KeyType, Any]: - """ - Dictionary deep update for nested dictionaries from pydantic: - https://github.com/pydantic/pydantic/blob/fd2991fe6a73819b48c906e3c3274e8e47d0f761/pydantic/utils.py#L200 + """Recursive update of dict from pydantic: https://github.com/pydantic/pydantic/blob/fd2991fe6a73819b48c906e3c3274e8e47d0f761/pydantic/utils.py#L200 + + Parameters + ---------- + mapping : Dict[KeyType, Any] + mapping of original dict + + Returns + ------- + Dict[KeyType, Any] + updated dict """ updated_mapping = mapping.copy() for updating_mapping in updating_mappings: