[PYTHON] Effectuer un calcul de chemin sur une grille bidimensionnelle avec Networkx

Il semblait facile d'utiliser la bibliothèque Python Networkx pour faire un simple calcul de chemin, j'ai donc essayé de l'utiliser.

Tout d'abord, considérons l'environnement de réseau bidimensionnel suivant. La taille de la grille est de 20x20.

import networkx as nx
import matplotlib.pyplot as plt

ngrid = 20
gp = nx.grid_graph(dim=[ngrid, ngrid])
nx.draw(gp,
        pos=dict((n, n) for n in gp.nodes()),
        node_color=[gp.node[n].get('color', 'white') for n in gp.nodes_iter()],
        node_size=200)
plt.axis('equal')
plt.show()

Je voulais également que le chemin diagonal en forme de grille soit OK, donc j'ajouterai des bords diagonaux à chaque nœud.

import networkx as nx
import matplotlib.pyplot as plt

def add_cross_edge(gp, shape):
    """
Ajouter des arêtes diagonales à un graphique en grille 2D
    """
    for node in gp.nodes_iter():
        nx_node = (node[0] + 1, node[1] + 1)
        if nx_node[0] < shape[0] and nx_node[1] < shape[1]:
            gp.add_edge(node, nx_node)
        nx_node = (node[0] + 1, node[1] - 1)
        if nx_node[0] < shape[0] and nx_node[1] >= 0:
            gp.add_edge(node, nx_node)

ngrid = 20
gp = nx.grid_graph(dim=[ngrid, ngrid])
add_cross_edge(gp, [ngrid, ngrid])
nx.draw(gp,
        pos=dict((n, n) for n in gp.nodes()),
        node_color=[gp.node[n].get('color', 'white') for n in gp.nodes_iter()],
        node_size=200)
plt.axis('equal')
plt.show()

Enfin, définissez des obstacles et démarrez des objectifs, et vous avez terminé de créer l'environnement lui-même. Ici, les obstacles sont codés par couleur en noir, commencent en vert et les buts en rouge. A partir de cet environnement, nous calculerons le chemin reliant le départ et le but tout en évitant les obstacles.

import networkx as nx
import matplotlib.pyplot as plt

def add_cross_edge(gp, shape):
    """
Ajouter des arêtes diagonales à un graphique en grille 2D
    """
    for node in gp.nodes_iter():
        nx_node = (node[0] + 1, node[1] + 1)
        if nx_node[0] < shape[0] and nx_node[1] < shape[1]:
            gp.add_edge(node, nx_node)
        nx_node = (node[0] + 1, node[1] - 1)
        if nx_node[0] < shape[0] and nx_node[1] >= 0:
            gp.add_edge(node, nx_node)

ngrid = 20
gp = nx.grid_graph(dim=[ngrid, ngrid])
add_cross_edge(gp, [ngrid, ngrid])
idcs = np.random.choice(len(gp.nodes()), int(ngrid * ngrid * 0.2), replace=False)
#Définir départ / objectif / obstacle
st, gl, obs = gp.nodes()[idcs[0]], gp.nodes()[idcs[1]], [gp.nodes()[i] for i in idcs[2:]]
gp.node[st]['color'] = 'green'
gp.node[gl]['color'] = 'red'
for o in obs:
    gp.node[o]['color'] = 'black'
nx.draw(gp,
        pos=dict((n, n) for n in gp.nodes()),
        node_color=[gp.node[n].get('color', 'white') for n in gp.nodes_iter()],
        node_size=200)
plt.axis('equal')
plt.show()

Ensuite, définissez un poids entre chaque bord pour éviter les obstacles. J'essaye d'imposer une pénalité lorsque le nœud porte un obstacle.

import numpy as np

def cost(a, b):
    """
Fonction de coût
    """
    x1 = np.array(a, dtype=np.float32)
    x2 = np.array(b, dtype=np.float32)
    dist = np.linalg.norm(x1 - x2)
    if any([(a == o or b == o) for o in obs]):
        penalty = 1.0e6
    else:
        penalty = 0.0
    return dist + penalty

for u, v, d in gp.edges_iter(data=True):
    d['weight'] = cost(u, v)

Il existe plusieurs fonctions de recherche de chemin dans Networkx, mais cette fois nous utiliserons l'algorithme A *. L'algorithme A * nécessite quelque chose appelé une fonction heuristique, qui est définie comme suit.

def dist(a, b):
    """
Fonction hulistique
    """
    x1 = np.array(a, dtype=np.float32)
    x2 = np.array(b, dtype=np.float32)
    return np.linalg.norm(x1 - x2)

path = nx.astar_path(gp, st, gl, dist)
length = nx.astar_path_length(gp, st, gl, dist)
print(path)
print(length)

Enfin, dessinons le résultat de la recherche de chemin. Les nœuds sur le chemin sont affichés en bleu.

for p in path[1:-1]:
    if gp.node[p].get('color', '') == 'black':
        print('Invalid path')
        continue
    gp.node[p]['color'] = 'blue'

nx.draw(gp,
        pos=dict((n, n) for n in gp.nodes()),
        node_color=[gp.node[n].get('color', 'white') for n in gp.nodes_iter()],
        node_size=200)
plt.axis('equal')
plt.show()

Le code entier ressemble à ceci:

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import numpy as np
import networkx as nx
import matplotlib.pyplot as plt

def add_cross_edge(gp, shape):
    """
Ajouter des arêtes diagonales à un graphique en grille 2D
    """
    for node in gp.nodes_iter():
        nx_node = (node[0] + 1, node[1] + 1)
        if nx_node[0] < shape[0] and nx_node[1] < shape[1]:
            gp.add_edge(node, nx_node)
        nx_node = (node[0] + 1, node[1] - 1)
        if nx_node[0] < shape[0] and nx_node[1] >= 0:
            gp.add_edge(node, nx_node)

ngrid = 20
gp = nx.grid_graph(dim=[ngrid, ngrid])
add_cross_edge(gp, [ngrid, ngrid])
idcs = np.random.choice(len(gp.nodes()), int(ngrid * ngrid * 0.2), replace=False)
#Définir départ / objectif / obstacle
st, gl, obs = gp.nodes()[idcs[0]], gp.nodes()[idcs[1]], [gp.nodes()[i] for i in idcs[2:]]
gp.node[st]['color'] = 'green'
gp.node[gl]['color'] = 'red'
for o in obs:
    gp.node[o]['color'] = 'black'

def dist(a, b):
    """
Fonction hulistique
    """
    x1 = np.array(a, dtype=np.float32)
    x2 = np.array(b, dtype=np.float32)
    return np.linalg.norm(x1 - x2)

def cost(a, b, k1=1.0, k2=10.0, kind='intsct'):
    """
Fonction de coût
    """
    x1 = np.array(a, dtype=np.float32)
    x2 = np.array(b, dtype=np.float32)
    dist = np.linalg.norm(x1 - x2)
    if any([(a == o or b == o) for o in obs]):
        penalty = 1.0e6
    else:
        penalty = 0.0
    return dist + penalty

for u, v, d in gp.edges_iter(data=True):
    d['weight'] = cost(u, v)

path = nx.astar_path(gp, st, gl, dist)
length = nx.astar_path_length(gp, st, gl, dist)
print(path)
print(length)
for p in path[1:-1]:
    if gp.node[p].get('color', '') == 'black':
        print('Invalid path')
        continue
    gp.node[p]['color'] = 'blue'

nx.draw(gp,
        pos=dict((n, n) for n in gp.nodes()),
        node_color=[gp.node[n].get('color', 'white') for n in gp.nodes_iter()],
        node_size=200)
plt.axis('equal')
plt.show()

Networkx est pratique. L'environnement avec des obstacles dans la grille 2D cette fois semble être utilisable pour l'apprentissage par renforcement.