towards quick graphics

get_line_follow_sim/RobotSimulationManager.py

@@ -7,6 +7,7 @@ from PIL import Image, ImageDraw
 from typing import List, Tuple, Optional
 from abc import ABC, abstractmethod
 from tqdm import tqdm
+import time
 
 from get_line_follow_sim.util import translate, rotate
 
@@ -157,10 +158,10 @@ class Robot(ABC):
         self.patches = []
 
         # simulation parameters
-        self.mass = 1.0  # kilograms
-        self.inertia = 0.1  # kg.m^2
-        self.max_tire_angular_velocity = 10.0  # radians per second
-        self.motor_response_coefficient = 1.0  # Scaling factor for motor delay
+        self.mass = 0.150  # kilograms
+        self.inertia = 0.015  # kg.m^2
+        self.max_tire_angular_velocity = 6.0  # radians per second
+        self.motor_response_coefficient = 0.3  # Scaling factor for motor delay
         self.pose = np.array([0.0, 0.0, 0.0])  # [x, y, theta]
         self.robot_vx = 0.0  # Linear velocity in x-direction
         self.robot_omega = 0.0  # Angular velocity (yaw rate)
@@ -190,6 +191,7 @@ class Robot(ABC):
     def build_chassis_patches(self) -> None:
         self.__chassis_patch = \
             plt.Polygon([[0, -self.wheel_base / 2], [0, self.wheel_base / 2], [self.length, 0]], color='blue')
+        self.__chassis_patch.set_animated(True)
 
         rect_w = self.tire_radius * 2
         rect_h = self.wheel_base / 6
@@ -201,6 +203,8 @@ class Robot(ABC):
                 (0, 0),
                 rect_w, rect_h, color='gray')
         ]
+        for patch in self.__tire_patches:
+            patch.set_animated(True)
 
     def get_patches(self) -> List[matplotlib.patches.Patch]:
         return [self.__chassis_patch] + self.__tire_patches + [
@@ -325,6 +329,7 @@ class BinarySensor(Sensor):
 
         marker_path = path.Path(vertices, codes)
         self.__patch = patches.PathPatch(marker_path, color='red')
+        self.__patch.set_animated(True)
         return self.__patch
 
     def evaluate(self, pose: np.ndarray, environment: Environment) -> int:
@@ -347,6 +352,7 @@ class CircularSensor(Sensor):
 
         self.__patch = patches.Circle(
             (float(self.offset[0,0]), float(self.offset[1,0])), radius=self.diameter / 2, color='red', fill=False)
+        self.__patch.set_animated(True)
 
         return self.__patch
 
@@ -435,7 +441,9 @@ class Simulator:
         iterations = 0
         max_iterations = min(max_its, int(max_time / dt))
         gui_patches = []
-        time = 0.0
+        sim_time = 0.0
+        frame_time = time.time()
+        target_fps = 1.0
 
         # old_state = self.environment.start_pose.copy()
         # old_state = np.array([[1, 300, 0.0, 1.0]]).T
@@ -453,7 +461,7 @@ class Simulator:
                 ax.add_patch(patch)
 
         for _ in tqdm(range(max_iterations)):
-            time = iterations * dt
+            sim_time = iterations * dt
 
             # robot state update
             state = self.robot.update_state(self.environment, dt)
@@ -477,11 +485,12 @@ class Simulator:
                 break
 
             # Visualization
-            if gui:
+            if gui and frame_time + 1/target_fps > time.time():
                 self.robot.update_ui()
 
                 plt.draw()
                 plt.pause(1e-6)
+                frame_time += 1/target_fps
 
             if real_time:
                 time.sleep(dt)
@@ -493,4 +502,4 @@ class Simulator:
             plt.ioff()
             plt.show()
 
-        print(f'Simulation ended after {iterations} its/{time:.2f} s. Metrics:', self.metrics)
\ No newline at end of file
+        print(f'Simulation ended after {iterations} its/{sim_time:.2f} s. Metrics:', self.metrics)
\ No newline at end of file