From fb0a736f293bb382dd3fc11957b63144cf795b9e Mon Sep 17 00:00:00 2001 From: Infinity2Beyond Date: Thu, 14 May 2026 09:24:52 +0300 Subject: [PATCH 1/2] First Setup Setup Pygame in Enviroment, created sample pygame_render file --- pyproject.toml | 1 + src/simulator/main.py | 3 +++ src/simulator/pygame_renderer.py | 7 +++++++ uv.lock | 24 ++++++++++++++++++++++++ 4 files changed, 35 insertions(+) create mode 100644 src/simulator/pygame_renderer.py diff --git a/pyproject.toml b/pyproject.toml index 370ab92..be3208f 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -7,6 +7,7 @@ requires-python = ">=3.12" dependencies = [ "numpy>=2.4.4", "pre-commit>=4.6.0", + "pygame>=2.6.1", "pytest>=9.0.3", "ruff>=0.15.12", ] diff --git a/src/simulator/main.py b/src/simulator/main.py index bd19be6..02c644c 100644 --- a/src/simulator/main.py +++ b/src/simulator/main.py @@ -3,6 +3,8 @@ from physics import PhysicsEngine from renderer import Renderer +from pygame_renderer import PygameRenderer + from world import World from objects import RobotTree, TwoLink, Tree7, CartPole @@ -15,6 +17,7 @@ def main(): physics = PhysicsEngine(fd_solver, gravity=[0.0, -9.81, 0.0]) renderer = Renderer() + # renderer = PygameRenderer() world = World(physics, renderer) diff --git a/src/simulator/pygame_renderer.py b/src/simulator/pygame_renderer.py new file mode 100644 index 0000000..aba6340 --- /dev/null +++ b/src/simulator/pygame_renderer.py @@ -0,0 +1,7 @@ +import pygame +import numpy as np +import sys + +class PygameRenderer: + def __init__(self): + pygame.init() \ No newline at end of file diff --git a/uv.lock b/uv.lock index 2eff7b6..de11c4a 100644 --- a/uv.lock +++ b/uv.lock @@ -169,6 +169,28 @@ wheels = [ { url = "https://files.pythonhosted.org/packages/80/6e/4b28b62ecb6aae56769c34a8ff1d661473ec1e9519e2d5f8b2c150086b26/pre_commit-4.6.0-py2.py3-none-any.whl", hash = "sha256:e2cf246f7299edcabcf15f9b0571fdce06058527f0a06535068a86d38089f29b", size = 226472, upload-time = "2026-04-21T20:31:40.092Z" }, ] +[[package]] +name = "pygame" +version = "2.6.1" +source = { registry = "https://pypi.org/simple" } +sdist = { url = "https://files.pythonhosted.org/packages/49/cc/08bba60f00541f62aaa252ce0cfbd60aebd04616c0b9574f755b583e45ae/pygame-2.6.1.tar.gz", hash = "sha256:56fb02ead529cee00d415c3e007f75e0780c655909aaa8e8bf616ee09c9feb1f", size = 14808125, upload-time = "2024-09-29T13:41:34.698Z" } +wheels = [ + { url = "https://files.pythonhosted.org/packages/92/16/2c602c332f45ff9526d61f6bd764db5096ff9035433e2172e2d2cadae8db/pygame-2.6.1-cp312-cp312-macosx_10_9_x86_64.whl", hash = "sha256:4ee7f2771f588c966fa2fa8b829be26698c9b4836f82ede5e4edc1a68594942e", size = 13118279, upload-time = "2024-09-29T14:26:30.427Z" }, + { url = "https://files.pythonhosted.org/packages/cd/53/77ccbc384b251c6e34bfd2e734c638233922449a7844e3c7a11ef91cee39/pygame-2.6.1-cp312-cp312-macosx_11_0_arm64.whl", hash = "sha256:c8040ea2ab18c6b255af706ec01355c8a6b08dc48d77fd4ee783f8fc46a843bf", size = 12384524, upload-time = "2024-09-29T14:26:49.996Z" }, + { url = "https://files.pythonhosted.org/packages/06/be/3ed337583f010696c3b3435e89a74fb29d0c74d0931e8f33c0a4246307a9/pygame-2.6.1-cp312-cp312-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:c47a6938de93fa610accd4969e638c2aebcb29b2fca518a84c3a39d91ab47116", size = 13587123, upload-time = "2024-09-29T11:10:50.072Z" }, + { url = "https://files.pythonhosted.org/packages/fd/ca/b015586a450db59313535662991b34d24c1f0c0dc149cc5f496573900f4e/pygame-2.6.1-cp312-cp312-manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:33006f784e1c7d7e466fcb61d5489da59cc5f7eb098712f792a225df1d4e229d", size = 14275532, upload-time = "2024-09-29T11:39:59.356Z" }, + { url = "https://files.pythonhosted.org/packages/b9/f2/d31e6ad42d657af07be2ffd779190353f759a07b51232b9e1d724f2cda46/pygame-2.6.1-cp312-cp312-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:1206125f14cae22c44565c9d333607f1d9f59487b1f1432945dfc809aeaa3e88", size = 13952653, upload-time = "2024-09-29T11:40:01.781Z" }, + { url = "https://files.pythonhosted.org/packages/f3/42/8ea2a6979e6fa971702fece1747e862e2256d4a8558fe0da6364dd946c53/pygame-2.6.1-cp312-cp312-win32.whl", hash = "sha256:84fc4054e25262140d09d39e094f6880d730199710829902f0d8ceae0213379e", size = 10252421, upload-time = "2024-09-29T11:14:26.877Z" }, + { url = "https://files.pythonhosted.org/packages/5f/90/7d766d54bb95939725e9a9361f9c06b0cfbe3fe100aa35400f0a461a278a/pygame-2.6.1-cp312-cp312-win_amd64.whl", hash = "sha256:3a9e7396be0d9633831c3f8d5d82dd63ba373ad65599628294b7a4f8a5a01a65", size = 10624591, upload-time = "2024-09-29T11:52:54.489Z" }, + { url = "https://files.pythonhosted.org/packages/e1/91/718acf3e2a9d08a6ddcc96bd02a6f63c99ee7ba14afeaff2a51c987df0b9/pygame-2.6.1-cp313-cp313-macosx_10_13_x86_64.whl", hash = "sha256:ae6039f3a55d800db80e8010f387557b528d34d534435e0871326804df2a62f2", size = 13090765, upload-time = "2024-09-29T14:27:02.377Z" }, + { url = "https://files.pythonhosted.org/packages/0e/c6/9cb315de851a7682d9c7568a41ea042ee98d668cb8deadc1dafcab6116f0/pygame-2.6.1-cp313-cp313-macosx_11_0_arm64.whl", hash = "sha256:2a3a1288e2e9b1e5834e425bedd5ba01a3cd4902b5c2bff8ed4a740ccfe98171", size = 12381704, upload-time = "2024-09-29T14:27:10.228Z" }, + { url = "https://files.pythonhosted.org/packages/9f/8f/617a1196e31ae3b46be6949fbaa95b8c93ce15e0544266198c2266cc1b4d/pygame-2.6.1-cp313-cp313-manylinux_2_17_aarch64.manylinux2014_aarch64.whl", hash = "sha256:27eb17e3dc9640e4b4683074f1890e2e879827447770470c2aba9f125f74510b", size = 13581091, upload-time = "2024-09-29T11:30:27.653Z" }, + { url = "https://files.pythonhosted.org/packages/3b/87/2851a564e40a2dad353f1c6e143465d445dab18a95281f9ea458b94f3608/pygame-2.6.1-cp313-cp313-manylinux_2_17_i686.manylinux2014_i686.whl", hash = "sha256:4c1623180e70a03c4a734deb9bac50fc9c82942ae84a3a220779062128e75f3b", size = 14273844, upload-time = "2024-09-29T11:40:04.138Z" }, + { url = "https://files.pythonhosted.org/packages/85/b5/aa23aa2e70bcba42c989c02e7228273c30f3b44b9b264abb93eaeff43ad7/pygame-2.6.1-cp313-cp313-manylinux_2_17_x86_64.manylinux2014_x86_64.whl", hash = "sha256:ef07c0103d79492c21fced9ad68c11c32efa6801ca1920ebfd0f15fb46c78b1c", size = 13951197, upload-time = "2024-09-29T11:40:06.785Z" }, + { url = "https://files.pythonhosted.org/packages/a6/06/29e939b34d3f1354738c7d201c51c250ad7abefefaf6f8332d962ff67c4b/pygame-2.6.1-cp313-cp313-win32.whl", hash = "sha256:3acd8c009317190c2bfd81db681ecef47d5eb108c2151d09596d9c7ea9df5c0e", size = 10249309, upload-time = "2024-09-29T11:10:23.329Z" }, + { url = "https://files.pythonhosted.org/packages/7e/11/17f7f319ca91824b86557e9303e3b7a71991ef17fd45286bf47d7f0a38e6/pygame-2.6.1-cp313-cp313-win_amd64.whl", hash = "sha256:813af4fba5d0b2cb8e58f5d95f7910295c34067dcc290d34f1be59c48bd1ea6a", size = 10620084, upload-time = "2024-09-29T11:48:51.587Z" }, +] + [[package]] name = "pygments" version = "2.20.0" @@ -285,6 +307,7 @@ source = { virtual = "." } dependencies = [ { name = "numpy" }, { name = "pre-commit" }, + { name = "pygame" }, { name = "pytest" }, { name = "ruff" }, ] @@ -293,6 +316,7 @@ dependencies = [ requires-dist = [ { name = "numpy", specifier = ">=2.4.4" }, { name = "pre-commit", specifier = ">=4.6.0" }, + { name = "pygame", specifier = ">=2.6.1" }, { name = "pytest", specifier = ">=9.0.3" }, { name = "ruff", specifier = ">=0.15.12" }, ] From 343495cce26de44e937546d73471307eeae0b176 Mon Sep 17 00:00:00 2001 From: Infinity2Beyond Date: Thu, 14 May 2026 22:09:22 +0300 Subject: [PATCH 2/2] Update solution and simple test Wrote pygame_render logic and simple test_pygame for fps benchmark --- src/simulator/main.py | 4 +- src/simulator/pygame_renderer.py | 274 ++++++++++++++++++++++++++++++- tests/test_pygame.py | 33 ++++ 3 files changed, 307 insertions(+), 4 deletions(-) create mode 100644 tests/test_pygame.py diff --git a/src/simulator/main.py b/src/simulator/main.py index 02c644c..1236973 100644 --- a/src/simulator/main.py +++ b/src/simulator/main.py @@ -16,8 +16,8 @@ def main(): fd_solver = ABAlgorithm() physics = PhysicsEngine(fd_solver, gravity=[0.0, -9.81, 0.0]) - renderer = Renderer() - # renderer = PygameRenderer() + # renderer = Renderer() + renderer = PygameRenderer() world = World(physics, renderer) diff --git a/src/simulator/pygame_renderer.py b/src/simulator/pygame_renderer.py index aba6340..8d6daf1 100644 --- a/src/simulator/pygame_renderer.py +++ b/src/simulator/pygame_renderer.py @@ -1,7 +1,277 @@ import pygame import numpy as np import sys +import colorsys +import os +import tkinter as tk +from tkinter import filedialog class PygameRenderer: - def __init__(self): - pygame.init() \ No newline at end of file + def __init__(self, x_limits=(-10.0, 10.0), y_limits=(-10.0, 10.0), max_colors=20): + pygame.init() + pygame.font.init() + self.font = pygame.font.SysFont('Arial', 14) + + # Configure display window + self.width = 800 + self.height = 800 + self.screen = pygame.display.set_mode((self.width, self.height)) + pygame.display.set_caption("Pygame Backend (Scroll: Zoom | Drag: Pan | R: Reset | S: Save)") + + # Configure Margins + self.margin_left = 60 + self.margin_right = 30 + self.margin_top = 30 + self.margin_bottom = 50 + + # Save original state for the Reset feature (R key) + self.orig_x_limits = list(x_limits) + self.orig_y_limits = list(y_limits) + # Current coordinate limits + self.x_limits = list(x_limits) + self.y_limits = list(y_limits) + + # State variables for Mouse Events + self.is_panning = False + self.last_mouse_pos = (0, 0) + + # Color Palette for robot links + self.colors = [] + for i in range(max_colors): + hue = i / max_colors + r, g, b = colorsys.hsv_to_rgb(hue, 0.9, 0.9) + self.colors.append((int(r * 255), int(g * 255), int(b * 255))) + + def _get_plot_dims(self): + """Calculate the actual size of the plot area (excluding margins)""" + pw = self.width - self.margin_left - self.margin_right + ph = self.height - self.margin_top - self.margin_bottom + return pw, ph + + def _to_screen(self, x, y): + """ + Coordinate Mapping: WORLD -> SCREEN + Convert real-world coordinates (meters) to screen coordinates (pixels) + """ + pw, ph = self._get_plot_dims() + screen_x = self.margin_left + (x - self.x_limits[0]) / (self.x_limits[1] - self.x_limits[0]) * pw + screen_y = self.margin_top + (self.y_limits[1] - y) / (self.y_limits[1] - self.y_limits[0]) * ph + return int(screen_x), int(screen_y) + + def _to_world(self, screen_x, screen_y): + """ + Coordinate Mapping: SCREEN -> WORLD + Determine real-world coordinates from mouse position (used for Zoom/Pan) + """ + pw, ph = self._get_plot_dims() + x = self.x_limits[0] + (screen_x - self.margin_left) / pw * (self.x_limits[1] - self.x_limits[0]) + y = self.y_limits[1] - (screen_y - self.margin_top) / ph * (self.y_limits[1] - self.y_limits[0]) + return x, y + + def _zoom(self, zoom_factor, mouse_pos): + """Zoom simulation space, centered on the mouse cursor position""" + mx, my = self._to_world(*mouse_pos) + self.x_limits[0] = mx - (mx - self.x_limits[0]) * zoom_factor + self.x_limits[1] = mx + (self.x_limits[1] - mx) * zoom_factor + self.y_limits[0] = my - (my - self.y_limits[0]) * zoom_factor + self.y_limits[1] = my + (self.y_limits[1] - my) * zoom_factor + + def _pan(self, dx, dy): + """Shift camera view via mouse dragging""" + pw, ph = self._get_plot_dims() + w_world = self.x_limits[1] - self.x_limits[0] + h_world = self.y_limits[1] - self.y_limits[0] + + dx_world = (dx / pw) * w_world + dy_world = (dy / ph) * h_world + + self.x_limits[0] -= dx_world + self.x_limits[1] -= dx_world + self.y_limits[0] += dy_world + self.y_limits[1] += dy_world + + def _get_optimal_step(self, view_range, max_ticks=8): + raw_step = view_range / max_ticks + magnitude = 10 ** np.floor(np.log10(raw_step)) + normalized = raw_step / magnitude + + if normalized < 1.5: step = 1.0 + elif normalized < 3.5: step = 2.0 + elif normalized < 7.5: step = 5.0 + else: step = 10.0 + + return step * magnitude + + def _draw_axes(self): + """Draw Bounding Box, Grid lines, and numerical Labels""" + grid_color = (235, 235, 235) + text_color = (0, 0, 0) + box_color = (0, 0, 0) + + # Rectangle defining the exact drawing area (respecting margins) + plot_rect = pygame.Rect(self.margin_left, self.margin_top, + self.width - self.margin_left - self.margin_right, + self.height - self.margin_top - self.margin_bottom) + # Calculate grid step size for X and Y axes + step_x = self._get_optimal_step(self.x_limits[1] - self.x_limits[0]) + step_y = self._get_optimal_step(self.y_limits[1] - self.y_limits[0]) + + # Draw axis X + start_x = np.ceil(self.x_limits[0] / step_x) * step_x + for x in np.arange(start_x, self.x_limits[1] + step_x/2, step_x): + sx, _ = self._to_screen(x, 0) + if self.margin_left <= sx <= self.width - self.margin_right: + # draw grid line + pygame.draw.line(self.screen, grid_color, (sx, self.margin_top), (sx, self.height - self.margin_bottom), 1) + # draw tick line + pygame.draw.line(self.screen, box_color, (sx, self.height - self.margin_bottom), (sx, self.height - self.margin_bottom + 5), 1) + # draw number + txt = self.font.render(f"{x:g}", True, text_color) + self.screen.blit(txt, (sx - txt.get_width() // 2, self.height - self.margin_bottom + 10)) + + # Draw axis Y + start_y = np.ceil(self.y_limits[0] / step_y) * step_y + for y in np.arange(start_y, self.y_limits[1] + step_y/2, step_y): + _, sy = self._to_screen(0, y) + if self.margin_top <= sy <= self.height - self.margin_bottom: + # draw grid line + pygame.draw.line(self.screen, grid_color, (self.margin_left, sy), (self.width - self.margin_right, sy), 1) + # draw tick line + pygame.draw.line(self.screen, box_color, (self.margin_left - 5, sy), (self.margin_left, sy), 1) + # draw number + txt = self.font.render(f"{y:g}", True, text_color) + self.screen.blit(txt, (self.margin_left - txt.get_width() - 10, sy - txt.get_height() // 2)) + + # ox, oy = self._to_screen(0, 0) + # if self.margin_left <= ox <= self.width - self.margin_right: + # pygame.draw.line(self.screen, (150, 150, 150), (ox, self.margin_top), (ox, self.height - self.margin_bottom), 2) + # if self.margin_top <= oy <= self.height - self.margin_bottom: + # pygame.draw.line(self.screen, (150, 150, 150), (self.margin_left, oy), (self.width - self.margin_right, oy), 2) + + pygame.draw.rect(self.screen, box_color, plot_rect, 1) + + def _save_figure(self): + """Open Native OS Dialog to save screenshot""" + root = tk.Tk() + root.withdraw() + + root.attributes('-topmost', True) + # Call Save As dialog + file_path = filedialog.asksaveasfilename( + title="Save the figure (Save As)", + initialfile="simulator_figure.png", + defaultextension=".png", + filetypes=[ + ("PNG Image", "*.png"), + ("JPEG Image", "*.jpg;*.jpeg"), + ("Bitmap Image", "*.bmp"), + ("All Files", "*.*") + ] + ) + + root.destroy() + + if file_path: + try: + pygame.image.save(self.screen, file_path) + print(f"[Pygame] Successfully saved figure at: {os.path.abspath(file_path)}") + except Exception as e: + print(f"[Pygame] ERROR: Could not save figure. Details: {e}") + else: + print("[Pygame] Figure save cancelled.") + + def update(self, objects, dt=0.0001): + + # EVENT POLLING + for event in pygame.event.get(): + if event.type == pygame.QUIT: + pygame.quit() + sys.exit() + # Mouse events (Zoom & Pan) + elif event.type == pygame.MOUSEBUTTONDOWN: + if event.button == 4: self._zoom(0.8, event.pos) + elif event.button == 5: self._zoom(1.25, event.pos) + elif event.button == 1: + self.is_panning = True + self.last_mouse_pos = event.pos + + elif event.type == pygame.MOUSEBUTTONUP: + if event.button == 1: self.is_panning = False + + elif event.type == pygame.MOUSEMOTION: + if getattr(self, 'is_panning', False): + dx = event.pos[0] - self.last_mouse_pos[0] + dy = event.pos[1] - self.last_mouse_pos[1] + self._pan(dx, dy) + self.last_mouse_pos = event.pos + + # Keyboard events (Reset & Save) + elif event.type == pygame.KEYDOWN: + char = event.unicode.lower() + if event.key == pygame.K_r or char in ('r', 'к'): + self.x_limits = list(self.orig_x_limits) + self.y_limits = list(self.orig_y_limits) + elif event.key == pygame.K_s or char in ('s', 'ы'): + self._save_figure() + + # DRAW ENVIRONMENT + self.screen.fill((255, 255, 255)) + self._draw_axes() + + if not objects: + pygame.display.flip() + return + + def draw_tree(obj, q): + parents = obj.model["parent"] + nodes = [] + edges = [] + angles = [0.0] * len(parents) + length = 1.0 + for i in range(len(parents)): + parent = parents[i] + if parent == -1: + x_p, y_p = 0.0, 0.0 + angles[i] = q[i] + else: + x_p, y_p = nodes[parent] + angles[i] = angles[parent] + q[i] + x_child = x_p + length * np.cos(angles[i]) + y_child = y_p + length * np.sin(angles[i]) + nodes.append((x_child, y_child)) + edges.append((x_p, y_p, x_child, y_child)) + return nodes, edges + + # Clipping to ensure drawing only happens within the plot area + plot_rect = pygame.Rect(self.margin_left, self.margin_top, + self.width - self.margin_left - self.margin_right, + self.height - self.margin_top - self.margin_bottom) + self.screen.set_clip(plot_rect) + + # DRAW ROBOT + origin = self._to_screen(0, 0) + for obj in objects: + nodes, edges = draw_tree(obj, obj.q) + for i, edge in enumerate(edges): + x_p, y_p, x_c, y_c = edge + p_screen = self._to_screen(x_p, y_p) + c_screen = self._to_screen(x_c, y_c) + color = self.colors[i % len(self.colors)] + pygame.draw.line(self.screen, color, p_screen, c_screen, 6) + + for node in nodes: + n_screen = self._to_screen(node[0], node[1]) + pygame.draw.circle(self.screen, (173, 216, 230), n_screen, 10) + + x_arrow = self._to_screen(self.x_limits[1] / 20, 0) + y_arrow = self._to_screen(0, self.y_limits[1] / 20) + pygame.draw.line(self.screen, (255, 0, 0), origin, x_arrow, 2) + pygame.draw.line(self.screen, (0, 255, 0), origin, y_arrow, 2) + pygame.draw.circle(self.screen, (0, 0, 255), origin, 8) + + self.screen.set_clip(None) + + pygame.display.flip() + + def close(self): + pygame.quit() \ No newline at end of file diff --git a/tests/test_pygame.py b/tests/test_pygame.py new file mode 100644 index 0000000..eae8750 --- /dev/null +++ b/tests/test_pygame.py @@ -0,0 +1,33 @@ +import time +import sys +import os + +sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), '../src/simulator'))) + +from physics import PhysicsEngine +from pygame_renderer import PygameRenderer +from world import World +from objects import CartPole +from dynamics.ab_algorithm import ABAlgorithm + +def test_pygame_renderer_fps(monkeypatch): + fd_solver = ABAlgorithm() + physics = PhysicsEngine(fd_solver, gravity=[0.0, -9.81, 0.0]) + renderer = PygameRenderer() + world = World(physics, renderer) + world.add_object(CartPole()) + + monkeypatch.setattr("world.time.sleep", lambda dt: None) + + steps = 500 + start_time = time.perf_counter() + world.run(steps) + end_time = time.perf_counter() + + total_time = end_time - start_time + fps = steps / total_time + + renderer.close() + + print(f"\n[Benchmark] Pygame FPS: {fps:.2f}") + assert fps > 100, f"Hiệu năng quá thấp: {fps:.2f} FPS" \ No newline at end of file