Adds Multi-body dynamics primarily

.github/workflows/ci.yml

@@ -11,16 +11,20 @@ permissions:
 
 jobs:
   build:
-
     runs-on: ubuntu-latest
     strategy:
       fail-fast: false
       matrix:
         python-version: ["3.9", "3.10", "3.11", "3.12"]
+    # env:
+    #   QT_QPA_PLATFORM: offscreen
 
     steps:
+    - name: Update apt
+      run: sudo apt update
+
     - name: Install openGL
-      run: sudo apt install freeglut3-dev
+      run: sudo apt install -y freeglut3-dev libglib2.0-0 libsm6 libxrender1 libxext6 libxkbcommon-x11-0 libdbus-1-dev
 
     - uses: actions/checkout@v2
 

examples/waypoint_follow.py

@@ -5,6 +5,7 @@
 import numpy as np
 from numba import njit
 
+from f1tenth_gym.envs.f110_env import F110Env
 
 """
 Planner Helpers
@@ -187,6 +188,9 @@ def __init__(self, track, wb):
         self.lookahead_point = None
         self.current_index = None
 
+        self.lookahead_point_render = None
+        self.local_plan_render = None
+
     def load_waypoints(self, conf):
         """
         loads waypoints
@@ -201,16 +205,26 @@ 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)
+            points = self.lookahead_point[:2][None]  # shape (1, 2)~
+            if self.lookahead_point_render is None:
+                self.lookahead_point_render = e.render_points(
+                    points, color=(0, 0, 128), size=2
+                )
+            else:
+                self.lookahead_point_render.setData(points)
 
     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)
+            if self.local_plan_render is None:
+                self.local_plan_render = e.render_lines(
+                    points, color=(0, 128, 0), size=1
+                )
+            else:
+                self.local_plan_render.updateItems(points)
 
     def _get_current_waypoint(
         self, waypoints, lookahead_distance, position, theta
@@ -287,11 +301,11 @@ def main():
     work = {
         "mass": 3.463388126201571,
         "lf": 0.15597534362552312,
-        "tlad": 0.82461887897713965,
-        "vgain": 1,
+        "tlad": 0.82461887897713965 * 10,
+        "vgain": 1.0,
     }
 
-    num_agents = 3
+    num_agents = 1
     env = gym.make(
         "f1tenth_gym:f1tenth-v0",
         config={
@@ -300,16 +314,23 @@ def main():
             "timestep": 0.01,
             "integrator": "rk4",
             "control_input": ["speed", "steering_angle"],
-            "model": "st",
+            "model": "mb",
             "observation_config": {"type": "kinematic_state"},
-            "params": {"mu": 1.0},
+            "params": F110Env.fullscale_vehicle_params(),
             "reset_config": {"type": "rl_random_static"},
+            "scale": 10.0,
         },
         render_mode="human",
     )
     track = env.unwrapped.track
 
-    planner = PurePursuitPlanner(track=track, wb=0.17145 + 0.15875)
+    planner = PurePursuitPlanner(
+        track=track,
+        wb=(
+            F110Env.fullscale_vehicle_params()["lf"]
+            + F110Env.fullscale_vehicle_params()["lr"]
+        ),
+    )
 
     env.unwrapped.add_render_callback(track.raceline.render_waypoints)
     env.unwrapped.add_render_callback(planner.render_local_plan)
@@ -333,7 +354,6 @@ def main():
                 work["vgain"],
             )
             action[i] = np.array([steer, speed])
-
         obs, step_reward, done, truncated, info = env.step(action)
         laptime += step_reward
         frame = env.render()
@@ -343,3 +363,12 @@ def main():
 
 if __name__ == "__main__":
     main()
+# %%
+work = {
+    "mass": 3.463388126201571,
+    "lf": 0.15597534362552312,
+    "tlad": 0.82461887897713965 * 10,
+    "vgain": 1,
+}
+
+num_agents = 1

f1tenth_gym/envs/base_classes.py

@@ -26,12 +26,13 @@
 Replacement of the old RaceCar, Simulator classes in C++
 Author: Hongrui Zheng
 """
+
 from __future__ import annotations
 import numpy as np
 from .dynamic_models import DynamicModel
 from .action import CarAction
 from .collision_models import collision_multiple, get_vertices
-from .integrator import EulerIntegrator, IntegratorType
+from .integrator import EulerIntegrator, Integrator
 from .laser_models import ScanSimulator2D, check_ttc_jit, ray_cast
 from .track import Track
 
@@ -101,9 +102,10 @@ 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()
+        self.state = self.model.get_initial_state(params=self.params)
 
         # pose of opponents in the world
         self.opp_poses = None
@@ -178,14 +180,15 @@ def update_params(self, params):
         """
         self.params = params
 
-    def set_map(self, map: str | Track):
+    def set_map(self, map: str | Track, map_scale: float = 1.0):
         """
         Sets the map for scan simulator
 
         Args:
             map (str | Track): name of the map, or Track object
+            map_scale (float, default=1.0): scale of the map, larger scale means larger map
         """
-        RaceCar.scan_simulator.set_map(map)
+        RaceCar.scan_simulator.set_map(map, map_scale)
 
     def reset(self, pose):
         """
@@ -203,7 +206,7 @@ def reset(self, pose):
         # clear collision indicator
         self.in_collision = False
         # init state from pose
-        self.state = self.model.get_initial_state(pose=pose)
+        self.state = self.model.get_initial_state(pose=pose, params=self.params)
 
         self.steer_buffer = np.empty((0,))
         # reset scan random generator
@@ -310,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(
@@ -354,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):
     """
@@ -379,7 +392,7 @@ def __init__(
         num_agents,
         seed,
         action_type: CarAction,
-        integrator=IntegratorType.RK4,
+        integrator=Integrator,
         model=DynamicModel.ST,
         time_step=0.01,
         ego_idx=0,
@@ -428,18 +441,19 @@ def __init__(
         num_beams = self.agents[0].scan_simulator.num_beams
         self.agent_scans = np.empty((self.num_agents, num_beams))
 
-    def set_map(self, map: str | Track):
+    def set_map(self, map: str | Track, map_scale: float = 1.0):
         """
         Sets the map of the environment and sets the map for scan simulator of each agent
 
         Args:
             map (str | Track): name of the map, or Track object
+            map_scale (float, default=1.0): scale of the map, larger scale means larger map
 
         Returns:
             None
         """
         for agent in self.agents:
-            agent.set_map(map)
+            agent.set_map(map, map_scale)
 
     def update_params(self, params, agent_idx=-1):
         """