Final A*
This commit is contained in:
parent
019c38c344
commit
88eff2c4b7
BIN
src/__pycache__/agent.cpython-311.pyc
Normal file
BIN
src/__pycache__/agent.cpython-311.pyc
Normal file
Binary file not shown.
BIN
src/__pycache__/simulation.cpython-311.pyc
Normal file
BIN
src/__pycache__/simulation.cpython-311.pyc
Normal file
Binary file not shown.
291
src/agent.py
291
src/agent.py
@ -3,6 +3,20 @@ import pygame as pg
|
||||
import heapq
|
||||
|
||||
|
||||
class Node:
|
||||
def __init__(self, state, parent, cost):
|
||||
self.state = state
|
||||
self.parent = parent
|
||||
self.cost = cost
|
||||
|
||||
def __lt__(self, other):
|
||||
return self.cost < other.cost
|
||||
|
||||
class State:
|
||||
def __init__(self, position, house_list):
|
||||
self.position = position
|
||||
self.house_list = house_list
|
||||
|
||||
def vector_to_tuple(vector):
|
||||
tup = (int(vector.x), int(vector.y))
|
||||
return tup
|
||||
@ -27,43 +41,42 @@ class Agent:
|
||||
self.weights = {}
|
||||
self.orientation = 90
|
||||
|
||||
|
||||
# utworzenie grafu dróg
|
||||
roads_pos = [vector_to_tuple(pos) for pos in self.simulation.state.road_pos_g + self.simulation.state.road_pos_r]
|
||||
roads_pos = [vector_to_tuple(pos) for pos in
|
||||
self.simulation.state.road_pos_g + self.simulation.state.road_pos_r]
|
||||
roads_pos.sort()
|
||||
for index, pos in enumerate(roads_pos):
|
||||
if pos[0] < 0 or pos[0] >= 27 or pos[1] < 0 or pos[1] >= 14:
|
||||
continue
|
||||
for another_pos in roads_pos[index:]:
|
||||
if pos == another_pos or another_pos[0] < 0 or another_pos[0] >= 27 or another_pos[1] < 0 or another_pos[1] >= 14:
|
||||
if pos == another_pos or another_pos[0] < 0 or another_pos[0] >= 27 or another_pos[1] < 0 or \
|
||||
another_pos[1] >= 14:
|
||||
continue
|
||||
if ((abs(pos[0] - another_pos[0]) == 1 and abs(pos[1] - another_pos[1]) == 0) or (abs(pos[0] - another_pos[0]) == 0 and abs(pos[1] - another_pos[1]) == 1)):
|
||||
if ((abs(pos[0] - another_pos[0]) == 1 and abs(pos[1] - another_pos[1]) == 0) or (
|
||||
abs(pos[0] - another_pos[0]) == 0 and abs(pos[1] - another_pos[1]) == 1)):
|
||||
if pos not in self.graph.keys():
|
||||
self.graph[pos] = set()
|
||||
if another_pos not in self.graph.keys():
|
||||
self.graph[another_pos] = set()
|
||||
if another_pos in self.simulation.state.road_pos_r:
|
||||
if another_pos in self.simulation.state.road_pos_r:
|
||||
weight = 1
|
||||
elif another_pos in self.simulation.state.road_pos_g:
|
||||
weight = 3
|
||||
weight = 100
|
||||
else:
|
||||
weight = 1
|
||||
self.graph[pos].add(another_pos)
|
||||
self.graph[another_pos].add(pos)
|
||||
self.weights[(pos,another_pos)] = weight
|
||||
self.weights[(pos, another_pos)] = weight
|
||||
self.weights[(another_pos, pos)] = weight
|
||||
|
||||
|
||||
|
||||
|
||||
# dołączenie domów i składowisk do grafu dróg
|
||||
entities = self.simulation.state.entities
|
||||
for entity in entities:
|
||||
entity_pos = vector_to_tuple(entity.position)
|
||||
for neighbour_pos in [(entity_pos[0]-1, entity_pos[1]),
|
||||
(entity_pos[0]+1, entity_pos[1]),
|
||||
(entity_pos[0], entity_pos[1]-1),
|
||||
(entity_pos[0], entity_pos[1]+1)]:
|
||||
for neighbour_pos in [(entity_pos[0] - 1, entity_pos[1]),
|
||||
(entity_pos[0] + 1, entity_pos[1]),
|
||||
(entity_pos[0], entity_pos[1] - 1),
|
||||
(entity_pos[0], entity_pos[1] + 1)]:
|
||||
if neighbour_pos[0] < 0 or neighbour_pos[0] >= 27 or neighbour_pos[1] < 0 or neighbour_pos[1] >= 14:
|
||||
continue
|
||||
if neighbour_pos in roads_pos:
|
||||
@ -77,10 +90,9 @@ class Agent:
|
||||
self.dumps[entity.trash_type] = vector_to_tuple(entity.position)
|
||||
if entity.entity_type == 'house':
|
||||
self.houses[vector_to_tuple(entity.position)] = HousePOI()
|
||||
|
||||
self.path = self.A_star()
|
||||
|
||||
self.path = self.a_star()
|
||||
pass
|
||||
|
||||
|
||||
def update(self):
|
||||
entities = self.simulation.state.entities
|
||||
@ -90,183 +102,130 @@ class Agent:
|
||||
self.fullness = entity.fullness
|
||||
self.orientation = entity.orientation
|
||||
|
||||
|
||||
def decide_move(self):
|
||||
move_a = self.path[0]
|
||||
move = (move_a[0] - self.current_pos[0], move_a[1] - self.current_pos[1])
|
||||
if self.path:
|
||||
move_a = self.path[0]
|
||||
move = (move_a[0] - self.current_pos[0], move_a[1] - self.current_pos[1])
|
||||
|
||||
if self.orientation == 0:
|
||||
if move[0] == 0:
|
||||
if move[1] == -1:
|
||||
self.path.pop(0)
|
||||
return pg.Vector2(move)
|
||||
else:
|
||||
return 180
|
||||
elif move[1] == 0:
|
||||
if move[0] == -1:
|
||||
return 270
|
||||
else:
|
||||
return 90
|
||||
elif self.orientation == 90:
|
||||
if move[0] == 0:
|
||||
if move[1] == -1:
|
||||
return 0
|
||||
else:
|
||||
return 180
|
||||
elif move[1] == 0:
|
||||
if move[0] == -1:
|
||||
return 270
|
||||
else:
|
||||
self.path.pop(0)
|
||||
return pg.Vector2(move)
|
||||
elif self.orientation == 180:
|
||||
if move[0] == 0:
|
||||
if move[1] == -1:
|
||||
return 0
|
||||
else:
|
||||
self.path.pop(0)
|
||||
return pg.Vector2(move)
|
||||
elif move[1] == 0:
|
||||
if move[0] == -1:
|
||||
return 270
|
||||
else:
|
||||
return 90
|
||||
if self.orientation == 0:
|
||||
if move[0] == 0:
|
||||
if move[1] == -1:
|
||||
self.path.pop(0)
|
||||
return pg.Vector2(move)
|
||||
else:
|
||||
return 180
|
||||
elif move[1] == 0:
|
||||
if move[0] == -1:
|
||||
return 270
|
||||
else:
|
||||
return 90
|
||||
elif self.orientation == 90:
|
||||
if move[0] == 0:
|
||||
if move[1] == -1:
|
||||
return 0
|
||||
else:
|
||||
return 180
|
||||
elif move[1] == 0:
|
||||
if move[0] == -1:
|
||||
return 270
|
||||
else:
|
||||
self.path.pop(0)
|
||||
return pg.Vector2(move)
|
||||
elif self.orientation == 180:
|
||||
if move[0] == 0:
|
||||
if move[1] == -1:
|
||||
return 0
|
||||
else:
|
||||
self.path.pop(0)
|
||||
return pg.Vector2(move)
|
||||
elif move[1] == 0:
|
||||
if move[0] == -1:
|
||||
return 270
|
||||
else:
|
||||
return 90
|
||||
else:
|
||||
if move[0] == 0:
|
||||
if move[1] == -1:
|
||||
return 0
|
||||
else:
|
||||
return 180
|
||||
elif move[1] == 0:
|
||||
if move[0] == -1:
|
||||
self.path.pop(0)
|
||||
return pg.Vector2(move)
|
||||
else:
|
||||
return 90
|
||||
else:
|
||||
if move[0] == 0:
|
||||
if move[1] == -1:
|
||||
return 0
|
||||
else:
|
||||
return 180
|
||||
elif move[1] == 0:
|
||||
if move[0] == -1:
|
||||
self.path.pop(0)
|
||||
return pg.Vector2(move)
|
||||
else:
|
||||
return 90
|
||||
|
||||
def heuristic(self, start, end):
|
||||
return abs((end[0][0] - start[0][0])) + abs((end[0][1] - start[0][1])) // (abs(len(start[1]) - len(end[1])) + 1)
|
||||
return pg.Vector2(0, 0)
|
||||
|
||||
def weight_cost(self, start_pos, end_pos):
|
||||
return self.weights[(start_pos, end_pos)]
|
||||
|
||||
def get_move_cost(self, start_pos, end_pos):
|
||||
return self.heuristic(start_pos, end_pos) + self.weight_cost(start_pos, end_pos)
|
||||
|
||||
def get_start_state(self):
|
||||
# entities = self.simulation.state.entities
|
||||
# orientation - self.orientation
|
||||
position = self.current_pos
|
||||
house_list = tuple(self.houses)
|
||||
start_state = (position, house_list)
|
||||
return start_state
|
||||
|
||||
def get_end_state(self):
|
||||
position = (0,1)
|
||||
house_list = ()
|
||||
end_state = (position, house_list)
|
||||
return end_state
|
||||
|
||||
def succesor(self, state):
|
||||
def successors(self, state):
|
||||
successors_pos = self.graph[state[0]]
|
||||
house_list = state[1]
|
||||
successors = ()
|
||||
successors = []
|
||||
for pos in successors_pos:
|
||||
if pos in house_list:
|
||||
house_list = list(house_list)
|
||||
house_list.remove(pos)
|
||||
house_list = tuple(house_list)
|
||||
|
||||
successors = list(successors)
|
||||
successors.append((pos, tuple(house_list)))
|
||||
successors = tuple(successors)
|
||||
else:
|
||||
successors = list(successors)
|
||||
successors.append((pos, tuple(house_list)))
|
||||
successors = tuple(successors)
|
||||
successors.append((pos, tuple(house_list)))
|
||||
|
||||
return successors
|
||||
|
||||
def cost(self, current_node, succ_state):
|
||||
if succ_state[0] not in current_node.state[1]:
|
||||
cost = current_node.cost + self.weights[current_node.state[0], succ_state[0]]
|
||||
else:
|
||||
cost = float(1)
|
||||
return cost
|
||||
|
||||
'''def A_star(self):
|
||||
def heuristic(self, succ_state, goal_state):
|
||||
position = succ_state[0]
|
||||
if len(succ_state[1]) != 0:
|
||||
estimated_cost = 0
|
||||
for house in succ_state[1]:
|
||||
estimated_cost += abs(position[0] - house[0]) + abs(position[1] - house[1])
|
||||
else:
|
||||
goal_position = goal_state[0]
|
||||
estimated_cost = abs(position[0] - goal_position[0]) + abs(position[1] - goal_position[1])
|
||||
return estimated_cost
|
||||
|
||||
def a_star(self):
|
||||
fringe = []
|
||||
explored = set()
|
||||
istate = self.get_start_state()
|
||||
goaltest = self.get_end_state()
|
||||
node = (istate, None)
|
||||
actions = []
|
||||
initial_state = (self.current_pos, tuple(self.houses))
|
||||
goal_state = (self.current_pos, ())
|
||||
start_node = Node(initial_state, None, 0)
|
||||
|
||||
heapq.heappush(fringe, (self.heuristic(istate[0], goaltest[0]), node))
|
||||
heapq.heappush(fringe, start_node)
|
||||
|
||||
while fringe:
|
||||
_, el = heapq.heappop(fringe)
|
||||
elem = el[0]
|
||||
parent = el[1]
|
||||
current_node = heapq.heappop(fringe)
|
||||
|
||||
if elem == goaltest:
|
||||
while parent is not None:
|
||||
actions.append(elem[0])
|
||||
elem = parent
|
||||
actions.reverse()
|
||||
return actions
|
||||
if current_node.state == goal_state:
|
||||
actions = []
|
||||
while current_node:
|
||||
actions.append(current_node.state[0])
|
||||
current_node = current_node.parent
|
||||
c = (4, 0) in actions
|
||||
return list(reversed(actions))
|
||||
|
||||
explored.add(elem)
|
||||
explored.add(current_node.state)
|
||||
|
||||
succs = self.succesor(elem)
|
||||
for succ in succs:
|
||||
succ_states = self.successors(current_node.state)
|
||||
|
||||
node = (succ, elem)
|
||||
p = self.heuristic(succ[0], goaltest[0]) + self.weight_cost(elem[0], succ[0])
|
||||
for succ_state in succ_states:
|
||||
if succ_state in explored:
|
||||
continue
|
||||
|
||||
if succ not in explored and not any(tup[1][0] == succ for tup in fringe):
|
||||
heapq.heappush(fringe, (p,node))
|
||||
elif any(tup[1] == succ for tup in fringe):
|
||||
i = next(i for node in enumerate(fringe) if node[1][0] == succ)
|
||||
if fringe[i][0] > p:
|
||||
fringe[i] = (p,node)
|
||||
return actions'''
|
||||
g = self.cost(current_node, succ_state)
|
||||
h = self.heuristic(succ_state, goal_state)
|
||||
succ_cost = g + h
|
||||
succ_node = Node(succ_state, current_node, succ_cost)
|
||||
|
||||
def A_star(self):
|
||||
fringe = []
|
||||
explored = set()
|
||||
istate = self.get_start_state()
|
||||
goaltest = self.get_end_state()
|
||||
node = (istate, None)
|
||||
actions = []
|
||||
|
||||
heapq.heappush(fringe, (self.heuristic(istate, goaltest), node))
|
||||
|
||||
while fringe:
|
||||
_, el = heapq.heappop(fringe)
|
||||
elem = el[0]
|
||||
parent = el[1]
|
||||
|
||||
if elem == goaltest:
|
||||
while parent is not None:
|
||||
actions.append(elem[0])
|
||||
elem = parent[0]
|
||||
parent = parent[1]
|
||||
actions.reverse()
|
||||
return actions
|
||||
|
||||
explored.add(elem)
|
||||
|
||||
succs = self.succesor(elem)
|
||||
for succ in succs:
|
||||
node = (succ, el)
|
||||
heuristic = self.heuristic(succ, goaltest)
|
||||
cost = 7*self.weight_cost(elem[0], succ[0])
|
||||
p = heuristic + cost
|
||||
|
||||
if succ not in explored and not any(tup[1][0] == succ for tup in fringe):
|
||||
heapq.heappush(fringe, (p, node))
|
||||
elif any(tup[1][0] == succ for tup in fringe):
|
||||
i = next(i for i, node in enumerate(fringe) if node[1][0] == succ)
|
||||
if fringe[i][0] > p:
|
||||
fringe[i] = (p, node)
|
||||
|
||||
return actions
|
||||
heapq.heappush(fringe, succ_node)
|
||||
|
||||
return None
|
||||
|
||||
def discover(self):
|
||||
if self.current_pos in self.houses.keys():
|
||||
|
||||
Loading…
Reference in New Issue
Block a user