feat(agent): creation of the robo cinematic model 🔥

microvault/environment/continuous.py

@@ -1,4 +1,4 @@
-import matplotlib.animation as animation
+import matplotlib.animation as animation  # HTMLWriter
 import matplotlib.pyplot as plt
 import numpy as np
 
@@ -24,12 +24,12 @@ def __init__(
         time: int = 100,  # max step
         size: float = 3.0,  # size robot
         fps: int = 100,  # 10 frames per second
-        random: float = 1e20,  # 100 random points
+        random: int = 1300,  # 100 random points
         max_speed: float = 0.6,  # 0.2 m/s
         min_speed: float = 0.5,  # 0.1 m/s
         num_rays: int = 40,  # num range lidar
         max_range: int = 6,  # max range
-        grid_lenght: int = 10,  # TODO: error < 5 -> [5 - 15]
+        grid_lenght: int = 20,  # TODO: error < 5 -> [5 - 15]
     ):
         self.time = time
         self.reset_time = time
@@ -40,6 +40,9 @@ def __init__(
         self.max_speed = max_speed
         self.min_speed = min_speed
 
+        self.vr = 0
+        self.vl = 0
+
         self.random = random
 
         self.grid_lenght = grid_lenght
@@ -55,6 +58,7 @@ def __init__(
         self.max_speed = self.initial_max_speed
 
         self.segments = None
+        self.poly = None
 
         self.fov = 2 * np.pi  # -90 * np.pi / 180
 
@@ -188,6 +192,19 @@ def _ray_casting(poly: Polygon, x: float, y: float) -> bool:
         circle = center.buffer(0.5)
         return circle.within(poly)
 
+    @staticmethod
+    def _setup_space() -> Space:
+        """
+        Sets up the pymunk physics space.
+
+        Returns:
+        Space: The pymunk physics space.
+        """
+        space = Space()
+        space.gravity = (0, 0)  # 0.0 m/s^2
+        space.damping = 0.99
+        return space
+
     @staticmethod
     def _get_label(timestep: int) -> str:
         """
@@ -203,7 +220,7 @@ def _get_label(timestep: int) -> str:
         line2 = "Time Step:".ljust(14) + f"{timestep:4.0f}\n"
         return line1 + line2
 
-    def reset(self) -> None:
+    def reset_world(self) -> None:
         """
         Resets the environment.
 
@@ -219,16 +236,14 @@ def reset(self) -> None:
 
         new_map_path, poly, seg = self.generator.world()
         self.segments = seg
+        self.poly = poly
 
         self.ax.add_patch(new_map_path)
         art3d.pathpatch_2d_to_3d(new_map_path, z=0, zdir="z")
 
         self.target_x = np.random.uniform(0, self.xmax)
         self.target_y = np.random.uniform(0, self.ymax)
 
-        self.x[0] = np.random.uniform(0, self.xmax)
-        self.y[0] = np.random.uniform(0, self.ymax)
-
         self.robot_body = Body()
         self.robot_body.position = (self.x[0], self.y[0])
         self.robot_shape = Circle(self.robot_body, radius=self.size)
@@ -242,18 +257,22 @@ def reset(self) -> None:
             self.target_x = np.random.uniform(0, self.xmax)
             self.target_y = np.random.uniform(0, self.ymax)
 
+            if self._ray_casting(poly, self.target_x, self.target_y):
+                target_inside = True
+
+    def reset_agent(self, poly) -> None:
+        self.theta[0] = np.random.uniform(0, 2 * np.pi)
+        self.x[0] = np.random.uniform(0, self.xmax)
+        self.y[0] = np.random.uniform(0, self.ymax)
+
+        agent_inside = False
+
+        while not agent_inside:
             self.x[0] = np.random.uniform(0, self.xmax)
             self.y[0] = np.random.uniform(0, self.ymax)
 
-            if self._ray_casting(
-                poly, self.target_x, self.target_y
-            ) and self._ray_casting(poly, self.x[0], self.y[0]):
-                target_inside = True
-
-        self.sp[0] = np.random.uniform(self.min_speed, self.max_speed)
-        self.theta[:] = np.random.uniform(0, 2 * np.pi)
-        self.vx[0] = self.sp[0] * np.cos(self.theta[0])
-        self.vy[0] = self.sp[0] * np.sin(self.theta[0])
+            if self._ray_casting(poly, self.x[0], self.y[0]):
+                agent_inside = True
 
     def step(self, i: int) -> None:
         """
@@ -265,32 +284,21 @@ def step(self, i: int) -> None:
         Returns:
         None
         """
+
         # if self.reset_flag:
         #     self.reset_flag = False
         #     self.reset()
         #     return
 
-        self.space.step(1 / self.fps)
+        # self.space.step(1 / self.fps)
 
-        # print(self.robot_body.position)
+        self.vr = np.random.uniform(0.1, 0.9)
+        self.vl = np.random.uniform(0.1, 0.9)
 
-        # self.robot.x_advance(i, self.x, self.vx)
-        # self.robot.y_advance(i, self.y, self.vy)
-        #
-
-        # self.max_speed += self.speed_increase_rate
-
-        # self.robot_body.position += v * dt * pymunk.Vec2d(self.robot_body.rotation_vector)
-        # self.robot_body.angle += omega * dt
-
-        self.max_speed = min(self.max_speed, self.initial_max_speed)
-
-        vl = np.random.uniform(self.min_speed, self.max_speed)
-        vr = np.random.uniform(self.min_speed, self.max_speed)
-
-        self.robot.move(i, self.x, self.y, self.theta, self.vx, self.vy, vl, vr)
-
-        # print(f'X: {self.x[i]} Y: {self.y[i]}')
+        if i == 0:
+            self.reset_agent(self.poly)
+        else:
+            self.robot.move(i, self.x, self.y, self.theta, self.vl, self.vr)
 
         seg = filter_segment(self.segments, self.x[i], self.y[i], 6)
 
@@ -326,14 +334,14 @@ def step(self, i: int) -> None:
 
         self.label.set_text(self._get_label(i))
 
-        if i == 50:  # Replace with your condition
-            self.reset()
-            self.ani.frame_seq = self.ani.new_frame_seq()
+        # if i == 50:  # Replace with your condition
+        #     self.reset()
+        #     self.ani.frame_seq = self.ani.new_frame_seq()
 
         #     self.time = 100
         #     self.reset_flag = True
 
-    def show(self, plot: bool = False) -> None:
+    def render(self, plot: str = "local") -> None:
         """
         Displays the simulation.
 
@@ -343,28 +351,28 @@ def show(self, plot: bool = False) -> None:
         Returns:
         None
         """
-        # TODO
-        # fig2 = plt.figure()
-        # ax2 = fig2.add_subplot(111)
-        # ax2.plot([1, 2, 3, 4], [1, 4, 9, 16])
-        # if self.ani:
-        #     self.ani.event_source.stop()
 
         self.ani = animation.FuncAnimation(
             self.fig,
             self.step,
-            init_func=self.reset(),
+            init_func=self.reset_world,
             blit=False,
             frames=self.time,
             interval=self.fps,
         )
 
-        if plot:
+        if plot == "local":
             plt.show()
-        # else:
-        #     ani.save("animacao.html", writer=HTMLWriter(fps=self.time))
-        # ani.save("animacao.mp4", fps=self.time)
+        # elif plot == "html":
+        # self.ani.save("animacao.html", writer=HTMLWriter(fps=self.time))
+        elif plot == "video":
+            self.ani.save("animacao.mp4", fps=self.time)
+        else:
+            pass
+
+    def trainer(self, visualize: str = "local"):
+        self.render(visualize)
 
 
-# env = Continuous()
-# env.show(plot=True)
+# agent = Continuous()
+# agent.trainer()

microvault/environment/generate.py

@@ -96,19 +96,6 @@ def world(self) -> Tuple[PathPatch, Polygon, List]:
         border = self._map_border(m)
         map_grid = 1 - border
 
-        # print(map_grid)
-
-        # new_resolution = 0.1
-        # new_map = self.upscale_map(map_grid, new_resolution)
-
-        # print(new_map)
-
-        # map_grid = new_map
-
-        # plt.imshow(new_map, cmap='binary', origin='lower')
-        # plt.colorbar()
-        # plt.show()
-
         contours = measure.find_contours(map_grid, 0.5)
 
         exterior = [
@@ -137,29 +124,6 @@ def world(self) -> Tuple[PathPatch, Polygon, List]:
 
         segment = extract_segment(stacks)
 
-        # x, y = segment.xy  # Extrai coordenadas x e y do segmento
-
-        # for seg in segment:
-        #     x_values = [seg[0], seg[2]]
-        #     y_values = [seg[1], seg[3]]
-        #     plt.plot(x_values, y_values, color='blue')
-        # plt.xlabel('X')
-        # plt.ylabel('Y')
-        # plt.title('Line Segments')
-        # plt.grid(True)
-        # plt.show()
-
-        # for i in range(len(stacks)):
-        #     x, y = stacks[i].T
-        #     plt.plot(x, y, color='black')
-
-        # plt.plot(x, y)  # Plota o segmento
-        # plt.xlabel('X')
-        # plt.ylabel('Y')
-        # plt.title('Segmento')
-        # plt.gca().set_aspect('equal', adjustable='box')
-        # plt.show()
-
         poly = Polygon(exterior, holes=interiors)
 
         if not poly.is_valid:
@@ -176,7 +140,3 @@ def world(self) -> Tuple[PathPatch, Polygon, List]:
         )
 
         return path_patch, poly, segment
-
-
-# env = Generator()
-# path_patch, poly, segment = env.world()

microvault/environment/robot.py

@@ -25,6 +25,7 @@ def __init__(
 
         self.wheel_radius = wheel_radius
         self.wheel_base = wheel_base
+        self.body = None
 
     def init_agent(self, ax: Axes3D) -> Tuple[
         np.ndarray,
@@ -62,94 +63,29 @@ def init_agent(self, ax: Axes3D) -> Tuple[
 
         return (x, y, sp, theta, vx, vy, radius)
 
-    def calculate_velocities(self, vl, vr):
-        v = (self.wheel_radius / 2) * (vl + vr)
-        omega = (self.wheel_radius / self.wheel_base) * (vr - vl)
-        return v, omega
+    def move(self, dt, x, y, theta, vl, vr) -> None:
+        delta_t = 1  # Assuming time step of 1 unit
 
-    def update_position(self, dt, x, y, theta, vl, vr):
-        v, omega = self.calculate_velocities(vl, vr)
-        theta_new = theta + omega * dt
+        v_right = vr * self.wheel_radius
+        v_left = vl * self.wheel_radius
 
-        max_distance = v * dt
-        dx = max_distance * np.cos(theta_new)
-        dy = max_distance * np.sin(theta_new)
-        x_new = min(max(0, x + dx), self.xmax)
-        y_new = min(max(0, y + dy), self.ymax)
+        v = (v_right + v_left) / 2
+        omega = (v_right - v_left) / self.wheel_base
 
-        return x_new, y_new, theta_new
+        theta_prev = theta[dt - 1]
+        theta_new = theta_prev + omega * delta_t
 
-    def x_advance(self, dt: int, x: np.ndarray, vx: np.ndarray) -> None:
-        """
-        Advances the agent's x position based on its current velocity.
-
-        Parameters:
-        dt (int): The current time step.
-        x (np.ndarray): Array of agent's x positions over time.
-        vx (np.ndarray): Array of agent's x velocities over time.
-
-        Returns:
-        None
-        """
-        if (self.time - 1) != dt:
-            if x[dt] + vx[dt] >= self.xmax or x[dt] + vx[dt] <= 0:
-                x[dt + 1] = x[dt] - vx[dt]
-                vx[dt + 1] = -vx[dt]
-            else:
-                x[dt + 1] = x[dt] + vx[dt]
-                vx[dt + 1] = vx[dt]
-        else:
-            if x[dt] + vx[dt] >= self.xmax or x[dt] + vx[dt] <= 0:
-                x[dt] = x[dt] - vx[dt]
-                vx[dt] = -vx[dt]
-            else:
-                x[dt] = x[dt] + vx[dt]
-                vx[dt] = vx[dt]
-
-    def y_advance(self, dt: int, y: np.ndarray, vy: np.ndarray) -> None:
-        """
-        Advances the agent's y position based on its current velocity.
-
-        Parameters:
-        dt (int): The current time step.
-        y (np.ndarray): Array of agent's y positions over time.
-        vy (np.ndarray): Array of agent's y velocities over time.
-
-        Returns:
-        None
-        """
-        if (self.time - 1) != dt:
-            if y[dt] + vy[dt] >= self.ymax or y[dt] + vy[dt] <= 0:
-                y[dt + 1] = y[dt] - vy[dt]
-                vy[dt + 1] = -vy[dt]
-            else:
-                y[dt + 1] = y[dt] + vy[dt]
-                vy[dt + 1] = vy[dt]
-        else:
-            if y[dt] + vy[dt] >= self.ymax or y[dt] + vy[dt] <= 0:
-                y[dt] = y[dt] - vy[dt]
-                vy[dt] = -vy[dt]
-            else:
-                y[dt] = y[dt] + vy[dt]
-                vy[dt] = vy[dt]
-
-    def move(self, dt, x, y, theta, vx, vy, vl, vr):
-        """
-        Move o robô com base nas velocidades das rodas esquerda e direita.
+        # Ensure theta stays within -2π to 2π
+        if theta_new > 2 * np.pi or theta_new < -2 * np.pi:
+            theta_new = 0
 
-        Parameters:
-        vl (float): Velocidade linear da roda esquerda.
-        vr (float): Velocidade linear da roda direita.
+        theta[dt] = theta_new
 
-        Returns:
-        None
-        """
-        v, omega = self.calculate_velocities(vl, vr)
-        x[dt + 1], y[dt + 1], theta[dt + 1] = self.update_position(
-            dt, x[dt], y[dt], theta[dt], vl, vr
-        )
+        x_temp = x[dt - 1] + v * np.cos(theta_new) * delta_t
+        y_temp = y[dt - 1] + v * np.sin(theta_new) * delta_t
 
-        # print("Pos X: ", x)
+        x_new = max(0.0, float(min(x_temp, self.xmax)))
+        y_new = max(0.0, float(min(y_temp, self.ymax)))
 
-        vx[dt + 1] = v * np.cos(theta[dt + 1])
-        vy[dt + 1] = v * np.sin(theta[dt + 1])
+        x[dt] = x_new
+        y[dt] = y_new