feat: astar

This commit is contained in:
Hourglass 2023-05-05 06:13:03 +02:00
parent 04b6664b86
commit d62779cfe8
16 changed files with 203 additions and 84 deletions

View File

@ -1,6 +1,8 @@
import pygame
from Shelf import Shelf
from Grid import Grid
from num_map import num_matrix
HORIZONTAL = 1250
VERTICAL = 750
@ -15,8 +17,18 @@ class Environment:
self.window = window
self.grid = Grid(self.window)
self.Shelf = Shelf(self.window)
self.image = pygame.image.load('resources/texture5.PNG').convert_alpha()
self.rock = pygame.image.load('resources/texture4.PNG').convert_alpha()
self.dirt = pygame.image.load('resources/texture3.PNG').convert_alpha()
def drawEnviroment(self):
self.window.fill(background)
for j in range(0, int(VERTICAL / TILE_SIZE)):
for i in range(0, int(HORIZONTAL / TILE_SIZE)):
if num_matrix[j][i] == 0:
self.window.blit(self.image, (i * TILE_SIZE, j * TILE_SIZE))
elif str(num_matrix[j][i]) in 'r':
self.window.blit(self.rock, (i * TILE_SIZE, j * TILE_SIZE))
elif str(num_matrix[j][i]) in 'd':
self.window.blit(self.dirt, (i * TILE_SIZE, j * TILE_SIZE))
self.Shelf.drawShelves()
self.grid.drawGrid()

View File

@ -1,12 +1,11 @@
import pygame
import numpy as np
HORIZONTAL = 1250
VERTICAL = 750
TILE_SIZE = 50
line_color = (85, 85, 85)
line_color = (0, 0, 0)
class Grid:

View File

@ -1,4 +1,3 @@
import numpy as np
import pygame
import glob2
import random

View File

@ -1,8 +1,7 @@
import pygame
import bfs
import random
import a_star
from num_map import num_matrix
from pygame.locals import *
from Forklift import Forklift
@ -11,14 +10,14 @@ VERTICAL = 750
TILE_SIZE = 50
## 4x6x5
def handle_turn(initial_state, turn_count):
if turn_count % 2 == 0:
commands = bfs.graphsearch(initial_state=initial_state, goal_list=(7, 23))
commands = a_star.a_star(state=initial_state, goal=(7, 23))
else:
row = random.randint(0, 1) * 10 + 2 * random.randint(0, 2)
col = random.randint(1, 5) + 6 * random.randint(0, 3)
commands = bfs.graphsearch(initial_state=initial_state, goal_list=(row, col))
commands = a_star.a_star(state=initial_state, goal=(row, col))
print('I need to go to row: ' + str(row) + ' column: ' + str(col))
return commands
@ -32,69 +31,28 @@ class Program:
self.agent.drawForklift()
running = True
currRow = 7
currCol = 1
currDirec = 1
turn_count = 0
initial_state = bfs.State(row=int(self.agent.y/50), column=int(self.agent.x/50), direction=self.agent.direction)
initial_state = a_star.State(row=int(self.agent.y/50), column=int(self.agent.x/50),
direction=self.agent.direction)
while running:
initial_state.row = int(self.agent.y/50)
initial_state.column = int(self.agent.x/50)
initial_state.direction = self.agent.direction
commands = handle_turn(initial_state=initial_state, turn_count=turn_count)
for event in pygame.event.get():
if event.type == KEYDOWN:
if event.key == K_RIGHT:
if currCol + 1 < 25 and num_matrix[currRow][currCol + 1] == 0:
num_matrix[currRow][currCol] = 0
currCol = currCol + 1
num_matrix[currRow][currCol] = -1
self.agent.moveForkliftRight()
if event.key == K_LEFT:
if currCol - 1 > -1 and num_matrix[currRow][currCol - 1] == 0:
num_matrix[currRow][currCol] = 0
currCol = currCol - 1
num_matrix[currRow][currCol] = -1
self.agent.moveForkliftLeft()
if event.key == K_UP:
if currRow - 1 > -1 and num_matrix[currRow - 1][currCol] == 0:
num_matrix[currRow][currCol] = 0
currRow -= 1
num_matrix[currRow][currCol] = -1
self.agent.moveForkliftUp()
if event.key == K_DOWN:
if currRow + 1 < 15 and num_matrix[currRow + 1][currCol] == 0:
num_matrix[currRow][currCol] = 0
currRow += 1
num_matrix[currRow][currCol] = -1
self.agent.moveForkliftDown()
if event.key == K_ESCAPE:
running = False
elif event.type == pygame.QUIT:
running = False
while commands:
pygame.event.poll()
step = commands.pop(0)
if step == 'rotate_left':
self.agent.rotate_forklift_left()
currDirec = self.agent.direction
elif step == 'rotate_right':
self.agent.rotate_forklift_right()
currDirec = self.agent.direction
elif step == 'go':
self.agent.forklift_move()
currRow = int(self.agent.y/50)
currCol = int(self.agent.x/50)
pygame.time.delay(250)
print('row: ' + str(currRow) + ' col: ' + str(currCol))
print(commands)
turn_count += 1
print(turn_count)
pygame.time.delay(2000)
initial_state.row = int(self.agent.y / 50)
initial_state.column = int(self.agent.x / 50)
initial_state.direction = self.agent.direction

View File

@ -1,5 +1,6 @@
import pygame
import numpy as np
from num_map import num_matrix
HORIZONTAL = 1250
VERTICAL = 750
@ -14,12 +15,10 @@ class Shelf:
self.color = shelf
self.width = 5 * TILE_SIZE
self.height = TILE_SIZE
self.image = pygame.image.load('resources/shelf.PNG').convert_alpha()
def drawShelves(self):
for j in range(0, 5 * TILE_SIZE, 2 * TILE_SIZE):
for i in range(TILE_SIZE + 1, HORIZONTAL - TILE_SIZE, 6 * TILE_SIZE):
pygame.draw.rect(self.window, shelf, pygame.Rect(i, j, self.width, self.height))
for k in range(VERTICAL - 5 * TILE_SIZE, VERTICAL, 2 * TILE_SIZE):
for l in range(TILE_SIZE + 1, HORIZONTAL - TILE_SIZE, 6 * TILE_SIZE):
pygame.draw.rect(self.window, shelf, pygame.Rect(l, k, self.width, self.height))
for j in range(0, int(VERTICAL / TILE_SIZE)):
for i in range(0, int(HORIZONTAL / TILE_SIZE)):
if str(num_matrix[j][i]) in 's':
self.window.blit(self.image, (i * TILE_SIZE, j * TILE_SIZE))

156
a_star.py Normal file
View File

@ -0,0 +1,156 @@
import heapq
from num_map import num_matrix
MAX_ROWS = 15
MAX_COLS = 25
mapping = {
'd': 10,
's': 20,
'r': 999
}
class State:
"""
Directions
UP: 0
RIGHT: 1
DOWN: 2
LEFT: 3
"""
def __init__(self, row, column, direction):
self.direction = direction
self.row = row
self.column = column
def rotate_left(self):
return (self.direction - 1) % 4
def rotate_right(self):
return (self.direction + 1) % 4
def __eq__(self, state: "State"):
return (state.row, state.column, state.direction) == (self.row, self.column, self.direction)
def __lt__(self, state: "State"):
return (self.row, self.column) < (state.row, state.column)
def __hash__(self):
return hash((self.row, self.column))
class Node:
def __init__(self, state: "State", parent=None, action=None, cost=1):
self.state = state
self.parent = parent
self.action = action
self.cost = cost
def __lt__(self, node):
return self.state < node.state
def h(state: State, goal: tuple[int, int]):
"""
Heuristics calculating Manhattan distance
"""
x1, y1 = state.row, state.column
x2, y2 = goal
return abs(x1 - x2) + abs(y1 + y2)
def f(curr_node: Node, goal: tuple[int, int]):
"""
f(n) = g(n) + h(n)
"""
return curr_node.cost + h(state=curr_node.state, goal=goal)
def goal_test(goal_list, state: State):
if (state.row, state.column) == goal_list:
return True
return False
def is_valid_move(target_row, target_column):
if 0 <= target_row < MAX_ROWS and 0 < target_column < MAX_COLS:
return True
return False
def get_successor(state: "State"):
successors = list()
rotate_left = State(row=state.row, column=state.column, direction=state.rotate_left())
rotate_right = State(row=state.row, column=state.column, direction=state.rotate_right())
successors.append(('rotate_left', rotate_left))
successors.append(('rotate_right', rotate_right))
if state.direction == 0:
if is_valid_move(target_row=state.row-1, target_column=state.column):
forward = State(row=state.row-1, column=state.column, direction=state.direction)
successors.append(('go', forward))
elif state.direction == 1:
if is_valid_move(target_row=state.row, target_column=state.column+1):
forward = State(row=state.row, column=state.column+1, direction=state.direction)
successors.append(('go', forward))
elif state.direction == 2:
if is_valid_move(target_row=state.row+1, target_column=state.column):
forward = State(row=state.row+1, column=state.column, direction=state.direction)
successors.append(('go', forward))
elif state.direction == 3:
if is_valid_move(target_row=state.row, target_column=state.column-1):
forward = State(row=state.row, column=state.column-1, direction=state.direction)
successors.append(('go', forward))
return successors
def get_path_from_start(node: Node):
path = [node.action]
while node.parent is not None:
node = node.parent
if node.action:
path.append(node.action)
path.reverse()
return path
def check_cost(row: int, col: int, action):
if action in ('rotate_left', 'rotate_right'):
return 1
else:
return mapping.get(num_matrix[row][col], 1)
def a_star(state: State, goal: tuple[int, int]):
node = Node(state=state, parent=None, action=None)
fringe = list()
heapq.heappush(fringe, (f(node, goal), node))
explored_states = set()
while fringe:
r, node = heapq.heappop(fringe)
if goal_test(goal, node.state):
return get_path_from_start(node)
explored_states.add(node.state)
for successor in get_successor(node.state):
action, next_state = successor
movement_cost = check_cost(row=next_state.row, col=next_state.column, action=action)+node.cost
child = Node(state=next_state, parent=node, action=action, cost=movement_cost)
p = f(child, goal=goal)
if child.state not in explored_states and (p, child) not in fringe:
heapq.heappush(fringe, (p, child))
elif (r, child) in fringe and r > p:
heapq.heappush(fringe, (p, child))

2
bfs.py
View File

@ -32,8 +32,6 @@ class Node:
self.action = action
## Temporarily removed collisons to allow bfs to work without putting in any extra work
## Should be: if 0 <= target_row < MAX_ROWS and 0 < target_column < MAX_COLS and num_matrix[target_row][target_column] not in [1]:
def is_valid_move(num_map, target_row, target_column):
if 0 <= target_row < MAX_ROWS and 0 < target_column < MAX_COLS:
return True

View File

@ -1,5 +1,3 @@
import pygame
from pygame.locals import *
from Program import Program
if __name__ == "__main__":

View File

@ -1,15 +1,15 @@
num_matrix = [[0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, -1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 0]]
num_matrix = [[0, 's', 's', 's', 's', 's', 0, 's', 's', 's', 's', 's', 0, 's', 's', 's', 's', 's', 0, 's', 's', 's', 's', 's', 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 'd', 0, 0, 'd', 0, 0, 0, 'd', 0, 0, 0, 0, 0, 0],
[0, 's', 's', 's', 's', 's', 0, 's', 's', 's', 's', 's', 0, 's', 's', 's', 's', 's', 0, 's', 's', 's', 's', 's', 0],
['d', 0, 0, 0, 0, 0, 0, 'd', 0, 'd', 0, 0, 0, 0, 0, 0, 0, 'd', 0, 0, 0, 'd', 'd', 'd', 'd'],
[0, 's', 's', 's', 's', 's', 'd', 's', 's', 's', 's', 's', 0, 's', 's', 's', 's', 's', 0, 's', 's', 's', 's', 's', 0],
[0, 0, 'd', 0, 0, 0, 0, 0, 0, 0, 'd', 0, 0, 0, 0, 'd', 0, 'd', 0, 0, 0, 0, 0, 0, 0],
[0, 'd', 0, 0, 'd', 0, 0, 'd', 0, 0, 0, 'd', 0, 0, 0, 0, 'd', 0, 'd', 'd', 'd', 0, 'r', 'r', 0],
['d', 0, 'd', 0, 0, 0, 0, 0, 'd', 0, 'd', 0, 0, 'd', 0, 'd', 'd', 'd', 'd', 0, 0, 0, 'r', 0, 0],
[0, 0, 0, 0, 0, 'd', 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 'd', 0, 0, 0, 0, 'r', 'r', 0],
['d', 0, 0, 0, 0, 0, 0, 'd', 'd', 0, 0, 'd', 0, 0, 0, 'd', 0, 0, 0, 'd', 0, 0, 0, 0, 0],
[0, 's', 's', 's', 's', 's', 0, 's', 's', 's', 's', 's', 0, 's', 's', 's', 's', 's', 0, 's', 's', 's', 's', 's', 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 'd', 'd', 'd', 0, 0, 'd', 0, 'd', 0, 0],
['d', 's', 's', 's', 's', 's', 0, 's', 's', 's', 's', 's', 0, 's', 's', 's', 's', 's', 0, 's', 's', 's', 's', 's', 0],
[0, 0, 0, 0, 0, 0, 0, 0, 'd', 0, 0, 0, 0, 'd', 0, 0, 0, 0, 0, 0, 'd', 0, 0, 0, 0],
[0, 's', 's', 's', 's', 's', 0, 's', 's', 's', 's', 's', 0, 's', 's', 's', 's', 's', 0, 's', 's', 's', 's', 's', 0]]

BIN
resources/shelf.PNG Normal file

Binary file not shown.

After

Width:  |  Height:  |  Size: 1.0 KiB

BIN
resources/texture1.PNG Normal file

Binary file not shown.

After

Width:  |  Height:  |  Size: 3.5 KiB

BIN
resources/texture2.PNG Normal file

Binary file not shown.

After

Width:  |  Height:  |  Size: 5.2 KiB

BIN
resources/texture3.PNG Normal file

Binary file not shown.

After

Width:  |  Height:  |  Size: 7.0 KiB

BIN
resources/texture4.PNG Normal file

Binary file not shown.

After

Width:  |  Height:  |  Size: 2.6 KiB

BIN
resources/texture5.PNG Normal file

Binary file not shown.

After

Width:  |  Height:  |  Size: 1.4 KiB

BIN
resources/texture6.PNG Normal file

Binary file not shown.

After

Width:  |  Height:  |  Size: 2.5 KiB