#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)

#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)

display(Image(filename='almacen.gif'))