Skip to content

Commit

Permalink
Updated observations for futher compatibility.
Browse files Browse the repository at this point in the history
  • Loading branch information
nandantumu committed Sep 27, 2024
1 parent 00e268d commit c782fb4
Show file tree
Hide file tree
Showing 8 changed files with 149 additions and 75 deletions.
13 changes: 12 additions & 1 deletion f1tenth_gym/envs/base_classes.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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(
Expand Down Expand Up @@ -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):
"""
Expand Down
23 changes: 19 additions & 4 deletions f1tenth_gym/envs/dynamic_models/__init__.py
Original file line number Diff line number Diff line change
Expand Up @@ -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


Expand All @@ -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.")
Expand Down Expand Up @@ -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}")
16 changes: 16 additions & 0 deletions f1tenth_gym/envs/dynamic_models/kinematic.py
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
import numpy as np
from numba import njit
from numba.typed import Dict

from .utils import steering_constraint, accl_constraints

Expand Down Expand Up @@ -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
2 changes: 1 addition & 1 deletion f1tenth_gym/envs/dynamic_models/multi_body/__init__.py
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down
16 changes: 16 additions & 0 deletions f1tenth_gym/envs/dynamic_models/multi_body/multi_body.py
Original file line number Diff line number Diff line change
@@ -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

Expand Down Expand Up @@ -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
16 changes: 16 additions & 0 deletions f1tenth_gym/envs/dynamic_models/single_track.py
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
import numpy as np
from numba import njit
from numba.typed import Dict

from .utils import steering_constraint, accl_constraints

Expand Down Expand Up @@ -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
36 changes: 15 additions & 21 deletions f1tenth_gym/envs/observation.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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):
Expand All @@ -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 = {
Expand Down
102 changes: 54 additions & 48 deletions tests/test_dynamics.py
Original file line number Diff line number Diff line change
Expand Up @@ -193,65 +193,71 @@ 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()
for i in range(10000):
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
Expand Down

0 comments on commit c782fb4

Please sign in to comment.