From c782fb41d7ef8b18f42c3dd71e07cd8e976a5b75 Mon Sep 17 00:00:00 2001 From: nandant Date: Sat, 28 Sep 2024 01:13:58 +0200 Subject: [PATCH] Updated observations for futher compatibility. --- f1tenth_gym/envs/base_classes.py | 13 ++- f1tenth_gym/envs/dynamic_models/__init__.py | 23 +++- f1tenth_gym/envs/dynamic_models/kinematic.py | 16 +++ .../dynamic_models/multi_body/__init__.py | 2 +- .../dynamic_models/multi_body/multi_body.py | 16 +++ .../envs/dynamic_models/single_track.py | 16 +++ f1tenth_gym/envs/observation.py | 36 +++---- tests/test_dynamics.py | 102 +++++++++--------- 8 files changed, 149 insertions(+), 75 deletions(-) diff --git a/f1tenth_gym/envs/base_classes.py b/f1tenth_gym/envs/base_classes.py index 0dea4e6e..4bfe5c82 100644 --- a/f1tenth_gym/envs/base_classes.py +++ b/f1tenth_gym/envs/base_classes.py @@ -102,6 +102,7 @@ def __init__( self.integrator = integrator self.action_type = action_type self.model = model + self.standard_state_fn = self.model.get_standardized_state_fn() # state of the vehicle self.state = self.model.get_initial_state(params=self.params) @@ -312,7 +313,7 @@ def update_pose(self, raw_steer, vel): ) # bound yaw angle - self.state[4] %= 2 * np.pi + self.state[4] %= 2 * np.pi # TODO: This is a problem waiting to happen # update scan current_scan = RaceCar.scan_simulator.scan( @@ -356,6 +357,16 @@ def update_scan(self, agent_scans, agent_index): agent_scans[agent_index] = new_scan + @property + def standard_state(self) -> dict: + """ + Returns the state of the vehicle as an observation + + Returns: + np.ndarray (7, ): state of the vehicle + """ + return self.standard_state_fn(self.state) + class Simulator(object): """ diff --git a/f1tenth_gym/envs/dynamic_models/__init__.py b/f1tenth_gym/envs/dynamic_models/__init__.py index 8dec7f4d..9837f013 100644 --- a/f1tenth_gym/envs/dynamic_models/__init__.py +++ b/f1tenth_gym/envs/dynamic_models/__init__.py @@ -7,9 +7,9 @@ from enum import Enum import numpy as np -from .kinematic import vehicle_dynamics_ks -from .single_track import vehicle_dynamics_st -from .multi_body import init_mb, vehicle_dynamics_mb +from .kinematic import vehicle_dynamics_ks, get_standardized_state_ks +from .single_track import vehicle_dynamics_st, get_standardized_state_st +from .multi_body import init_mb, vehicle_dynamics_mb, get_standardized_state_mb from .utils import pid_steer, pid_accl @@ -32,7 +32,7 @@ def from_string(model: str): else: raise ValueError(f"Unknown model type {model}") - def get_initial_state(self, pose=None, params: dict|None = None): + def get_initial_state(self, pose=None, params: dict | None = None): # Assert that if self is MB, params is not None if self == DynamicModel.MB and params is None: raise ValueError("MultiBody model requires parameters to be provided.") @@ -69,3 +69,18 @@ def f_dynamics(self): return vehicle_dynamics_mb else: raise ValueError(f"Unknown model type {self}") + + def get_standardized_state_fn(self): + """ + This function returns the standardized state information for the model. + This needs to be a function, because the state information is different for each model. + Slip is not directly available from the MB model. + """ + if self == DynamicModel.KS: + return get_standardized_state_ks + elif self == DynamicModel.ST: + return get_standardized_state_st + elif self == DynamicModel.MB: + return get_standardized_state_mb + else: + raise ValueError(f"Unknown model type {self}") diff --git a/f1tenth_gym/envs/dynamic_models/kinematic.py b/f1tenth_gym/envs/dynamic_models/kinematic.py index 95804a95..db4df61f 100644 --- a/f1tenth_gym/envs/dynamic_models/kinematic.py +++ b/f1tenth_gym/envs/dynamic_models/kinematic.py @@ -1,5 +1,6 @@ import numpy as np from numba import njit +from numba.typed import Dict from .utils import steering_constraint, accl_constraints @@ -171,3 +172,18 @@ def vehicle_dynamics_ks_cog(x: np.ndarray, u_init: np.ndarray, params: dict): ] return f + + +@njit(cache=True) +def get_standardized_state_ks(x: np.ndarray) -> dict: + """[X,Y,DELTA,V_X, V_Y,YAW,YAW_RATE,SLIP]""" + d = dict() + d["x"] = x[0] + d["y"] = x[1] + d["delta"] = x[2] + d["v_x"] = x[3] + d["v_y"] = 0.0 + d["yaw"] = x[4] + d["yaw_rate"] = x[5] + d["slip"] = 0.0 + return d diff --git a/f1tenth_gym/envs/dynamic_models/multi_body/__init__.py b/f1tenth_gym/envs/dynamic_models/multi_body/__init__.py index c6ef9afa..40d6db10 100644 --- a/f1tenth_gym/envs/dynamic_models/multi_body/__init__.py +++ b/f1tenth_gym/envs/dynamic_models/multi_body/__init__.py @@ -5,7 +5,7 @@ import numpy as np from numba import njit -from .multi_body import vehicle_dynamics_mb +from .multi_body import vehicle_dynamics_mb, get_standardized_state_mb def init_mb(init_state, params: dict) -> np.ndarray: diff --git a/f1tenth_gym/envs/dynamic_models/multi_body/multi_body.py b/f1tenth_gym/envs/dynamic_models/multi_body/multi_body.py index ea8bb377..dbe83fb9 100644 --- a/f1tenth_gym/envs/dynamic_models/multi_body/multi_body.py +++ b/f1tenth_gym/envs/dynamic_models/multi_body/multi_body.py @@ -1,5 +1,6 @@ import numpy as np from numba import njit +from numba.typed import Dict from f1tenth_gym.envs.dynamic_models.utils import steering_constraint, accl_constraints @@ -676,3 +677,18 @@ def vehicle_dynamics_mb(x: np.ndarray, u_init: np.ndarray, params: dict): f[28] = dot_delta_y_r return f + + +@njit(cache=True) +def get_standardized_state_mb(x: np.ndarray) -> dict: + """[X,Y,DELTA,V_X, V_Y,YAW,YAW_RATE,SLIP]""" + d = dict() + d["x"] = x[0] + d["y"] = x[1] + d["delta"] = x[2] + d["v_x"] = x[3] + d["v_y"] = x[10] + d["yaw"] = x[4] + d["yaw_rate"] = x[5] + d["slip"] = np.arctan2(x[10], x[3]) + return d diff --git a/f1tenth_gym/envs/dynamic_models/single_track.py b/f1tenth_gym/envs/dynamic_models/single_track.py index 8ce62d0e..522a57be 100644 --- a/f1tenth_gym/envs/dynamic_models/single_track.py +++ b/f1tenth_gym/envs/dynamic_models/single_track.py @@ -1,5 +1,6 @@ import numpy as np from numba import njit +from numba.typed import Dict from .utils import steering_constraint, accl_constraints @@ -150,3 +151,18 @@ def vehicle_dynamics_st(x: np.ndarray, u_init: np.ndarray, params: dict): ) return f + + +@njit(cache=True) +def get_standardized_state_st(x: np.ndarray) -> dict: + """[X,Y,DELTA,V_X, V_Y,YAW,YAW_RATE,SLIP]""" + d = dict() + d["x"] = x[0] + d["y"] = x[1] + d["delta"] = x[2] + d["v_x"] = x[3] * np.cos(x[6]) + d["v_y"] = x[3] * np.sin(x[6]) + d["yaw"] = x[4] + d["yaw_rate"] = x[5] + d["slip"] = x[6] + return d diff --git a/f1tenth_gym/envs/observation.py b/f1tenth_gym/envs/observation.py index c4031706..3b3c4929 100644 --- a/f1tenth_gym/envs/observation.py +++ b/f1tenth_gym/envs/observation.py @@ -124,11 +124,13 @@ def observe(self): lap_count = self.env.lap_counts[i] collision = self.env.sim.collisions[i] - x, y, theta = agent.state[xi], agent.state[yi], agent.state[yawi] - vx, vy = agent.state[vxi], 0.0 - angvel = ( - 0.0 if len(agent.state) < 7 else agent.state[yaw_ratei] - ) # set 0.0 when KST Model + std_state = agent.standard_state + + x, y, theta = std_state["x"], std_state["y"], std_state["yaw"] + + vx = std_state["v_x"] + vy = std_state["v_y"] + angvel = std_state["yaw_rate"] observations["scans"].append(agent_scan) observations["poses_x"].append(x) @@ -209,11 +211,6 @@ def space(self): return obs_space def observe(self): - # state indices - xi, yi, deltai, vxi, yawi, yaw_ratei, slipi = range( - 7 - ) # 7 largest state size (ST Model) - obs = {} # dictionary agent_id -> observation dict for i, agent_id in enumerate(self.env.agent_ids): @@ -222,17 +219,14 @@ def observe(self): lap_time = self.env.lap_times[i] lap_count = self.env.lap_counts[i] - x, y, theta = agent.state[xi], agent.state[yi], agent.state[yawi] - vlong = agent.state[vxi] - delta = agent.state[deltai] - beta = ( - 0.0 if len(agent.state) < 7 else agent.state[slipi] - ) # set 0.0 when KST Model - vx = vlong * np.cos(beta) - vy = vlong * np.sin(beta) - angvel = ( - 0.0 if len(agent.state) < 7 else agent.state[yaw_ratei] - ) # set 0.0 when KST Model + std_state = agent.standard_state + + x, y, theta = std_state["x"], std_state["y"], std_state["yaw"] + delta = std_state["delta"] + beta = std_state["slip"] + vx = std_state["v_x"] + vy = std_state["v_y"] + angvel = std_state["yaw_rate"] # create agent's observation dict agent_obs = { diff --git a/tests/test_dynamics.py b/tests/test_dynamics.py index ca2d6593..d0d6433e 100644 --- a/tests/test_dynamics.py +++ b/tests/test_dynamics.py @@ -193,42 +193,46 @@ def test_derivatives(self): f_ks = vehicle_dynamics_ks( x_ks, u, - self.mu, - self.C_Sf, - self.C_Sr, - self.lf, - self.lr, - self.h, - self.m, - self.I, - self.s_min, - self.s_max, - self.sv_min, - self.sv_max, - self.v_switch, - self.a_max, - self.v_min, - self.v_max, + params={ + "mu": self.mu, + "C_Sf": self.C_Sf, + "C_Sr": self.C_Sr, + "lf": self.lf, + "lr": self.lr, + "h": self.h, + "m": self.m, + "I": self.I, + "s_min": self.s_min, + "s_max": self.s_max, + "sv_min": self.sv_min, + "sv_max": self.sv_max, + "v_switch": self.v_switch, + "a_max": self.a_max, + "v_min": self.v_min, + "v_max": self.v_max, + }, ) f_st = vehicle_dynamics_st( x_st, u, - self.mu, - self.C_Sf, - self.C_Sr, - self.lf, - self.lr, - self.h, - self.m, - self.I, - self.s_min, - self.s_max, - self.sv_min, - self.sv_max, - self.v_switch, - self.a_max, - self.v_min, - self.v_max, + params={ + "mu": self.mu, + "C_Sf": self.C_Sf, + "C_Sr": self.C_Sr, + "lf": self.lf, + "lr": self.lr, + "h": self.h, + "m": self.m, + "I": self.I, + "s_min": self.s_min, + "s_max": self.s_max, + "sv_min": self.sv_min, + "sv_max": self.sv_max, + "v_switch": self.v_switch, + "a_max": self.a_max, + "v_min": self.v_min, + "v_max": self.v_max, + }, ) start = time.time() @@ -236,22 +240,24 @@ def test_derivatives(self): f_st = vehicle_dynamics_st( x_st, u, - self.mu, - self.C_Sf, - self.C_Sr, - self.lf, - self.lr, - self.h, - self.m, - self.I, - self.s_min, - self.s_max, - self.sv_min, - self.sv_max, - self.v_switch, - self.a_max, - self.v_min, - self.v_max, + params={ + "mu": self.mu, + "C_Sf": self.C_Sf, + "C_Sr": self.C_Sr, + "lf": self.lf, + "lr": self.lr, + "h": self.h, + "m": self.m, + "I": self.I, + "s_min": self.s_min, + "s_max": self.s_max, + "sv_min": self.sv_min, + "sv_max": self.sv_max, + "v_switch": self.v_switch, + "a_max": self.a_max, + "v_min": self.v_min, + "v_max": self.v_max, + }, ) duration = time.time() - start avg_fps = 10000 / duration