#Importación de librerías
import math #Usada para representar el infinito en el coste inicial
import cv2 #Usada para la creación de imagenes
import numpy as np #Usada enn conjunto con la anterior para crear la matriz inicial para obtener la imagen
import imageio #Usada para la creación de GIFs animados
from IPython.display import display, Image #Usada para mostrar en línea dichos GIFs
class Inventario:
def __init__(self, x, y, x_goal, y_goal, etiqueta='none'):
self.x = x
self.y = y
self.x_goal = x_goal
self.y_goal = y_goal
self.etiqueta = etiqueta
self.arrived = False
class Pared:
def __init__(self, x, y):
self.x = x
self.y = y
class Robot:
def __init__(self, x, y, loaded=False):
self.x = x
self.y = y
self.loaded = loaded
class Almacen:
def __init__(self, w, h, paredes, inventarios, objetivos, robot=Robot(0,0)):
self.w = w
self.h = h
self.paredes = paredes
self.inventarios = inventarios
self.objetivos = objetivos
self.robot = robot
class Accion:
def __init__(self, x, y, i, descripcion=''):
self.x = x
self.y = y
self.i = i
self.descripcion = descripcion
class Nodo:
def __init__(self, x, y, last):
self.x = x
self.y = y
self.g = math.inf
self.f = math.inf
self.last = last
def me(self):
return 'x: '+str(self.x)+' y: '+str(self.y)+' g: '+str(self.g)
def h(inicio, fin):
sol = []
for f in fin:
sol.append(abs(inicio.x - f.x) + abs(inicio.y - f.y))
sol.sort()
return sol[0]
def isInventarioArrived(obj):
if type(obj) == Inventario:
return obj.arrived
return False
def isGoal(obj1, obj2):
for o in obj2:
if obj1.x == o.x and obj1.y == o.y and not isInventarioArrived(o):
return True
return False
def camino(obj, goals):
txt = []
num = obj.g
inv = Inventario(0, 0, 0, 0, 'error')
#Encontrar inventario recogido
for g in goals:
if g.x == obj.x and g.y == obj.y:
inv = g
#Encontrar camino de vuelta
for i in range(num+1):
if obj.last != 'inicio':
txt.append(Accion(obj.x, obj.y, num-i, 'ir a'))
obj = obj.last
else:
return txt, inv
return txt, inv
def isWall(warehouse, obj):
#Comprobar si el robot está cargado, entonces considerar otros inventarios como pared
if warehouse.robot.loaded:
for i in warehouse.inventarios:
if obj.x == i.x and obj.y == i.y:
return True
#Comprobar paredes
for wall in warehouse.paredes:
if obj.x == wall.x and obj.y == wall.y:
return True
return False
def expandir(warehouse, obj):
#Expande en el sentido de las agujas del reloj, empezando desde el frente (restando Y)
nodoTemp = []
#Arriba
nodoTemp.append(Nodo(obj.x, obj.y - 1, obj))
#Derecha
nodoTemp.append(Nodo(obj.x + 1, obj.y, obj))
#Abajo
nodoTemp.append(Nodo(obj.x, obj.y + 1, obj))
#Izquierda
nodoTemp.append(Nodo(obj.x - 1, obj.y, obj))
#Filtrar resultados
filtrado = nodoTemp.copy()
for nod in filtrado:
#Comprobar que no salga fuera de los límites del almacén o colisione con una pared
if nod.x < 0 or nod.x >= warehouse.w or nod.y < 0 or nod.y >= warehouse.h or isWall(warehouse, nod):
nodoTemp.remove(nod)
#Devolver lista filtrada
return nodoTemp
def isIn(obj, lst):
for l in lst:
if obj.x == l.x and obj.y == l.y:
return True
return False
def getF(obj):
return obj.f
def getI(obj):
return obj.i
def busquedaAStar(warehouse, start, goals):
start.g = 0
start.f = h(start, goals)
colaAbierta = [start]
colaCerrada = []
while len(colaAbierta) > 0:
#Extraer el menor f de colaAbierta
colaAbierta.sort(key = getF) #Ordenar basado en valor de f
nodo = colaAbierta[0] #El primero es el de menor valor
colaAbierta.remove(nodo) #Se elimina de abierta
colaCerrada.append(nodo) #Se incluye en cerrada
if isGoal(nodo, goals):
return camino(nodo, goals)
sucesores = expandir(warehouse, nodo)
for sucesor in sucesores:
gFuture = nodo.g + 1 #Coste cte de 1 en todas las acciones
if gFuture < sucesor.g:
sucesor.g = gFuture
sucesor.f = sucesor.g + h(sucesor, goals)
if not isIn(sucesor, colaAbierta+colaCerrada):
colaAbierta.append(sucesor)
return [], Inventario(0, 0, 0, 0, 'error')
def printRes(res):
res.sort(key = getI)
txt = ''
for r in res:
txt += 'Accion '+str(r.i)+': '+r.descripcion+' ('+str(r.x)+', '+str(r.y)+')\n'
print(txt)
def sumI(obj, num):
for o in obj:
o.i += num
return obj
def resolverAlmacen(almacen):
#Resolver por cada armario
acciones = []
#Se ejecuta tantas veces como inventarios haya
for i, inve in enumerate(almacen.inventarios.copy()):
#(1) IR a inventario
inicio = Nodo(almacen.robot.x, almacen.robot.y, 'inicio')
res, inv = busquedaAStar(almacen, inicio, almacen.inventarios)
#Si no hay acciones no hay solución y se corta la ejecución
if len(res) <= 0:
return acciones
#(2) COGER inventario
res.append(Accion(res[0].x, res[0].y, len(res)+1, 'coger inventario en'))
almacen.robot.loaded = True
#(3) IR a objetivo
inicio = Nodo(res[0].x, res[0].y, 'inicio')
final = Nodo(inv.x_goal, inv.y_goal, 'final')
res2, inv2 = busquedaAStar(almacen, inicio, [final])
#Si no hay acciones no hay solución y se corta la ejecución
if len(res2) <= 0:
return acciones
#Se adhieren los elementos de forma ordenada
res += sumI(res2, len(res))
#Se actualiza la información del inventario movido
for a in almacen.inventarios:
if a.etiqueta == inv.etiqueta:
a.arrived = True
a.x = res2[0].x
a.y = res2[0].y
#(4) DEJAR inventario en objetivo
res.append(Accion(res2[0].x, res2[0].y, len(res)+1, 'dejar inventario en'))
almacen.robot.loaded = False
#Reinicializar posición robot
almacen.robot.x = res2[0].x
almacen.robot.y = res2[0].y
#Se adhieren los elementos de forma ordenada
acciones += sumI(res, len(acciones))
return acciones
#Lista de obstáculos
obstaculos = [Pared(1,0), Pared(1,1)]
#Lista de inventarios
armarios = [Inventario(0,0, 3,3, 'M1'), Inventario(0,2, 2,3, 'M2'), Inventario(3,0, 1,3, 'M3')]
#Robot
robot = Robot(2,2)
#Almacén conteniendo todo lo demás
almacen = Almacen(4, 4, obstaculos, armarios, armarios, robot)
acc = resolverAlmacen(almacen)
printRes(acc)
Accion 1: ir a (1, 2)
Accion 2: ir a (0, 2)
Accion 3: coger inventario en (0, 2)
Accion 4: ir a (1, 2)
Accion 5: ir a (2, 2)
Accion 6: ir a (2, 3)
Accion 7: dejar inventario en (2, 3)
Accion 8: ir a (2, 2)
Accion 9: ir a (2, 1)
Accion 10: ir a (2, 0)
Accion 11: ir a (3, 0)
Accion 12: coger inventario en (3, 0)
Accion 13: ir a (3, 1)
Accion 14: ir a (3, 2)
Accion 15: ir a (2, 2)
Accion 16: ir a (1, 2)
Accion 17: ir a (1, 3)
Accion 18: dejar inventario en (1, 3)
Accion 19: ir a (1, 2)
Accion 20: ir a (0, 2)
Accion 21: ir a (0, 1)
Accion 22: ir a (0, 0)
Accion 23: coger inventario en (0, 0)
Accion 24: ir a (0, 1)
Accion 25: ir a (0, 2)
Accion 26: ir a (1, 2)
Accion 27: ir a (2, 2)
Accion 28: ir a (3, 2)
Accion 29: ir a (3, 3)
Accion 30: dejar inventario en (3, 3)
#Recoger inventario
def getInv(invlst, x, y):
for i in invlst.copy():
if i.x == x and i.y == y:
invlst.remove(i)
return invlst
#Dejar inventario
def setInv(invlst, x, y):
return invlst.append(Inventario(x,y,0,0))
#Crear una animacion de las acciones dento del almacen
def createFrames(warehouse, actions):
bgr_robot=(43, 57, 192)
#Lista de marcos
frames = []
#Lista de inventarios
invlst = warehouse.inventarios.copy()
#Añadir estado inicial
actions.insert(0, Accion(warehouse.robot.x, warehouse.robot.y, -1, 'ir a'))
#Por cada accion se crea un marco
for a in actions:
image = np.zeros((warehouse.h , warehouse.w, 3), np.uint8)
#Renderizar suelo - blanco
bgr_color=(255, 255, 255)
image[:] = bgr_color
#Renderizar paredes - gris
bgr_color=(115, 101, 86)
for p in warehouse.paredes:
image[p.y, p.x] = bgr_color
#Renderizar inventario - naranja
bgr_color=(219, 152, 52)
for i in invlst:
image[i.y, i.x] = bgr_color
#Renderizar robot - azul: descargado / magenta: cargado
if a.descripcion == 'coger inventario en':
bgr_robot=(182, 89, 155)
getInv(invlst, a.x, a.y)
elif a.descripcion == 'dejar inventario en':
bgr_robot=(43, 57, 192)
setInv(invlst, a.x, a.y)
image[a.y, a.x] = bgr_robot
frames.append(cv2.resize(image, (400, 400), interpolation = cv2.INTER_NEAREST))
return frames
#Agrupa las imagenes en un GIF para su visualización
def frames2GIF(frames):
print("Guardando archivo GIF")
with imageio.get_writer("almacen.gif", mode="I") as writer:
writer.append_data(frames[0])
writer.append_data(frames[0])
for f in frames:
writer.append_data(f)
writer.append_data(frames[-1])
writer.append_data(frames[-1])
#Copiar los objetos anteriores con nombre cambiado
obstaculos2 = [Pared(1,0), Pared(1,1)]
armarios2 = [Inventario(0,0, 3,3, 'M1'), Inventario(0,2, 2,3, 'M2'), Inventario(3,0, 1,3, 'M3')]
robot2 = Robot(2,2)
almacen2 = Almacen(4, 4, obstaculos2, armarios2, armarios2, robot2)
#Crear el GIF
marcos = createFrames(almacen2, acc)
frames2GIF(marcos)
Guardando archivo GIF
display(Image(filename='almacen.gif'))