Hardi Kurnianto
Published

Linier Assignment With Hungarian Algorithm

Task allocation problem at multi-agent system navigation have to solved with Hungarian Algorithm.

IntermediateProtip3 hours74
Linier Assignment With Hungarian Algorithm

Things used in this project

Software apps and online services

VS Code
Microsoft VS Code

Story

Read more

Custom parts and enclosures

af_mXmFQtEC9Y.SLDPRT

Code

Hungarian_12S.py

Python
from importlib.resources import path
from xml.etree.ElementTree import PI
from scipy.optimize import linear_sum_assignment
import numpy as np
from pion import Pion
from queue import PriorityQueue
from peta import DiscreteMap
import time
################################################################################################################################################################################################
petagudang = DiscreteMap(110,72)
st = time.time()
################################################################################################################################################################################################
def distance(node1, node2):
    return np.sqrt((node1.x - node2.x)**2+(node1.y - node2.y)**2)

def heuristic_fct(node1, node2):
    return distance(node1, node2)
################################################################################################################################################################################################
def A_star(petagudang, v, target):

    S = PriorityQueue()
    path = [v]
    S.put((0, (v, path)))
    
    discovered_nodes = set()
    
    while not S.empty():
        cost, (v, path) = S.get()
        
        if not v in discovered_nodes:
            discovered_nodes.add(v)
            
            if v == target:
                cost_to_reach_target_node = 0
                for i in range(len(path) - 1):
                    cost_to_reach_target_node += distance(path[i], path[i+1])
                return path, discovered_nodes, cost_to_reach_target_node
            
            for w in petagudang.get_adjacent_nodes(v):
                new_path = path.copy()
                new_path.append(w)
        
                cost_path = 0
                for i in range(len(path) - 1):
                    cost_path += distance(path[i], path[i+1])
                
                S.put((cost_path + heuristic_fct(w, target), (w, new_path)))
################################################################################################################################################################################################
#set posisi awal packing
ta1= Pion(18,1)
ta2= Pion(21,1)
ta3= Pion(26,1)
ta4= Pion(28,1)
ta5= Pion(34,1)
ta6= Pion(36,1)
ta7= Pion(38,1)
ta8= Pion(42,1)
#set posisi awal loading
ta13=Pion(7 ,59)
ta14=Pion(15,59)
ta15=Pion(27,59)
ta16=Pion(35,59)

#set posisi tujuan packing
tu1= Pion(12,51)
tu2= Pion(32,51)
tu3= Pion(47,51)
tu4= Pion(47,56)
tu5= Pion(54,51)
tu6= Pion(54,56)
tu7= Pion(57,16)
tu8= Pion(77,16)
#set posisi tujuan loading
tu13=Pion(26,20)
tu14=Pion(32,20)
tu15=Pion(73,16)
tu16=Pion(51,56)

path11,  discovered_nodes, cost11  = A_star(petagudang, ta1, tu1)
path12,  discovered_nodes, cost12  = A_star(petagudang, ta1, tu2)
path13,  discovered_nodes, cost13  = A_star(petagudang, ta1, tu3)
path14,  discovered_nodes, cost14  = A_star(petagudang, ta1, tu4)
path15,  discovered_nodes, cost15  = A_star(petagudang, ta1, tu5)
path16,  discovered_nodes, cost16  = A_star(petagudang, ta1, tu6)
path17,  discovered_nodes, cost17  = A_star(petagudang, ta1, tu7)
path18,  discovered_nodes, cost18  = A_star(petagudang, ta1, tu8)
path113, discovered_nodes, cost113 = A_star(petagudang, ta1, tu13)
path114, discovered_nodes, cost114 = A_star(petagudang, ta1, tu14)
path115, discovered_nodes, cost115 = A_star(petagudang, ta1, tu15)
path116, discovered_nodes, cost116 = A_star(petagudang, ta1, tu16)

path21,  discovered_nodes, cost21  = A_star(petagudang, ta2, tu1)
path22,  discovered_nodes, cost22  = A_star(petagudang, ta2, tu2)
path23,  discovered_nodes, cost23  = A_star(petagudang, ta2, tu3)
path24,  discovered_nodes, cost24  = A_star(petagudang, ta2, tu4)
path25,  discovered_nodes, cost25  = A_star(petagudang, ta2, tu5)
path26,  discovered_nodes, cost26  = A_star(petagudang, ta2, tu6)
path27,  discovered_nodes, cost27  = A_star(petagudang, ta2, tu7)
path28,  discovered_nodes, cost28  = A_star(petagudang, ta2, tu8)
path213, discovered_nodes, cost213 = A_star(petagudang, ta2, tu13)
path214, discovered_nodes, cost214 = A_star(petagudang, ta2, tu14)
path215, discovered_nodes, cost215 = A_star(petagudang, ta2, tu15)
path216, discovered_nodes, cost216 = A_star(petagudang, ta2, tu16)

path31,  discovered_nodes, cost31  = A_star(petagudang, ta3, tu1)
path32,  discovered_nodes, cost32  = A_star(petagudang, ta3, tu2)
path33,  discovered_nodes, cost33  = A_star(petagudang, ta3, tu3)
path34,  discovered_nodes, cost34  = A_star(petagudang, ta3, tu4)
path35,  discovered_nodes, cost35  = A_star(petagudang, ta3, tu5)
path36,  discovered_nodes, cost36  = A_star(petagudang, ta3, tu6)
path37,  discovered_nodes, cost37  = A_star(petagudang, ta3, tu7)
path38,  discovered_nodes, cost38  = A_star(petagudang, ta3, tu8)
path313, discovered_nodes, cost313 = A_star(petagudang, ta3, tu13)
path314, discovered_nodes, cost314 = A_star(petagudang, ta3, tu14)
path315, discovered_nodes, cost315 = A_star(petagudang, ta3, tu15)
path316, discovered_nodes, cost316 = A_star(petagudang, ta3, tu16)

path41,  discovered_nodes, cost41  = A_star(petagudang, ta4, tu1)
path42,  discovered_nodes, cost42  = A_star(petagudang, ta4, tu2)
path43,  discovered_nodes, cost43  = A_star(petagudang, ta4, tu3)
path44,  discovered_nodes, cost44  = A_star(petagudang, ta4, tu4)
path45,  discovered_nodes, cost45  = A_star(petagudang, ta4, tu5)
path46,  discovered_nodes, cost46  = A_star(petagudang, ta4, tu6)
path47,  discovered_nodes, cost47  = A_star(petagudang, ta4, tu7)
path48,  discovered_nodes, cost48  = A_star(petagudang, ta4, tu8)
path413, discovered_nodes, cost413 = A_star(petagudang, ta4, tu13)
path414, discovered_nodes, cost414 = A_star(petagudang, ta4, tu14)
path415, discovered_nodes, cost415 = A_star(petagudang, ta4, tu15)
path416, discovered_nodes, cost416 = A_star(petagudang, ta4, tu16)

path51,  discovered_nodes, cost51  = A_star(petagudang, ta5, tu1)
path52,  discovered_nodes, cost52  = A_star(petagudang, ta5, tu2)
path53,  discovered_nodes, cost53  = A_star(petagudang, ta5, tu3)
path54,  discovered_nodes, cost54  = A_star(petagudang, ta5, tu4)
path55,  discovered_nodes, cost55  = A_star(petagudang, ta5, tu5)
path56,  discovered_nodes, cost56  = A_star(petagudang, ta5, tu6)
path57,  discovered_nodes, cost57  = A_star(petagudang, ta5, tu7)
path58,  discovered_nodes, cost58  = A_star(petagudang, ta5, tu8)
path513, discovered_nodes, cost513 = A_star(petagudang, ta5, tu13)
path514, discovered_nodes, cost514 = A_star(petagudang, ta5, tu14)
path515, discovered_nodes, cost515 = A_star(petagudang, ta5, tu15)
path516, discovered_nodes, cost516 = A_star(petagudang, ta5, tu16)

path61,  discovered_nodes, cost61  = A_star(petagudang, ta6, tu1)
path62,  discovered_nodes, cost62  = A_star(petagudang, ta6, tu2)
path63,  discovered_nodes, cost63  = A_star(petagudang, ta6, tu3)
path64,  discovered_nodes, cost64  = A_star(petagudang, ta6, tu4)
path65,  discovered_nodes, cost65  = A_star(petagudang, ta6, tu5)
path66,  discovered_nodes, cost66  = A_star(petagudang, ta6, tu6)
path67,  discovered_nodes, cost67  = A_star(petagudang, ta6, tu7)
path68,  discovered_nodes, cost68  = A_star(petagudang, ta6, tu8)
path613, discovered_nodes, cost613 = A_star(petagudang, ta6, tu13)
path614, discovered_nodes, cost614 = A_star(petagudang, ta6, tu14)
path615, discovered_nodes, cost615 = A_star(petagudang, ta6, tu15)
path616, discovered_nodes, cost616 = A_star(petagudang, ta6, tu16)

path71,  discovered_nodes, cost71  = A_star(petagudang, ta7, tu1)
path72,  discovered_nodes, cost72  = A_star(petagudang, ta7, tu2)
path73,  discovered_nodes, cost73  = A_star(petagudang, ta7, tu3)
path74,  discovered_nodes, cost74  = A_star(petagudang, ta7, tu4)
path75,  discovered_nodes, cost75  = A_star(petagudang, ta7, tu5)
path76,  discovered_nodes, cost76  = A_star(petagudang, ta7, tu6)
path77,  discovered_nodes, cost77  = A_star(petagudang, ta7, tu7)
path78,  discovered_nodes, cost78  = A_star(petagudang, ta7, tu8)
path713, discovered_nodes, cost713 = A_star(petagudang, ta7, tu13)
path714, discovered_nodes, cost714 = A_star(petagudang, ta7, tu14)
path715, discovered_nodes, cost715 = A_star(petagudang, ta7, tu15)
path716, discovered_nodes, cost716 = A_star(petagudang, ta7, tu16)

path81,  discovered_nodes, cost81  = A_star(petagudang, ta8, tu1)
path82,  discovered_nodes, cost82  = A_star(petagudang, ta8, tu2)
path83,  discovered_nodes, cost83  = A_star(petagudang, ta8, tu3)
path84,  discovered_nodes, cost84  = A_star(petagudang, ta8, tu4)
path85,  discovered_nodes, cost85  = A_star(petagudang, ta8, tu5)
path86,  discovered_nodes, cost86  = A_star(petagudang, ta8, tu6)
path87,  discovered_nodes, cost87  = A_star(petagudang, ta8, tu7)
path88,  discovered_nodes, cost88  = A_star(petagudang, ta8, tu8)
path813, discovered_nodes, cost813 = A_star(petagudang, ta8, tu13)
path814, discovered_nodes, cost814 = A_star(petagudang, ta8, tu14)
path815, discovered_nodes, cost815 = A_star(petagudang, ta8, tu15)
path816, discovered_nodes, cost816 = A_star(petagudang, ta8, tu16)

path131C,  discovered_nodes, cost131C  = A_star(petagudang, ta13, tu1)
path132C,  discovered_nodes, cost132C  = A_star(petagudang, ta13, tu2)
path133C,  discovered_nodes, cost133C  = A_star(petagudang, ta13, tu3)
path134C,  discovered_nodes, cost134C  = A_star(petagudang, ta13, tu4)
path135C,  discovered_nodes, cost135C  = A_star(petagudang, ta13, tu5)
path136C,  discovered_nodes, cost136C  = A_star(petagudang, ta13, tu6)
path137C,  discovered_nodes, cost137C  = A_star(petagudang, ta13, tu7)
path138C,  discovered_nodes, cost138C  = A_star(petagudang, ta13, tu8)
path1313C, discovered_nodes, cost1313C = A_star(petagudang, ta13, tu13)
path1314C, discovered_nodes, cost1314C = A_star(petagudang, ta13, tu14)
path1315C, discovered_nodes, cost1315C = A_star(petagudang, ta13, tu15)
path1316C, discovered_nodes, cost1316C = A_star(petagudang, ta13, tu16)

path141D,  discovered_nodes, cost141D  = A_star(petagudang, ta14, tu1)
path142D,  discovered_nodes, cost142D  = A_star(petagudang, ta14, tu2)
path143D,  discovered_nodes, cost143D  = A_star(petagudang, ta14, tu3)
path144D,  discovered_nodes, cost144D  = A_star(petagudang, ta14, tu4)
path145D,  discovered_nodes, cost145D  = A_star(petagudang, ta14, tu5)
path146D,  discovered_nodes, cost146D  = A_star(petagudang, ta14, tu6)
path147D,  discovered_nodes, cost147D  = A_star(petagudang, ta14, tu7)
path148D,  discovered_nodes, cost148D  = A_star(petagudang, ta14, tu8)
path1413D, discovered_nodes, cost1413D = A_star(petagudang, ta14, tu13)
path1414D, discovered_nodes, cost1414D = A_star(petagudang, ta14, tu14)
path1415D, discovered_nodes, cost1415D = A_star(petagudang, ta14, tu15)
path1416D, discovered_nodes, cost1416D = A_star(petagudang, ta14, tu16)

path151E,  discovered_nodes, cost151E  = A_star(petagudang, ta15, tu1)
path152E,  discovered_nodes, cost152E  = A_star(petagudang, ta15, tu2)
path153E,  discovered_nodes, cost153E  = A_star(petagudang, ta15, tu3)
path154E,  discovered_nodes, cost154E  = A_star(petagudang, ta15, tu4)
path155E,  discovered_nodes, cost155E  = A_star(petagudang, ta15, tu5)
path156E,  discovered_nodes, cost156E  = A_star(petagudang, ta15, tu6)
path157E,  discovered_nodes, cost157E  = A_star(petagudang, ta15, tu7)
path158E,  discovered_nodes, cost158E  = A_star(petagudang, ta15, tu8)
path1513E, discovered_nodes, cost1513E = A_star(petagudang, ta15, tu13)
path1514E, discovered_nodes, cost1514E = A_star(petagudang, ta15, tu14)
path1515E, discovered_nodes, cost1515E = A_star(petagudang, ta15, tu15)
path1516E, discovered_nodes, cost1516E = A_star(petagudang, ta15, tu16)

path161F,  discovered_nodes, cost161F  = A_star(petagudang, ta16, tu1)
path162F,  discovered_nodes, cost162F  = A_star(petagudang, ta16, tu2)
path163F,  discovered_nodes, cost163F  = A_star(petagudang, ta16, tu3)
path164F,  discovered_nodes, cost164F  = A_star(petagudang, ta16, tu4)
path165F,  discovered_nodes, cost165F  = A_star(petagudang, ta16, tu5)
path166F,  discovered_nodes, cost166F  = A_star(petagudang, ta16, tu6)
path167F,  discovered_nodes, cost167F  = A_star(petagudang, ta16, tu7)
path168F,  discovered_nodes, cost168F  = A_star(petagudang, ta16, tu8)
path1613F, discovered_nodes, cost1613F = A_star(petagudang, ta16, tu13)
path1614F, discovered_nodes, cost1614F = A_star(petagudang, ta16, tu14)
path1615F, discovered_nodes, cost1615F = A_star(petagudang, ta16, tu15)
path1616F, discovered_nodes, cost1616F = A_star(petagudang, ta16, tu16)

agent1  = [cost11,cost12,cost13,cost14,cost15,cost16,cost17,cost18,cost113,cost114,cost115,cost116]
agent2  = [cost21,cost22,cost23,cost24,cost25,cost26,cost27,cost28,cost213,cost214,cost215,cost116]
agent3  = [cost31,cost32,cost33,cost34,cost35,cost36,cost37,cost38,cost313,cost314,cost315,cost316]
agent4  = [cost41,cost42,cost43,cost44,cost45,cost46,cost47,cost48,cost413,cost414,cost415,cost416]
agent5  = [cost51,cost52,cost53,cost54,cost55,cost56,cost57,cost58,cost513,cost514,cost515,cost516]
agent6  = [cost61,cost62,cost63,cost64,cost65,cost66,cost67,cost68,cost613,cost614,cost615,cost616]
agent7  = [cost71,cost72,cost73,cost74,cost75,cost76,cost77,cost78,cost713,cost714,cost715,cost716]
agent8  = [cost81,cost82,cost83,cost84,cost85,cost86,cost87,cost88,cost813,cost814,cost815,cost816]
agent13 = [cost131C,cost132C,cost133C,cost134C,cost135C,cost136C,cost137C,cost138C,cost1313C,cost1314C,cost1315C,cost1316C]
agent14 = [cost141D,cost142D,cost143D,cost144D,cost145D,cost146D,cost147D,cost148D,cost1413D,cost1414D,cost1415D,cost1416D]
agent15 = [cost151E,cost152E,cost153E,cost154E,cost155E,cost156E,cost157E,cost158E,cost1513E,cost1514E,cost1515E,cost1516E]
agent16 = [cost161F,cost162F,cost163F,cost164F,cost165F,cost166F,cost167F,cost168F,cost1613F,cost1614F,cost1615F,cost1616F]

cost_all_path = np.array([
                          [round(cost11),round(cost12),round(cost13),round(cost14),round(cost15),round(cost16),round(cost17),round(cost18),round(cost113),round(cost114),round(cost115),round(cost116)],
                          [round(cost21),round(cost22),round(cost23),round(cost24),round(cost25),round(cost26),round(cost27),round(cost28),round(cost213),round(cost214),round(cost215),round(cost216)],
                          [round(cost31),round(cost32),round(cost33),round(cost34),round(cost35),round(cost36),round(cost37),round(cost38),round(cost313),round(cost314),round(cost315),round(cost316)],
                          [round(cost41),round(cost42),round(cost43),round(cost44),round(cost45),round(cost46),round(cost47),round(cost48),round(cost413),round(cost414),round(cost415),round(cost416)],
                          [round(cost51),round(cost52),round(cost53),round(cost54),round(cost55),round(cost56),round(cost57),round(cost58),round(cost513),round(cost514),round(cost515),round(cost516)],
                          [round(cost61),round(cost62),round(cost63),round(cost64),round(cost65),round(cost66),round(cost67),round(cost68),round(cost613),round(cost614),round(cost615),round(cost616)],
                          [round(cost71),round(cost72),round(cost73),round(cost74),round(cost75),round(cost76),round(cost77),round(cost78),round(cost713),round(cost714),round(cost715),round(cost716)],
                          [round(cost81),round(cost82),round(cost83),round(cost84),round(cost85),round(cost86),round(cost87),round(cost88),round(cost813),round(cost814),round(cost815),round(cost816)],
                          [round(cost131C),round(cost132C),round(cost133C),round(cost134C),round(cost135C),round(cost136C),round(cost137C),round(cost138C),round(cost1313C),round(cost1314C),round(cost1315C),round(cost1316C)],
                          [round(cost141D),round(cost142D),round(cost143D),round(cost144D),round(cost145D),round(cost146D),round(cost147D),round(cost148D),round(cost1413D),round(cost1414D),round(cost1415D),round(cost1416D)],
                          [round(cost151E),round(cost152E),round(cost153E),round(cost154E),round(cost155E),round(cost156E),round(cost157E),round(cost158E),round(cost1513E),round(cost1514E),round(cost1515E),round(cost1516E)],
                          [round(cost161F),round(cost162F),round(cost163F),round(cost164F),round(cost165F),round(cost166F),round(cost167F),round(cost168F),round(cost1613F),round(cost1614F),round(cost1615F),round(cost1616F)],                        
                         ]
                        )

print('Cost Path = \n',cost_all_path)
row_ind, col_ind = linear_sum_assignment(cost_all_path)
opt_ass = col_ind
tc = cost_all_path[row_ind,col_ind].sum()
print('Task Allocation = ',opt_ass)
print('Path Lenght = ',tc)
et = time.time()
ft = et-st
print('Computational Time',ft , 's')

#if (opt_ass[0],opt_ass[1],opt_ass[2],opt_ass[3]) == (0,3,2,1):
#petagudang.plot_path(path115,path29,path310,path416,path513,path611,path76,path88,path91,path102,path113,path125,path134,path147,path1514,path1612)
##if (opt_ass[0],opt_ass[1],opt_ass[2],opt_ass[3]) == (0,1,2,3):
##    petagudang.plot_path(path11,path22,path33,path44)



"""
print(round(cost11),round(cost12),round(cost13),round(cost14))
print(round(cost21),round(cost22),round(cost23),round(cost24))
print(round(cost31),round(cost32),round(cost33),round(cost34))
print(round(cost41),round(cost42),round(cost43),round(cost44))

print(min(agent1))
print(min(agent2))
print(min(agent3))
print(min(agent4))

print(round(cost11),round(cost24),round(cost32),round(cost43))

if (cost14,cost22,cost33,cost41) == (min(agent1), 12, 8.414213562373096, 15):
    petagudang.plot_path(path14,path22,path33,path41)
if (cost11,cost24,cost33,cost42) == (min(agent1), min(agent2), 20.82842712474619, 16):
    petagudang.plot_path(path11,path24,path33,path42)
if (cost14,cost21,cost33,cost42) == (18.414213562373096, 7, min(agent3), 13):
    petagudang.plot_path(path14,path21,path33,path42)
if (cost12,cost21,cost34,cost43) == (min(agent1), min(agent2), min(agent3), min(agent4)):
    petagudang.plot_path(path12,path21,path34,path43)
if (cost14,cost21,cost32,cost43) == (min(agent1), min(agent2), 9.414213562373096, min(agent4)):
    petagudang.plot_path(path14,path21,path32,path43)
if (round(cost14),round(cost23),round(cost32),round(cost41)) == (25,37,26,10):
    petagudang.plot_path(path14,path22,path33,path41)
"""

peta.py

Python
import abc
from cmath import pi
from tkinter import W
from tokenize import Pointfloat
import numpy as np
import matplotlib
import matplotlib.pyplot as plt
from pion import Pion


class Map:

    def __init__(self, h, w):
        self.h = h
        self.w = w

        # Build map
        self.map = np.zeros((h, w))
        #Office
        self._add_rectangular_table_(14,24) 
        #Packing Area
        self._add_rectangular_table_1_(6,61,9,67)
        self.map[9,63] = 1
        self.map[9,64] = 1
        self._add_rectangular_table_1_(10,61,13,67)
        self._add_rectangular_table_1_(14,61,17,67)
        self.map[17,63] = 1
        self.map[17,64] = 1
        self._add_rectangular_table_1_(18,61,21,67)
        self._add_rectangular_table_1_(22,64,25,67)
        self._add_rectangular_table_1_(26,61,29,67)
        self.map[29,63] = 1
        self.map[29,64] = 1
        self._add_rectangular_table_1_(30,61,33,67)
        self._add_rectangular_table_1_(34,61,37,67)
        self.map[37,63] = 1
        self.map[37,64] = 1
        self._add_rectangular_table_1_(38,61,41,67)
        self._add_rectangular_table_1_(42,64,45,67)

        #Loading Area
        self.map[18,0] = 1
        self.map[21,0] = 1
        self.map[26,0] = 1
        self.map[28,0] = 1
        self.map[34,0] = 1
        self.map[36,0] = 1
        self.map[38,0] = 1
        self.map[42,0] = 1
        self.map[44,0] = 1
        self.map[46,0] = 1
        self.map[51,0] = 1
        self.map[54,0] = 1

        self._add_rectangular_table_5_(18,21,40,51) 
        self._add_rectangular_table_5_(0,28,18,51) 
        self._add_rectangular_table_5_(48,58,110,72) 
        self._add_rectangular_table_5_(48,22,110,51) 
        self._add_rectangular_table_5_(58,0,110,16) 
        

        
    def _add_rectangular_table_(self,xc,yc):
        for i in range(xc):
            for j in range(yc):
                self.map[i,j] = 1

    def _add_rectangular_table_1_(self,xc,yc,w,h):
        for i in range(xc,w):
            for j in range(yc,h):
                self.map[i,j] = 1
    
    def _add_rectangular_table_5_(self,xc,yc,w,h):
        for i in range(xc,w):
            for j in range(yc,h):
                self.map[i,j] = 1

    def _in_bounds_(self, node):
        return (node.x >= 0) and (node.x < self.h) and (node.y >= 0) and (node.y < self.w)

    def show(self, show_grid=True, do_show=True):

        cmap = matplotlib.colors.ListedColormap(['white', 'grey'])
        plt.imshow(self.map.T, cmap=cmap)

        if show_grid:
            plt.gca().grid(which='major', axis='both', linestyle='-', color='k',
                           linewidth=2, alpha=.7)
            plt.gca().set_xticks(np.arange( -0.5, self.h, 1))
            plt.gca().set_yticks(np.arange( -0.5, self.w, 1))
        else:
            plt.gca().set_xticks([])
            plt.gca().set_yticks([])

        if do_show:
            plt.show()

    def plot_path(self, p1, p2, p3, p4, p5, p6, p7, p8,figsize=(6, 6)):

        plt.figure(figsize=figsize)
        for i in range(len(p1) - 1):
            plt.plot([p1[i].x, p1[i + 1].x],
                     [p1[i].y, p1[i + 1].y], c='r')
        for i in range(len(p2) - 1):
            plt.plot([p2[i].x, p2[i + 1].x],
                     [p2[i].y, p2[i + 1].y], c='r')
        for i in range(len(p3) - 1):
            plt.plot([p3[i].x, p3[i + 1].x],
                     [p3[i].y, p3[i + 1].y], c='r')
        for i in range(len(p4) - 1):
            plt.plot([p4[i].x, p4[i + 1].x],
                     [p4[i].y, p4[i + 1].y], c='r')
        for i in range(len(p5) - 1):
            plt.plot([p5[i].x, p5[i + 1].x],
                     [p5[i].y, p5[i + 1].y], c='g')
        for i in range(len(p6) - 1):
            plt.plot([p6[i].x, p6[i + 1].x],
                     [p6[i].y, p6[i + 1].y], c='g')
        for i in range(len(p7) - 1):
            plt.plot([p7[i].x, p7[i + 1].x],
                     [p7[i].y, p7[i + 1].y], c='g')
        for i in range(len(p8) - 1):
            plt.plot([p8[i].x, p8[i + 1].x],
                     [p8[i].y, p8[i + 1].y], c='g')          
        self.show()

    abc.abstractmethod
    def get_adjacent_nodes(self, node):
        return NotImplemented


class DiscreteMap(Map):

    def get_adjacent_nodes(self, node):
        adjacent_nodes = []

        for i in range(-1, 2):
            for j in range(-1, 2):
                if not (i == j):
                    new_node = Pion(node.x + i, node.y + j)
                    if self._in_bounds_(new_node):
                        if self.map[new_node.x, new_node.y] == 0:
                            adjacent_nodes.append(new_node)

        return adjacent_nodes

pion.py

Python
class Pion:
    def __init__(self, x, y):
        self.x = x
        self.y = y
    
    def __eq__(self,other):
        return(self.x == other.x) and (self.y == other.y)

    def __hash__(self):
        return hash((self.x, self.y))
    
    def __lt__(self, other):
        return (self.x, self.y) < (other.x, other.y)

Credits

Hardi Kurnianto
17 projects • 16 followers
Master student at Intelligent Control and Systems Engineering Department

Comments

Related channels and tags
  • Microsoft
  • Robotics
  • Robotics