From 9e498307bd3f1e389afb3cc3edb8e49677c7b15d Mon Sep 17 00:00:00 2001 From: Billy Zheng Date: Mon, 16 Sep 2024 10:21:27 -0400 Subject: [PATCH 1/2] update frenet conversion --- f1tenth_gym/envs/track/cubic_spline.py | 2 +- f1tenth_gym/envs/track/track.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/f1tenth_gym/envs/track/cubic_spline.py b/f1tenth_gym/envs/track/cubic_spline.py index a8af32da..ebb31991 100644 --- a/f1tenth_gym/envs/track/cubic_spline.py +++ b/f1tenth_gym/envs/track/cubic_spline.py @@ -182,7 +182,7 @@ def calc_arclength_inaccurate(self, x: float, y: float) -> tuple[float, float]: + t * (self.s[min_dist_segment + 1] - self.s[min_dist_segment]) ) - return s, 0.0 + return s, ey def _calc_tangent(self, s: float) -> np.ndarray: """ diff --git a/f1tenth_gym/envs/track/track.py b/f1tenth_gym/envs/track/track.py index c6a0d973..9dca442a 100644 --- a/f1tenth_gym/envs/track/track.py +++ b/f1tenth_gym/envs/track/track.py @@ -276,7 +276,7 @@ def cartesian_to_frenet(self, x, y, phi, s_guess=0): ey: lateral deviation ephi: heading deviation """ - s, ey = self.centerline.spline.calc_arclength(x, y, s_guess) + s, ey = self.centerline.spline.calc_arclength_inaccurate(x, y) if s > self.centerline.spline.s[-1]: # Wrap around s = s - self.centerline.spline.s[-1] From 11ca8de3699245e396de83905e38931ef2f88649 Mon Sep 17 00:00:00 2001 From: Billy Zheng Date: Mon, 16 Sep 2024 10:24:00 -0400 Subject: [PATCH 2/2] fix vy in obs, add frenet observation type. --- f1tenth_gym/envs/observation.py | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/f1tenth_gym/envs/observation.py b/f1tenth_gym/envs/observation.py index 263f8260..c4031706 100644 --- a/f1tenth_gym/envs/observation.py +++ b/f1tenth_gym/envs/observation.py @@ -223,11 +223,13 @@ def observe(self): lap_count = self.env.lap_counts[i] x, y, theta = agent.state[xi], agent.state[yi], agent.state[yawi] - vx, vy = agent.state[vxi], 0.0 + 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 @@ -284,5 +286,17 @@ def observation_factory(env, type: str | None, **kwargs) -> Observation: "beta", ] return FeaturesObservation(env, features=features) + elif type == "frenet_dynamic_state": + features = [ + "pose_x", + "pose_y", + "delta", + "linear_vel_x", + "linear_vel_y", + "pose_theta", + "ang_vel_z", + "beta", + ] + return FeaturesObservation(env, features=features) else: raise ValueError(f"Invalid observation type {type}.")