In einer 2d Karte ist ein Roboter mit einem vision cone unterwegs, der Müllobjekte kartieren soll. Es gibt einen manuellen Modus wo der User den Roboter mittels Pfeiltasten steuert und einen KI Modus der mit [k] aktiviert wird.
Der Roboter gibt die Statusinformationen einmal als Textnachrichten auf dem Bildschirm aus und zusätzlich noch als neural encoding, dieser ist ein feature vector mit 10 numerischen Werten. Im KI Modus wird dieser Feature vector verwendet um die Aktionen des Roboter zu planen.
import sys
import math
import random
import numpy as np
import pygame
# --- KONFIGURATION & FARBEN ---
GRID_WIDTH = 30
GRID_HEIGHT = 20
CELL_SIZE = 30
GRID_PIXEL_W = GRID_WIDTH * CELL_SIZE
GRID_PIXEL_H = GRID_HEIGHT * CELL_SIZE
UI_HEIGHT = 180
WINDOW_WIDTH = GRID_PIXEL_W
WINDOW_HEIGHT = GRID_PIXEL_H + UI_HEIGHT
# Farben (RGB)
COLOR_BG = (30, 30, 35)
COLOR_WALL = (50, 50, 60)
COLOR_TRASH = (210, 45, 45)
COLOR_UNKNOWN = (100, 100, 110)
COLOR_MAPPED_FREE = (240, 240, 245)
COLOR_MAPPED_TRASH = (255, 140, 0)
COLOR_ROBOT = (30, 144, 255)
COLOR_VISION = (255, 255, 0, 60)
# Neue UI-Farben (Weißer Hintergrund, schwarzer Text)
COLOR_UI_BG = (255, 255, 255)
COLOR_UI_TEXT = (0, 0, 0)
# Zell-Typen
EMPTY = 0
WALL = 1
TRASH = 2
class Robot:
def __init__(self, x, y, grid_w, grid_h):
self.x = x
self.y = y
self.grid_w = grid_w
self.grid_h = grid_h
self.orientation = 0 # 0=Rechts, 90=Unten, 180=Links, 270=Oben
self.fov_angle = 90
self.fov_range = 5
def move(self, dx, dy, grid):
new_x = self.x + dx
new_y = self.y + dy
if dx == 1:
self.orientation = 0
elif dx == -1:
self.orientation = 180
elif dy == 1:
self.orientation = 90
elif dy == -1:
self.orientation = 270
if (
0 <= new_x < self.grid_w
and 0 <= new_y < self.grid_h
and grid[new_y][new_x] != WALL
):
self.x = new_x
self.y = new_y
return True
return False
class Environment:
def __init__(self, w, h):
self.w = w
self.h = h
self.grid = np.zeros((h, w), dtype=int)
self.generate_map()
def generate_map(self):
self.grid[0, :] = WALL
self.grid[-1, :] = WALL
self.grid[:, 0] = WALL
self.grid[:, -1] = WALL
self.grid[4:14, 8] = WALL
self.grid[6, 8:18] = WALL
self.grid[12:18, 20] = WALL
trash_positions = [
(3, 3),
(5, 12),
(10, 15),
(15, 4),
(22, 8),
(25, 15),
(12, 2),
(18, 16),
]
for tx, ty in trash_positions:
self.grid[ty, tx] = TRASH
class RobotSystem:
def __init__(self):
pygame.init()
pygame.display.set_caption(
"2D Robot Mapping & Neural Encoding Simulation"
)
self.screen = pygame.display.set_mode((WINDOW_WIDTH, WINDOW_HEIGHT))
self.clock = pygame.time.Clock()
self.font = pygame.font.SysFont("Arial", 16, bold=False)
self.env = Environment(GRID_WIDTH, GRID_HEIGHT)
self.robot = Robot(2, 2, GRID_WIDTH, GRID_HEIGHT)
self.robot_map = np.full((GRID_HEIGHT, GRID_WIDTH), -1, dtype=int)
self.status_message = "SYS_INIT: ROBOT ONLINE."
self.feature_vector = np.zeros(10)
# KI Steuerung
self.ai_mode = False
self.ai_move_timer = 0
def compute_vision_and_map(self):
visible_cells = set()
trash_in_fov = []
start_angle = self.robot.orientation - (self.robot.fov_angle / 2)
end_angle = self.robot.orientation + (self.robot.fov_angle / 2)
for angle_deg in np.linspace(start_angle, end_angle, num=30):
rad = math.radians(angle_deg)
dx = math.cos(rad)
dy = math.sin(rad)
for step in range(1, self.robot.fov_range + 1):
cx = int(round(self.robot.x + dx * step))
cy = int(round(self.robot.y + dy * step))
if 0 <= cx < GRID_WIDTH and 0 <= cy < GRID_HEIGHT:
visible_cells.add((cx, cy))
cell_val = self.env.grid[cy][cx]
self.robot_map[cy][cx] = cell_val
if cell_val == TRASH:
dist = math.hypot(cx - self.robot.x, cy - self.robot.y)
trash_in_fov.append((cx, cy, dist))
if cell_val == WALL:
break
else:
break
self.robot_map[self.robot.y][self.robot.x] = EMPTY
visible_cells.add((self.robot.x, self.robot.y))
return visible_cells, trash_in_fov
def update_neural_encoding(self, trash_in_fov):
"""Erzeugt den 10-dimensionalen semantischen Feature-Vektor."""
v = np.zeros(10)
# v0, v1: Position (normiert)
v[0] = round(self.robot.x / (GRID_WIDTH - 1), 2)
v[1] = round(self.robot.y / (GRID_HEIGHT - 1), 2)
# v2-v5: Hindernis-Nähe (Vorne, Rechts, Hinten, Links) -> 1.0 = Wand nah
dirs = [
self.robot.orientation,
(self.robot.orientation + 90) % 360,
(self.robot.orientation + 180) % 360,
(self.robot.orientation + 270) % 360,
]
for i, d in enumerate(dirs):
rad = math.radians(d)
dx, dy = int(round(math.cos(rad))), int(round(math.sin(rad)))
dist = 0
cx, cy = self.robot.x, self.robot.y
while True:
cx += dx
cy += dy
dist += 1
if (
not (0 <= cx < GRID_WIDTH and 0 <= cy < GRID_HEIGHT)
or self.env.grid[cy][cx] == WALL
):
break
v[2 + i] = round(1.0 / max(dist, 1), 2)
# v6: Müll im Sichtfeld (0.0 oder 1.0)
v[6] = 1.0 if len(trash_in_fov) > 0 else 0.0
# v7: Distanz/Nähe zum Müll (1.0 = sehr nah, 0.0 = weit weg/keiner)
if len(trash_in_fov) > 0:
min_dist = min([t[2] for t in trash_in_fov])
v[7] = round(max(0.0, 1.0 - (min_dist / self.robot.fov_range)), 2)
else:
v[7] = 0.0
# v8: Kartierungsfortschritt
mapped_count = np.sum(self.robot_map != -1)
v[8] = round(mapped_count / (GRID_WIDTH * GRID_HEIGHT), 2)
# v9: Müll-Dichte / Erfassungsquote
found_trash = np.sum(self.robot_map == TRASH)
v[9] = round(min(1.0, found_trash / max(1, mapped_count * 0.1)), 2)
self.feature_vector = v
def ai_decide_move(self):
"""KI-Entscheidung: Greift AUSSCHLIESSLICH auf den Feature-Vektor zu."""
vec = self.feature_vector
# Mögliche Bewegungsrichtungen: [(dx, dy), Orientierung]
moves = [
((1, 0), 0), # Rechts
((0, 1), 90), # Unten
((-1, 0), 180), # Links
((0, -1), 270), # Oben
]
valid_moves = []
for (dx, dy), orient in moves:
# Zuordnung der Vektor-Wände (v2..v5) zur relativen Ausrichtung
rel_angle = (orient - self.robot.orientation) % 360
idx = int(rel_angle // 90) + 2
# Wenn die Wand nicht direkt davor steht (v < 1.0 ist frei)
if vec[idx] < 1.0:
valid_moves.append(((dx, dy), orient, vec[idx]))
if not valid_moves:
return
# STRATEGIE 1: Müll jagen (wenn v6 == 1.0)
if vec[6] == 1.0:
best_move = None
best_score = -999
for (dx, dy), orient, wall_proximity in valid_moves:
# Teste virtuell, wie sich v7 verändern würde
# Ausrichtungsauswertung in Richtung Müll
score = -wall_proximity * 2.0
if orient == self.robot.orientation:
score += 2.0 # Bevorzuge Vorwärtsbewegung zum Müll
if score > best_score:
best_score = score
best_move = (dx, dy)
if best_move:
self.robot.move(best_move[0], best_move[1], self.env.grid)
return
# STRATEGIE 2: Karte erkunden (Wänden ausweichen, geradeaus bevorzugen)
best_move = None
best_score = -999
for (dx, dy), orient, wall_proximity in valid_moves:
# Score: Viel Platz (geringer Wall-Proximity Wert) + Kontinuität
score = (1.0 - wall_proximity) * 3.0
if orient == self.robot.orientation:
score += 1.5 # Vorwärtsdrang
score += random.uniform(0.0, 0.5) # Zufallskomponente gegen Schleifen
if score > best_score:
best_score = score
best_move = (dx, dy)
if best_move:
self.robot.move(best_move[0], best_move[1], self.env.grid)
def update_telemetry(self, trash_in_fov):
mode_str = "AI-MODE (AUTONOMOUS)" if self.ai_mode else "MANUAL (KEYBOARD)"
msg_parts = [f"MODE: {mode_str}", f"POS:[{self.robot.x},{self.robot.y}]"]
if trash_in_fov:
closest = min(trash_in_fov, key=lambda item: item[2])
msg_parts.append(
f"ALERT: TRASH DETECTED AT [{closest[0]},{closest[1]}]"
)
else:
msg_parts.append("SCANNING... NO TRASH IN FOV")
total_trash_mapped = np.sum(self.robot_map == TRASH)
msg_parts.append(f"MAPPED TRASH TOTAL: {total_trash_mapped}")
self.status_message = " | ".join(msg_parts)
def draw(self, visible_cells):
self.screen.fill(COLOR_BG)
# 1. Gitter & Map zeichnen
for y in range(GRID_HEIGHT):
for x in range(GRID_WIDTH):
rect = pygame.Rect(
x * CELL_SIZE, y * CELL_SIZE, CELL_SIZE, CELL_SIZE
)
mapped_val = self.robot_map[y][x]
real_val = self.env.grid[y][x]
if mapped_val == -1:
color = COLOR_UNKNOWN
elif mapped_val == WALL:
color = COLOR_WALL
elif mapped_val == TRASH:
color = COLOR_MAPPED_TRASH
else:
color = COLOR_MAPPED_FREE
pygame.draw.rect(self.screen, color, rect)
if (x, y) in visible_cells and real_val == TRASH:
pygame.draw.circle(
self.screen, COLOR_TRASH, rect.center, CELL_SIZE // 3
)
pygame.draw.rect(self.screen, (50, 50, 50), rect, 1)
# 2. Vision Cone
rx_pix = self.robot.x * CELL_SIZE + CELL_SIZE // 2
ry_pix = self.robot.y * CELL_SIZE + CELL_SIZE // 2
cone_surface = pygame.Surface(
(GRID_PIXEL_W, GRID_PIXEL_H), pygame.SRCALPHA
)
for cx, cy in visible_cells:
c_rect = pygame.Rect(
cx * CELL_SIZE, cy * CELL_SIZE, CELL_SIZE, CELL_SIZE
)
pygame.draw.rect(cone_surface, COLOR_VISION, c_rect)
self.screen.blit(cone_surface, (0, 0))
# 3. Roboter
pygame.draw.circle(
self.screen, COLOR_ROBOT, (rx_pix, ry_pix), CELL_SIZE // 2 - 2
)
rad = math.radians(self.robot.orientation)
end_x = rx_pix + math.cos(rad) * (CELL_SIZE // 2)
end_y = ry_pix + math.sin(rad) * (CELL_SIZE // 2)
pygame.draw.line(
self.screen, (255, 255, 255), (rx_pix, ry_pix), (end_x, end_y), 3
)
# 4. UI / Textfeld (WEISSER HINTERGRUND & SCHWARZER TEXT)
ui_rect = pygame.Rect(0, GRID_PIXEL_H, WINDOW_WIDTH, UI_HEIGHT)
pygame.draw.rect(self.screen, COLOR_UI_BG, ui_rect)
pygame.draw.line(
self.screen,
(200, 200, 200),
(0, GRID_PIXEL_H),
(WINDOW_WIDTH, GRID_PIXEL_H),
2,
)
# Statuszeile
txt_surface = self.font.render(
f"> STATUS: {self.status_message}", True, COLOR_UI_TEXT
)
self.screen.blit(txt_surface, (15, GRID_PIXEL_H + 15))
# Neural Vector Zeile
vec_title = self.font.render(
"> NEURAL FEATURE VECTOR [v0..v9]:", True, COLOR_UI_TEXT
)
self.screen.blit(vec_title, (15, GRID_PIXEL_H + 45))
vec_str = np.array2string(
self.feature_vector, precision=2, suppress_small=True
)
vec_surface = self.font.render(
f" {vec_str}", True, COLOR_UI_TEXT
)
self.screen.blit(vec_surface, (15, GRID_PIXEL_H + 65))
# Vektor-Legende
legend = "[PosX, PosY, DistN, DistE, DistS, DistW, TrashVisible, TrashDist, MapProgress, TrashDensity]"
leg_surface = self.font.render(f" {legend}", True, COLOR_UI_TEXT)
self.screen.blit(leg_surface, (15, GRID_PIXEL_H + 90))
# Steuerungshinweis
ctrl_str = "Steuerung: Pfeiltasten (Manuell) | Taste 'K' drücken (KI-Modus Umschalten)"
ctrl_surface = self.font.render(ctrl_str, True, COLOR_UI_TEXT)
self.screen.blit(ctrl_surface, (15, GRID_PIXEL_H + 130))
pygame.display.flip()
def run(self):
running = True
while running:
self.clock.tick(30)
self.ai_move_timer += 1
for event in pygame.event.get():
if event.type == pygame.QUIT:
running = False
elif event.type == pygame.KEYDOWN:
if event.key == pygame.K_k:
# KI-Modus umschalten
self.ai_mode = not self.ai_mode
elif not self.ai_mode:
# Manuelle Steuerung nur, wenn KI aus ist
if event.key == pygame.K_UP:
self.robot.move(0, -1, self.env.grid)
elif event.key == pygame.K_DOWN:
self.robot.move(0, 1, self.env.grid)
elif event.key == pygame.K_LEFT:
self.robot.move(-1, 0, self.env.grid)
elif event.key == pygame.K_RIGHT:
self.robot.move(1, 0, self.env.grid)
# KI-Schritt ausführen (alle 5 Frames für eine flüssige Bewegung)
if self.ai_mode and self.ai_move_timer >= 5:
self.ai_decide_move()
self.ai_move_timer = 0
# Sensorik & System-Updates
visible_cells, trash_in_fov = self.compute_vision_and_map()
self.update_neural_encoding(trash_in_fov)
self.update_telemetry(trash_in_fov)
# Zeichnen
self.draw(visible_cells)
pygame.quit()
sys.exit()
if __name__ == "__main__":
sim = RobotSystem()
sim.run()
No comments:
Post a Comment