Welcome to Hackster!
Hackster is a community dedicated to learning hardware, from beginner to pro. Join us, it's free!
Hardi Kurnianto
Published

Simple Centralized Multi-Autonomous Forklift Path Finding

Part of the navigation for multi-autonomous forklifts in warehousing areas.

BeginnerWork in progress5 hours171
Simple Centralized Multi-Autonomous Forklift Path Finding

Things used in this project

Software apps and online services

VS Code
Microsoft VS Code

Story

Read more

Code

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)

peta.py

Python
import abc
from cmath import pi
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))

        self._add_garis_batas(20)
        self._add_rectangular_table_(5, 3, 3, 1)
        self._add_rectangular_table_(14, 3, 3, 1)

        self._add_rectangular_table_(5, 6, 3, 1)
        self._add_rectangular_table_(14, 6, 3, 1)

        self._add_rectangular_table_(10, 9, 0, 1)
        self._add_rectangular_table_(9, 9, 0, 1)

        self._add_rectangular_table_(5, 12, 3, 1)
        self._add_rectangular_table_(14, 12, 3, 1)

        self._add_rectangular_table_(5, 15, 3, 1)
        self._add_rectangular_table_(14, 15, 3, 1)

        self._add_rectangular_table_(5, 18, 3, 1)
        self._add_rectangular_table_(14, 18, 3, 1)


    def _add_garis_batas(self, w):
        for i in range(w):
            self.map[i,0] = 1
            self.map[0,i] = 1
            self.map[19,i] = 1
        
    def _add_rectangular_table_(self, xc, yc, w, h):
        for i in range(xc - w, xc + w +1 ):
            for j in range(yc - h, 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', 'gray'])
        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, 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')
        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


class ContinuousMap(Map):

    def __init__(self, h, w, nodes, adjacency_matrix):

        super().__init__(h, w)
        self.nodes = nodes
        self.adjacency_matrix = adjacency_matrix

    def get_adjacent_nodes(self, node):

        adjacent_nodes = []

        index = self.nodes.index(node)
        for i in range(self.adjacency_matrix.shape[1]):
            if self.adjacency_matrix[index, i] == 1:
                adjacent_nodes.append(self.nodes[i])

        return adjacent_nodes

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

        super().show(show_grid=show_grid, do_show=False)

        # Plot nodes
        for node in self.nodes:
            plt.scatter(node.x, node.y, marker='+', s=100, c='C0')

        # Plot connections between nodes
        for i in range(self.adjacency_matrix.shape[0]):
            for j in range(self.adjacency_matrix.shape[1]):
                if self.adjacency_matrix[i, j] == 1:
                    plt.plot([self.nodes[i].x, self.nodes[j].x],
                             [self.nodes[i].y, self.nodes[j].y], c='k', alpha=0.2)
        if do_show:
            plt.show()


class ContinuousMap1:

    @classmethod
    def get_nodes(cls):
        return [Pion(0, 0),
                Pion(0.24, 1.36),
                Pion(2.80, 0.83),
                Pion(0.74, 4.45),
                Pion(3.07, 3.11),
                Pion(5, 5),
                Pion(10, 0),
                Pion(7, 0.9),
                Pion(9.33, 4),
                Pion(15, 6),
                Pion(13, 1),
                Pion(15.5, 2.8),
                Pion(18, 0.4),
                Pion(5, 13),
                Pion(5.8, 15),
                Pion(8, 12),
                Pion(2, 7),
                Pion(1.3, 9),
                Pion(3.8, 10),
                Pion(2.12, 11.47),
                Pion(.66, 13.07),
                Pion(.01, 15.8),
                Pion(.5, 18.5),
                Pion(2.88, 18.97),
                Pion(7.86, 16.59),
                Pion(5.91, 17.94),
                Pion(11.32, 18.00),
                Pion(12.35, 15.02),
                Pion(11.81, 12.26),
                Pion(16.14, 12.85),
                Pion(18.5, 18.5),
                Pion(18.3, 5.07),
                Pion(18.65, 6.96),
                Pion(16.46, 9.89),
                Pion(18.25, 12.24),
                Pion(17.76, 15.46)]

    @classmethod
    def get_adjacency_matrix(cls):

        adjacency_matrix = np.zeros((len(cls.get_nodes()), len(cls.get_nodes())))
        adjacency_matrix[0, 1] = 1
        adjacency_matrix[0, 2] = 1
        adjacency_matrix[1, 2] = 1
        adjacency_matrix[1, 3] = 1
        adjacency_matrix[1, 4] = 1
        adjacency_matrix[2, 4] = 1
        adjacency_matrix[2, 7] = 1
        adjacency_matrix[7, 6] = 1
        adjacency_matrix[6, 10] = 1
        adjacency_matrix[3, 4] = 1
        adjacency_matrix[4, 7] = 1
        adjacency_matrix[10, 12] = 1
        adjacency_matrix[12, 11] = 1
        adjacency_matrix[7, 8] = 1
        adjacency_matrix[8, 10] = 1
        adjacency_matrix[8, 9] = 1
        adjacency_matrix[9, 31] = 1
        adjacency_matrix[9, 32] = 1
        adjacency_matrix[31, 32] = 1
        adjacency_matrix[3, 5] = 1
        adjacency_matrix[4, 5] = 1
        adjacency_matrix[7, 5] = 1
        adjacency_matrix[8, 5] = 1
        adjacency_matrix[3, 16] = 1
        adjacency_matrix[5, 16] = 1
        adjacency_matrix[16, 17] = 1
        adjacency_matrix[16, 18] = 1
        adjacency_matrix[32, 33] = 1
        adjacency_matrix[33, 34] = 1
        adjacency_matrix[29, 33] = 1
        adjacency_matrix[29, 34] = 1
        adjacency_matrix[29, 35] = 1
        adjacency_matrix[34, 35] = 1
        adjacency_matrix[35, 30] = 1
        adjacency_matrix[17, 19] = 1
        adjacency_matrix[18, 19] = 1
        adjacency_matrix[19, 20] = 1
        adjacency_matrix[19, 13] = 1
        adjacency_matrix[20, 21] = 1
        adjacency_matrix[21, 22] = 1
        adjacency_matrix[22, 23] = 1
        adjacency_matrix[23, 25] = 1
        adjacency_matrix[13, 14] = 1
        adjacency_matrix[14, 25] = 1
        adjacency_matrix[13, 15] = 1
        adjacency_matrix[15, 28] = 1
        adjacency_matrix[28, 29] = 1
        adjacency_matrix[28, 27] = 1
        adjacency_matrix[27, 26] = 1
        adjacency_matrix[26, 24] = 1
        adjacency_matrix[14, 24] = 1
        adjacency_matrix[25, 24] = 1

        # Make adjacency matrix symmetric
        for i in range(adjacency_matrix.shape[0]):
            for j in range(adjacency_matrix.shape[1]):
                if adjacency_matrix[i, j] == 1:
                    adjacency_matrix[j, i] = 1

        return adjacency_matrix

pathfinding.py

Python
from importlib.resources import path
from xml.etree.ElementTree import PI
import numpy as np
from pion import Pion
from queue import PriorityQueue
from peta import DiscreteMap, ContinuousMap, ContinuousMap1
################################################################################################################################################################################################
petagudang = DiscreteMap(20,20)

################################################################################################################################################################################################
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)))
################################################################################################################################################################################################
jumlah_agent = 4
#set poin awal
ta1=Pion(1,19);ta2=Pion(9,19);ta3=Pion(10,19);ta4=Pion(18,19)
#set poin tujuan
#tu1=Pion(15,7);tu2=Pion(6,10);tu3=Pion(13,13);tu4=Pion(5,16)
#tu1=Pion(2,1);tu2=Pion(15,1);tu3=Pion(15,4);tu4=Pion(5,7)
#tu1=Pion(8,13);tu2=Pion(18,6);tu3=Pion(12,16);tu4=Pion(5,4)
#tu1=Pion(8,10);tu2=Pion(2,7);tu3=Pion(17,7);tu4=Pion(11,10)
tu1=Pion(7,16);tu2=Pion(11,10);tu3=Pion(17,10);tu4=Pion(3,16)


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)

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)

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)

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)

agent1 = [cost11,cost12,cost13,cost14]
agent2 = [cost21,cost22,cost23,cost24]
agent3 = [cost31,cost32,cost33,cost34]
agent4 = [cost41,cost42,cost43,cost44]

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

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

print(cost14,cost22,cost33,cost41)

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)

Credits

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

Comments

Please log in or sign up to comment.

Related channels and tags
  • Machine Learning & AI
  • Machine Learning & AI
  • Microsoft
  • Robotics
  • Robotics