Merge branch 'v1.0.0' into dev_dynamics_updates

# Conflicts:
#	gym/f110_gym/envs/base_classes.py
#	gym/f110_gym/envs/dynamic_models.py

examples/random_trackgen.py

@@ -162,7 +162,7 @@ def create_track():
     assert i1 != -1
     assert i2 != -1
 
-    track = track[i1: i2 - 1]
+    track = track[i1 : i2 - 1]
     first_beta = track[0][1]
     first_perp_x = math.cos(first_beta)
     first_perp_y = math.sin(first_beta)

examples/video_recording.py

@@ -0,0 +1,78 @@
+import time
+import gymnasium as gym
+import gymnasium.wrappers
+import numpy as np
+
+from waypoint_follow import PurePursuitPlanner
+
+
+def main():
+    work = {
+        "mass": 3.463388126201571,
+        "lf": 0.15597534362552312,
+        "tlad": 0.82461887897713965,
+        "vgain": 1,
+    }
+
+    env = gym.make(
+        "f110_gym:f110-v0",
+        config={
+            "map": "Spielberg",
+            "num_agents": 1,
+            "timestep": 0.01,
+            "integrator": "rk4",
+            "control_input": "speed",
+            "model": "st",
+            "observation_config": {"type": "kinematic_state"},
+            "params": {"mu": 1.0},
+        },
+        render_mode="rgb_array",
+    )
+    env = gymnasium.wrappers.RecordVideo(env, f"video_{time.time()}")
+    track = env.unwrapped.track
+
+    planner = PurePursuitPlanner(track=track, wb=0.17145 + 0.15875)
+
+    poses = np.array(
+        [
+            [
+                track.raceline.xs[0],
+                track.raceline.ys[0],
+                track.raceline.yaws[0],
+            ]
+        ]
+    )
+
+    obs, info = env.reset(options={"poses": poses})
+    done = False
+
+    laptime = 0.0
+    start = time.time()
+
+    frames = [env.render()]
+    while not done and laptime < 15.0:
+        action = env.action_space.sample()
+        for i, agent_id in enumerate(obs.keys()):
+            speed, steer = planner.plan(
+                obs[agent_id]["pose_x"],
+                obs[agent_id]["pose_y"],
+                obs[agent_id]["pose_theta"],
+                work["tlad"],
+                work["vgain"],
+            )
+            action[i] = [steer, speed]
+
+        obs, step_reward, done, truncated, info = env.step(action)
+        laptime += step_reward
+
+        frame = env.render()
+        frames.append(frame)
+
+    print("Sim elapsed time:", laptime, "Real elapsed time:", time.time() - start)
+
+    # close env to trigger video saving
+    env.close()
+
+
+if __name__ == "__main__":
+    main()

examples/waypoint_follow.py

@@ -1,9 +1,10 @@
 import time
+from typing import Tuple
 
 import gymnasium as gym
 import numpy as np
 from numba import njit
-from pyglet.gl import GL_POINTS
+
 
 """
 Planner Helpers
@@ -183,6 +184,8 @@ def __init__(self, track, wb):
         self.max_reacquire = 20.0
 
         self.drawn_waypoints = []
+        self.lookahead_point = None
+        self.current_index = None
 
     def load_waypoints(self, conf):
         """
@@ -195,32 +198,42 @@ def load_waypoints(self, conf):
 
     def render_waypoints(self, e):
         """
-        update waypoints being drawn by EnvRenderer
+        Callback to render waypoints.
         """
         points = self.waypoints[:, :2]
+        e.render_closed_lines(points, color=(128, 0, 0), size=1)
+
+    def render_lookahead_point(self, e):
+        """
+        Callback to render the lookahead point.
+        """
+        if self.lookahead_point is not None:
+            points = self.lookahead_point[:2][None]  # shape (1, 2)
+            e.render_points(points, color=(0, 0, 128), size=2)
+
+    def render_local_plan(self, e):
+        """
+        update waypoints being drawn by EnvRenderer
+        """
+        if self.current_index is not None:
+            points = self.waypoints[self.current_index : self.current_index + 10, :2]
+            e.render_lines(points, color=(0, 128, 0), size=1)
 
-        scaled_points = 50.0 * points
-
-        for i in range(points.shape[0]):
-            if len(self.drawn_waypoints) < points.shape[0]:
-                b = e.batch.add(
-                    1,
-                    GL_POINTS,
-                    None,
-                    ("v3f/stream", [scaled_points[i, 0], scaled_points[i, 1], 0.0]),
-                    ("c3B/stream", [183, 193, 222]),
-                )
-                self.drawn_waypoints.append(b)
-            else:
-                self.drawn_waypoints[i].vertices = [
-                    scaled_points[i, 0],
-                    scaled_points[i, 1],
-                    0.0,
-                ]
-
-    def _get_current_waypoint(self, waypoints, lookahead_distance, position, theta):
+    def _get_current_waypoint(
+        self, waypoints, lookahead_distance, position, theta
+    ) -> Tuple[np.ndarray, int]:
         """
-        gets the current waypoint to follow
+        Returns the current waypoint to follow given the current pose.
+
+        Args:
+            waypoints: The waypoints to follow (Nx3 array)
+            lookahead_distance: The lookahead distance [m]
+            position: The current position (2D array)
+            theta: The current heading [rad]
+
+        Returns:
+            waypoint: The current waypoint to follow (x, y, speed)
+            i: The index of the current waypoint
         """
         wpts = waypoints[:, :2]
         lookahead_distance = np.float32(lookahead_distance)
@@ -231,61 +244,48 @@ def _get_current_waypoint(self, waypoints, lookahead_distance, position, theta):
                 position, lookahead_distance, wpts, t1, wrap=True
             )
             if i2 is None:
-                return None
+                return None, None
             current_waypoint = np.empty((3,), dtype=np.float32)
             # x, y
             current_waypoint[0:2] = wpts[i2, :]
             # speed
             current_waypoint[2] = waypoints[i, -1]
-            return current_waypoint
+            return current_waypoint, i
         elif nearest_dist < self.max_reacquire:
             # NOTE: specify type or numba complains
-            return wpts[i, :]
+            return wpts[i, :], i
         else:
-            return None
+            return None, None
 
     def plan(self, pose_x, pose_y, pose_theta, lookahead_distance, vgain):
         """
         gives actuation given observation
         """
         position = np.array([pose_x, pose_y])
-        lookahead_point = self._get_current_waypoint(
+        lookahead_point, i = self._get_current_waypoint(
             self.waypoints, lookahead_distance, position, pose_theta
         )
 
         if lookahead_point is None:
             return 4.0, 0.0
 
+        # for rendering
+        self.lookahead_point = lookahead_point
+        self.current_index = i
+
+        # actuation
         speed, steering_angle = get_actuation(
-            pose_theta, lookahead_point, position, lookahead_distance, self.wheelbase
+            pose_theta,
+            self.lookahead_point,
+            position,
+            lookahead_distance,
+            self.wheelbase,
         )
         speed = vgain * speed
 
         return speed, steering_angle
 
 
-class FlippyPlanner:
-    """
-    Planner designed to exploit integration methods and dynamics.
-    For testing only. To observe this error, use single track dynamics for all velocities >0.1
-    """
-
-    def __init__(self, speed=1, flip_every=1, steer=2):
-        self.speed = speed
-        self.flip_every = flip_every
-        self.counter = 0
-        self.steer = steer
-
-    def render_waypoints(self, *args, **kwargs):
-        pass
-
-    def plan(self, *args, **kwargs):
-        if self.counter % self.flip_every == 0:
-            self.counter = 0
-            self.steer *= -1
-        return self.speed, self.steer
-
-
 def main():
     """
     main entry point
@@ -296,7 +296,7 @@ def main():
         "lf": 0.15597534362552312,
         "tlad": 0.82461887897713965,
         "vgain": 1,
-    }  # 0.90338203837889}
+    }
 
     env = gym.make(
         "f110_gym:f110-v0",
@@ -312,40 +312,23 @@ def main():
         },
         render_mode="human",
     )
+    track = env.unwrapped.track
 
-    planner = PurePursuitPlanner(
-        track=env.track, wb=0.17145 + 0.15875
-    )  # FlippyPlanner(speed=0.2, flip_every=1, steer=10)
-
-    def render_callback(env_renderer):
-        # custom extra drawing function
-
-        e = env_renderer
-
-        # update camera to follow car
-        x = e.cars[0].vertices[::2]
-        y = e.cars[0].vertices[1::2]
-        top, bottom, left, right = max(y), min(y), min(x), max(x)
-        e.score_label.x = left
-        e.score_label.y = top - 700
-        e.left = left - 800
-        e.right = right + 800
-        e.top = top + 800
-        e.bottom = bottom - 800
-
-        planner.render_waypoints(env_renderer)
-
-    env.add_render_callback(render_callback)
+    planner = PurePursuitPlanner(track=track, wb=0.17145 + 0.15875)
 
     poses = np.array(
         [
             [
-                env.track.raceline.xs[0],
-                env.track.raceline.ys[0],
-                env.track.raceline.yaws[0],
+                track.raceline.xs[0],
+                track.raceline.ys[0],
+                track.raceline.yaws[0],
             ]
         ]
     )
+    env.unwrapped.add_render_callback(planner.render_waypoints)
+    env.unwrapped.add_render_callback(planner.render_local_plan)
+    env.unwrapped.add_render_callback(planner.render_lookahead_point)
+
     obs, info = env.reset(options={"poses": poses})
     done = False
     env.render()
@@ -354,17 +337,20 @@ def render_callback(env_renderer):
     start = time.time()
 
     while not done:
-        agent_id = env.agent_ids[0]
-        speed, steer = planner.plan(
-            obs[agent_id]["pose_x"],
-            obs[agent_id]["pose_y"],
-            obs[agent_id]["pose_theta"],
-            work["tlad"],
-            work["vgain"],
-        )
-        obs, step_reward, done, truncated, info = env.step(np.array([[steer, speed]]))
+        action = env.action_space.sample()
+        for i, agent_id in enumerate(obs.keys()):
+            speed, steer = planner.plan(
+                obs[agent_id]["pose_x"],
+                obs[agent_id]["pose_y"],
+                obs[agent_id]["pose_theta"],
+                work["tlad"],
+                work["vgain"],
+            )
+            action[i] = np.array([steer, speed])
+
+        obs, step_reward, done, truncated, info = env.step(action)
         laptime += step_reward
-        env.render()
+        frame = env.render()
 
     print("Sim elapsed time:", laptime, "Real elapsed time:", time.time() - start)
 

gym/f110_gym/__init__.py

@@ -1,6 +1,5 @@
 import gymnasium as gym
 
 gym.register(
-    id="f110-v0",
-    entry_point="f110_gym.envs:F110Env",
+    id="f110-v0", entry_point="f110_gym.envs:F110Env",
 )

gym/f110_gym/envs/base_classes.py

@@ -407,6 +407,7 @@ def __init__(
         self.ego_idx = ego_idx
         self.params = params
         self.agent_poses = np.empty((self.num_agents, 3))
+        self.agent_steerings = np.empty((self.num_agents,))
         self.agents: list[RaceCar] = []
         self.collisions = np.zeros((self.num_agents,))
         self.collision_idx = -1 * np.ones((self.num_agents,))
@@ -504,6 +505,7 @@ def step(self, control_inputs):
 
             # update sim's information of agent poses
             self.agent_poses[i, :] = np.append(agent.state[0:2], agent.state[4])
+            self.agent_steerings[i] = agent.state[2]
 
         # check collisions between all agents
         self.check_collision()

gym/f110_gym/envs/cubic_spline.py

@@ -64,7 +64,7 @@ def calc_position(self, x):
         i = self.__search_index(x)
         dx = x - self.x[i]
         position = (
-            self.a[i] + self.b[i] * dx + self.c[i] * dx**2.0 + self.d[i] * dx**3.0
+            self.a[i] + self.b[i] * dx + self.c[i] * dx ** 2.0 + self.d[i] * dx ** 3.0
         )
 
         return position
@@ -86,7 +86,7 @@ def calc_first_derivative(self, x):
 
         i = self.__search_index(x)
         dx = x - self.x[i]
-        dy = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx**2.0
+        dy = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx ** 2.0
         return dy
 
     def calc_second_derivative(self, x):
@@ -205,7 +205,7 @@ def calc_curvature(self, s):
         ddx = self.sx.calc_second_derivative(s)
         dy = self.sy.calc_first_derivative(s)
         ddy = self.sy.calc_second_derivative(s)
-        k = (ddy * dx - ddx * dy) / ((dx**2 + dy**2) ** (3 / 2))
+        k = (ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2) ** (3 / 2))
         return k
 
     def calc_yaw(self, s):