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/random_trackgen.py

@@ -37,6 +37,7 @@
 import matplotlib.pyplot as plt
 import numpy as np
 import shapely.geometry as shp
+from f1tenth_gym.envs.track import Track
 
 
 def main(args):
@@ -52,8 +53,9 @@ def main(args):
         while True:
             try:
                 print(f"[info] creating track {i}")
-                track, track_int, track_ext = create_track()
-                convert_track(track, track_int, track_ext, i, outdir)
+                WIDTH = 10.0
+                track, track_int, track_ext = create_track(WIDTH)
+                convert_track(track, track_int, track_ext, WIDTH, i, outdir)
                 print(f"[info] saved track {i} in {outdir}/")
                 break
             except Exception as _:  # noqa: F841
@@ -62,13 +64,12 @@ def main(args):
             print()
 
 
-def create_track():
+def create_track(track_width):
     CHECKPOINTS = 16
     SCALE = 6.0
     TRACK_RAD = 900 / SCALE
     TRACK_DETAIL_STEP = 21 / SCALE
     TRACK_TURN_RATE = 0.31
-    WIDTH = 10.0
 
     start_alpha = 0.0
 
@@ -183,15 +184,15 @@ def create_track():
     track_poly = shp.Polygon(track_xy)
 
     # Finding interior and exterior walls
-    track_xy_offset_in = track_poly.buffer(WIDTH)
-    track_xy_offset_out = track_poly.buffer(-WIDTH)
+    track_xy_offset_in = track_poly.buffer(track_width)
+    track_xy_offset_out = track_poly.buffer(-track_width)
     track_xy_offset_in_np = np.array(track_xy_offset_in.exterior.xy).T
     track_xy_offset_out_np = np.array(track_xy_offset_out.exterior.xy).T
 
     return track_xy, track_xy_offset_in_np, track_xy_offset_out_np
 
 
-def convert_track(track, track_int, track_ext, track_id, outdir):
+def convert_track(track, track_int, track_ext, track_width, track_id, outdir):
     # converts track to image and saves the centerline as waypoints
     fig, ax = plt.subplots()
     fig.set_size_inches(20, 20)
@@ -238,11 +239,16 @@ def convert_track(track, track_int, track_ext, track_id, outdir):
 
     # Saving centerline as a csv
     centerline_filepath = outdir / f"map{track_id}_centerline.csv"
+    x_m, y_m = xy_pixels[:, 0] * 0.05, xy_pixels[:, 1] * 0.05
     with open(centerline_filepath, "w") as waypoints_csv:
-        waypoints_csv.write("#x,y\n")
+        waypoints_csv.write("# x_m, y_m, w_tr_right_m, w_tr_left_m\n")
         for row in xy_pixels:
-            waypoints_csv.write(f"{0.05 * row[0]}, {0.05 * row[1]}\n")
+            waypoints_csv.write(f"{0.05 * row[0]}, {0.05 * row[1]}, {track_width}, {track_width}\n")
 
+    raceline_format = Track.from_refline(x=x_m, y=y_m, velx=np.ones_like(x_m) * 4.0)
+    raceline_format.spec.name = f"map{track_id}"
+    raceline_format.save_raceline(outdir)
+    print(f"[info] saved centerline in {centerline_filepath}")
 
 if __name__ == "__main__":
     import argparse

examples/run_in_empty_track.py

@@ -31,8 +31,8 @@ def main():
     planner = PurePursuitPlanner(track=track, wb=0.17145 + 0.15875)
 
     # rendering callbacks
-    env.add_render_callback(track.raceline.render_waypoints)
-    env.add_render_callback(planner.render_lookahead_point)
+    env.unwrapped.add_render_callback(track.raceline.render_waypoints)
+    env.unwrapped.add_render_callback(planner.render_lookahead_point)
 
     # simulation
     obs, info = env.reset()

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,29 +301,36 @@ 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={
-            "map": "Spielberg",
+            "map": "Spielberg_blank",
             "num_agents": num_agents,
             "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