# -*- coding: utf-8 -*-
'''
Module for defining the classes related to the :math:`\\mathrm{A}^\\ast`
algorithm.
:math:`\\mathrm{A}^\\ast` algorithm (pronounced as "A star") is a search
algorithm proposed in 1968 by
`Hart, Nilsson & Raphael (1968) <https://doi.org/10.1109/TSSC.1968.300136>`_.
It optimally finds the minimum cost path in a graph from a start node
:math:`s` to a goal node :math:`g`; in other words, the shortest path if the
cost is given by its length. In addition, the algorithm does not expand as many
nodes as other algorithms do; then, for a graph with a huge quantity of nodes,
the computational performance is higher with respect to other search algorithms
The :math:`\\mathrm{A}^\\ast` algorithm works constantly evaluating an
evaluation function :math:`f(n)` composed of two parts: one is the actual cost
of the optimum path traveled from :math:`s` to the current node :math:`n`
given by the expression :math:`g(n)`, and the second is the cost of the optimum
path from the current node :math:`n` to :math:`g` given by the expression
:math:`h(n)`, which is the heuristic component of the algorithm and it could be
for example either the
`euclidean <https://en.wikipedia.org/wiki/euclidean_distance>`_ or
`manhattan <https://en.wikipedia.org/wiki/Taxicab_geometry>`_ distance.
Thereby, the evaluation function to measure the path cost is
:math:`f(n) = g(n) + h(n)`.
'''
# %%
[docs]class PreferredPath:
'''Creates an instance of an object that stores the indexes of a
polyline in the space of the grid-graph that represents a preferential path
to be followed when the :math:`\\mathrm{A}^\\ast` algorithm is applied. ::
PreferredPath(coordsIdx, factor)
The object has an attribute called factor that represents a coefficient
:math:`k` that multiplies the distance :math:`d` between the current node
:math:`n` and the polyline. Considering the above, the function for
evaluating the total cost of a node is modified as
:math:`f(n) = g(n) + h(n) + kd`
Attributes:
polyline (`numpy.ndarray`): (2, n) array with the indexes of a polyline
in the space of a grid-graph where the :math:`\\mathrm{A}^\\ast`
algorithm is applied. The first row corresponds to the rows and the
second to the columns of the grid-graph.
factor (`int` or `float`): Multiplier of the shortest distance
between the current node and the polyline.
Examples:
>>> from numpy import array
>>> from pybimstab.astar import PreferredPath
>>> coordsIdx = array([[0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13,
13, 13, 13, 13, 13, 13, 13, 13, 13, 13],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,
2, 3, 4, 5, 6, 7, 8, 9]])
>>> preferredPath = PreferredPath(coordsIdx, factor=1)
>>> preferredPath.__dict__
{'coordsIdx': array([[0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 13,
13, 13, 13, 13, 13, 13, 13, 13, 13],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,
2, 3, 4, 5, 6, 7, 8, 9]]),
'factor': 1}
'''
def __init__(self, coordsIdx, factor):
'''
PreferredPath(coordsIdx, factor)
'''
from numpy import array
self.coordsIdx = array(coordsIdx)
self.factor = factor
# %%
[docs]class Node:
'''Creates an instance of an object that defines the structure of a
node that belongs to a grid-graph where is wanted to find the optimum path
through the :math:`\\mathrm{A}^\\ast` algorithm. ::
Node(pos=None, father=None, gCost=None, hCost=None, val=0)
Attributes:
pos (`tuple`): Indexes of the the current node in the grid-graph.
father (`tuple`): Indexes of the father node of the current node.
Default value is ``None``.
gCost (`int` or `float`): Length of the traveled path from the
start node to the current node. Default value is ``None``.
hCost (`int` or `float`): Heuristic length of the path from the
current node to the goal node. Default value is ``None``.
val (`int`): Value that store the node cell in the matrix that defines
the grid-graph. Default value is ``0``.
Note:
The class ``Node`` requires `numpy <http://www.numpy.org/>`_ and
`shapely <https://pypi.python.org/pypi/Shapely>`_
Examples:
>>> node = Node(pos=(6, 7), father=(5, 7), gCost=5, hCost=10, val=1)
>>> node.__dict__
{'father': (5, 7), 'gCost': 5, 'hCost': 10, 'pos': (6, 7), 'val': 1}
'''
def __init__(self, pos, father=None, gCost=None, hCost=None, val=0):
'''
Node(pos=None, father=None, gCost=None, hCost=None, val=0)
'''
self.pos = pos
self.father = father
self.gCost = gCost
self.hCost = hCost
self.val = val
[docs] def getHcost(self, goalNode, heuristic='manhattan', preferredPath=None):
'''Method to obtain the heuristic component, :math:`h(n)`, that
estimates the cost (or length) of the shortest path from the current
node :math:`n` to the goal node.
It must be selected either ``manhattan`` or ``euclidean`` as the model
to estimate the length of the optimum path.
- **manhattan** is the sum of the cathetus of the right triangle\
defined by the current node and the goal node.
- **euclidean** is the length of the hypotenuse of the right\
triangle defined by the current node and the goal node.
It is possible to append a polyline to force the path to follow a
preferential path.
Args:
goalNode (`Node` object): object with the structure of the goal
node.
heuristic (`str`): Name of the geometric model to determine the
heuristic distance. It must be selected either ``manhattan``
or ``euclidean``. The first one is the default value.
preferredPath (`Node` object): Optional argument of the class
``Node`` to force the path. ``None`` is the default value.
Returns:
(`int` or `float`): value of the estimated heuristic distance of\
the opmtimum path.
Examples:
>>> from pybimstab.astar import Node
>>> goalNode = Node(pos=(9, 9))
>>> node = Node(pos=(0, 0))
>>> node.getHcost(goalNode, heuristic='manhattan',
>>> preferredPath=None)
18
>>> from pybimstab.astar import Node
>>> goalNode = Node(pos=(9, 9))
>>> node = Node(pos=(0, 0))
>>> node.getHcost(goalNode, heuristic='euclidean',
>>> preferredPath=None)
12.727922061357855
'''
from shapely.geometry import LineString, Point
# Verifying if is wanted to append the extra modifier to the heuristic
plusCost = 0
if preferredPath is not None:
pline = LineString(preferredPath.coordsIdx.T)
point = Point(self.pos)
plusCost = point.distance(pline) * preferredPath.factor
# extract coordinates from structure
cNode = self.pos
gNode = goalNode.pos
if heuristic == 'manhattan':
cost = abs(cNode[0]-gNode[0]) + abs(cNode[1]-gNode[1])
elif heuristic == 'euclidean':
cost = ((cNode[0]-gNode[0])**2 + (cNode[1]-gNode[1])**2)**0.5
else:
print('Invalid heuristic, You must have selected manhattan or ' +
'euclidean. manhattan selected by default')
cost = abs(cNode[0]-gNode[0]) + abs(cNode[1]-gNode[1])
cost += plusCost
setattr(self, 'hCost', cost)
return cost
[docs] def getGcost(self, fatherNode):
'''Method to obtain the cumulated cost :math:`g(n)`, of the traveled
path from the start node to the current node :math:`n`.
Args:
fatherNode (``Node`` object): object with the structure\
of the current node's father.
Returns:
(`int` or `float`): traveled-path length from the the start node\
to the current node.
Examples:
>>> from pybimstab.astar import Node
>>> node = Node(pos=(9, 9))
>>> fatherNode = Node(pos=(9, 8), gCost=15)
>>> node.getGcost(fatherNode)
16
>>> from pybimstab.astar import Node
>>> node = Node(pos=(9, 9))
>>> fatherNode = Node(pos=(8, 8), gCost=15)
>>> node.getGcost(fatherNode)
16.4142
'''
from numpy import array
if any(array(self.pos) == array(fatherNode.pos)):
cost = fatherNode.gCost + 1 # horizontal or vertical movement.
else:
cost = fatherNode.gCost + 1.4142 # diagonal movement
setattr(self, 'gCost', cost)
return cost
# %%
[docs]class Astar:
'''Creates an instance of an object that defines the optimum path into a
grid-graph maze, from a start node to a goal node given. ::
Astar(grid, startNode, goalNode, heuristic='manhattan',
reverseLeft=True, reverseUp=True, preferredPath=None)
Attributes:
gridGraph (`MazeStructure` object): object with the structure\
of maze where is wanted to find the optimum path.
startNode (`tuple`, `list` or `numpy.ndarray`): indexes of \
the ``gridGraph.matrix`` where the initial node is located. It has\
to be a matrix cell, *ie*, ``gridGraph.matrix[startNode]==0``.
goalNode (`tuple`, `list` or `numpy.ndarray`): indexes of the\
``gridGraph.matrix`` where the ending node is located. It has to\
be a matrix cell, *ie*, ``gridGraph.matrix[goalNode]==0``.
heuristic (`str`): Name of the geometric model to determine the\
heuristic distance. It must be selected either ``manhattan``\
or ``euclidean``. The first one is the default value.
reverseLeft (`bool`): Logical variable to allow or not reverses\
movements to the left. Default value is ``True``.
reverseUp (`bool`): Logical variable to allow or not reverses\
movements to upward. Default value is ``True``.
*forcedPath (`PreferredPath` object): Optional aguments to force the\
optimum path close to a specific polyline.
Note:
The class ``Astar`` requires `NumPy <http://www.numpy.org/>`_\
and `Matplotlib <https://matplotlib.org/>`_.
Examples:
>>> from numpy import array
>>> from pybimstab.astar import PreferredPath, Astar
>>> grid = array([[ 0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
>>> [ 0, 0, 0, 0, 1, 1, 0, 0, 0, 0],
>>> [ 0, 0, 0, 1, 1, 0, 1, 0, 1, 0],
>>> [ 0, 1, 1, 0, 0, 0, 0, 1, 0, 0],
>>> [ 0, 0, 0, 1, 0, 0, 0, 1, 1, 1],
>>> [ 0, 0, 1, 0, 1, 1, 0, 0, 0, 0],
>>> [ 0, 0, 1, 0, 1, 0, 1, 0, 0, 0],
>>> [ 1, 1, 0, 1, 0, 0, 0, 0, 1, 1],
>>> [ 1, 0, 0, 0, 0, 0, 0, 0, 0, 0],
>>> [ 0, 0, 0, 0, 1, 0, 1, 0, 0, 1],
>>> [ 0, 1, 0, 0, 0, 1, 0, 0, 0, 0],
>>> [ 1, 0, 0, 0, 0, 0, 0, 0, 0, 0],
>>> [ 0, 0, 0, 0, 0, 0, 0, 0, 1, 0],
>>> [ 1, 0, 0, 0, 1, 0, 0, 0, 0, 0]
>>> ])
>>> coordsIdx = array([[0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13,
>>> 13, 13, 13, 13, 13, 13, 13, 13, 13, 13],
>>> [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,
>>> 2, 3, 4, 5, 6, 7, 8, 9]])
>>> preferredPath = PreferredPath(coordsIdx, factor=1)
>>> astar = Astar(grid, startNode=(0, 0), goalNode=(13, 9),
>>> heuristic='manhattan', reverseLeft=True,
>>> reverseUp=True, preferredPath=preferredPath)
>>> astar.__dict__.keys()
dict_keys(['grid', 'startNode', 'goalNode', 'heuristic', 'reverseLeft',
'reverseUp', 'preferredPath', 'mazeStr', 'optimumPath'])
'''
def __init__(self, grid, startNode, goalNode, heuristic='manhattan',
reverseLeft=True, reverseUp=True, preferredPath=None):
'''
Astar(grid, startNode, goalNode, heuristic='manhattan',
reverseLeft=True, reverseUp=True, preferredPath=None)
'''
self.grid = grid
self.startNode = startNode
self.goalNode = goalNode
self.heuristic = heuristic
self.reverseLeft = reverseLeft
self.reverseUp = reverseUp
self.preferredPath = preferredPath
# for defining the maze structure
self.defineMazeStr()
# Get the optimum path via A star algortihm
self.getPath()
[docs] def defineMazeStr(self):
'''Defines the clean structure of the grid-graph using objects
instaced from the class ``Node`` for each node (or cell of the grid).
Those cells with value equal to one or minus one will have a g-value
equal to infinite because over those cells, a path is impossible to be
traced.
Returns:
(`numpy.ndarray`): array with the same shape of the input grid\
where each cell is an object from the class ``Node``\
with the structure of its respective node.
Examples:
>>> # after executing the example of the class
>>> astar.mazeStr[0, 0].__dict__
{'father': (0, 0), 'gCost': 0, 'hCost': 22.0, 'pos': (0, 0),
'val': 0}
'''
import numpy as np
numberRows, numberCols = self.grid.shape
mazeStr = np.empty((numberRows, numberCols), dtype=object)
for m in range(numberRows):
for n in range(numberCols):
mazeStr[m, n] = Node(pos=(m, n), val=self.grid[m, n])
# blocks or outside cells have infinite value
if abs(self.grid[m, n]):
mazeStr[m, n].gCost = np.inf
setattr(self, 'mazeStr', mazeStr)
# setting the structure to the start node
mazeStr[self.startNode].father = self.startNode
mazeStr[self.startNode].getHcost(goalNode=mazeStr[self.goalNode],
heuristic=self.heuristic,
preferredPath=self.preferredPath)
mazeStr[self.startNode].gCost = 0
return mazeStr
def __checkerrors(self):
'''Function to check if there is any problem with either the start
or goal nodes. It includes either the existence of a block or an
unallowed cell in those nodes.'''
if self.mazeStr[self.startNode].val == -1:
raise ValueError('Start node: not in the allowed cells')
elif self.mazeStr[self.startNode].val == 1:
raise ValueError('Start node: is a block-cell')
elif self.mazeStr[self.goalNode].val == -1:
raise ValueError('Goal node: not in the allowed cells')
elif self.mazeStr[self.goalNode].val == 1:
raise ValueError('Goal node: is a block-cell')
return
[docs] def getNeighbours(self, node):
'''Method for obtaining the possible neighbours of an specific node
that belongs to the grid given.
Each neighbour is given as a tuple with the indexes of the grid.
Args:
node (``Node`` object): object with the structure of the node which
is wanted to know its possible neighbours.
Returns:
(`list`): Tuples with the indexes of possible neighbours of the\
node in question.
Examples:
>>> # after executing the example of the class
>>> from pybimstab.astar import Node
>>> astar.getNeighbours(Node(pos=(1, 1)))
[(0, 1), (0, 2), (1, 2), (2, 2), (2, 1), (2, 0), (1, 0), (0, 0)]
>>> astar.getNeighbours(Node(pos=(0, 0)))
[(0, 1), (1, 1), (1, 0)]
>>> astar.getNeighbours(Node(pos=(0, 1)))
[(0, 2), (1, 2), (1, 1), (1, 0), (0, 0)]
>>> astar.getNeighbours(Node(pos=(0, 2)))
[(1, 2), (1, 1), (0, 1)]
>>> astar.getNeighbours(Node(pos=(1, 2)))
[(0, 2), (2, 2), (2, 1), (1, 1), (0, 1)]
>>> astar.getNeighbours(Node(pos=(2, 2)))
[(1, 2), (2, 1), (1, 1)]
>>> astar.getNeighbours(Node(pos=(2, 1)))
[(1, 1), (1, 2), (2, 2), (2, 0), (1, 0)]
>>> astar.getNeighbours(Node(pos=(2, 0)))
[(1, 0), (1, 1), (2, 1)]
>>> astar.getNeighbours(Node(pos=(1, 0)))
[(0, 0), (0, 1), (1, 1), (2, 1), (2, 0)]
'''
i, j = node.pos
numberRows, numberCols = self.grid.shape
allNeighbours = [(i-1, j), (i-1, j+1), (i, j+1), (i+1, j+1), (i+1, j),
(i+1, j-1), (i, j-1), (i-1, j-1)]
neighbours = list()
for neighbour in allNeighbours:
if neighbour[0] < 0 or neighbour[1] < 0 or \
neighbour[0] == numberRows or neighbour[1] == numberCols:
continue
if not self.reverseLeft and neighbour[1] < j:
continue
if not self.reverseUp and neighbour[0] > i:
continue
neighbours.append(neighbour)
return neighbours
[docs] def getWayBack(self, node):
'''Method for obtaining the whole way back of an specific node which
has been opened by the *A star* algorithm.
Args:
node (``Node`` object): object with the structure of the node which
is wanted to know its way back.
Returns:
(`numpy.ndarray`): :math:`\\left(2 \\times n \\right)` array\
where :math:`n` is the number of nodes where the path has\
crossed; the first row of the array contains the abscisses and\
the second one contains the ordinates of the nodes into the\
grid-graph
Examples:
>>> import numpy as np
>>> from pybimstab.astar import PreferredPath, Node, Astar
>>> grid = np.zeros((3,3))
>>> grid[1:3, 0:2] = 1
>>> astar = Astar(grid, startNode=(0, 0), goalNode=(2, 2),
>>> heuristic='manhattan', reverseLeft=True,
>>> reverseUp=True, preferredPath=None)
>>> # returning the way back
>>> astar.getWayBack(astar.mazeStr[2, 2])
array([[2, 1, 0, 0],
[2, 1, 1, 0]])
>>> astar.getWayBack(astar.mazeStr[1, 2])
array([[1, 0, 0],
[2, 1, 0]])
>>> astar.getWayBack(astar.mazeStr[1, 1])
ValueError: Input node is a block. It doesn't have a wayback
'''
import numpy as np
if node.val == -1:
raise ValueError("Input node is outside the allowed cells")
elif node.val == 1:
raise ValueError(
"Input node is a block. It doesn't have a wayback")
pathX = list()
pathY = list()
if node.father is not None:
while node.father != node.pos:
pathX.append(node.pos[1])
pathY.append(node.pos[0])
node = self.mazeStr[node.father]
pathX.append(self.startNode[1])
pathY.append(self.startNode[0])
else:
pathX = []
pathY = []
wayBack = np.array([pathY, pathX])
return wayBack
[docs] def getPath(self):
'''Method for obtaining the optimum path between two points into a
grid-graph through the :math:`\\mathrm{A}^\\ast` algorithm
`Hart, Nilsson & Raphael (1968) <https://doi.org/10.1109/TSSC.1968.300136>`_.
Returns:
(`numpy.ndarray`): :math:`\\left(2 \\times n \\right)` array\
where :math:`n` is the number of nodes where the path has\
crossed; the first row of the array contains the abscisses and\
the second one contains the ordinates of the nodes into the\
grid-graph
Examples:
>>> from numpy import array
>>> from pybimstab.astar import PreferredPath, Astar
>>> grid = array([[ 0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
>>> [ 0, 0, 0, 0, 1, 1, 0, 0, 0, 0],
>>> [ 0, 0, 0, 1, 1, 0, 1, 0, 1, 0],
>>> [ 0, 1, 1, 0, 0, 0, 0, 1, 0, 0],
>>> [ 0, 0, 0, 1, 0, 0, 0, 1, 1, 1],
>>> [ 0, 0, 1, 0, 1, 1, 0, 0, 0, 0],
>>> [ 0, 0, 1, 0, 1, 0, 1, 0, 0, 0],
>>> [ 1, 1, 0, 1, 0, 0, 0, 0, 1, 1],
>>> [ 1, 0, 0, 0, 0, 0, 0, 0, 0, 0],
>>> [ 0, 0, 0, 0, 1, 0, 1, 0, 0, 1],
>>> [ 0, 1, 0, 0, 0, 1, 0, 0, 0, 0],
>>> [ 1, 0, 0, 0, 0, 0, 0, 0, 0, 0],
>>> [ 0, 0, 0, 0, 0, 0, 0, 0, 1, 0],
>>> [ 1, 0, 0, 0, 1, 0, 0, 0, 0, 0]
>>> ])
>>> coordsIdx = array(
>>> [[0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 13, 13, 13,
>>> 13, 13, 13, 13, 13, 13, 13],
>>> [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4,
>>> 5, 6, 7, 8, 9]])
>>> preferredPath = PreferredPath(coordsIdx, factor=1)
>>> # without a forced path
>>> astar = Astar(grid, startNode=(0, 0), goalNode=(13, 9),
>>> heuristic='manhattan', reverseLeft=True,
>>> reverseUp=True, preferredPath=None)
>>> astar.getPath()
array(
[[13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 4, 3, 2, 1, 0],
[ 9, 9, 9, 9, 8, 8, 7, 7, 6, 5, 4, 3, 2, 1, 0]])
>>> # with a forced path
>>> astar = Astar(grid, startNode=(0, 0), goalNode=(13, 9),
>>> heuristic='manhattan', reverseLeft=True,
>>> reverseUp=True, preferredPath=preferredPath)
>>> astar.getPath()
array([
[13, 13, 13, 13, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0],
[ 9, 8, 7, 6, 5, 4, 3, 2, 2, 2, 2, 1, 0, 0, 0, 0, 0, 0]])
>>> from numpy import array
>>> from pybimstab.astar import PreferredPath, Astar
>>> grid = array([[0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 1, 1, 0, 0],
[1, 1, 0, 0, 1, 0, 1, 0, 0, 0],
[0, 0, 0, 1, 1, 0, 1, 0, 0, 1],
[0, 0, 1, 1, 0, 1, 1, 1, 0, 0],
[0, 0, 0, 1, 1, 0, 1, 0, 0, 1],
[0, 0, 0, 0, 1, 0, 0, 1, 0, 0],
[0, 0, 0, 1, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 1],
[0, 0, 1, 0, 0, 1, 0, 0, 0, 0]])
>>> astar = Astar(grid, startNode=(0, 0), goalNode=(9, 9),
>>> heuristic='manhattan', reverseLeft=True,
>>> reverseUp=True, preferredPath=None)
>>> astar.getPath()
array([[9, 8, 7, 6, 5, 4, 3, 2, 1, 0, 0, 0, 0, 0, 0, 0, 0],
[9, 8, 8, 8, 8, 8, 8, 8, 8, 7, 6, 5, 4, 3, 2, 1, 0]])
>>> astar = Astar(grid, startNode=(0, 0), goalNode=(9, 9),
>>> heuristic='euclidean', reverseLeft=True,
>>> reverseUp=True, preferredPath=None)
>>> astar.getPath()
array([[9, 8, 7, 6, 5, 4, 3, 2, 1, 0, 0, 0, 0],
[9, 8, 7, 6, 5, 4, 5, 5, 4, 3, 2, 1, 0]])
'''
import numpy as np
# Check possible errors in the goal and start cells.
self.__checkerrors()
# Required lists
openList = list() # open nodes for potential paths
closeList = list() # open nodes that have been ruled out
openList.append(self.mazeStr[self.startNode])
continueWhile = True
# Main loop
while continueWhile:
# If there is not a solution, stop the program.
if len(openList) == 0:
raise ValueError('There is not a path between the start ' +
'and goal nodes')
# Get the cost of openList
costList = [node.getGcost(fatherNode=self.mazeStr[node.father]) +
node.getHcost(goalNode=self.mazeStr[self.goalNode],
heuristic=self.heuristic,
preferredPath=self.preferredPath)
for node in openList]
# Extract tail-node of current optimum path (currentNode)
minCostIdx = costList.index(min(costList)) # Index in the list
currentNode = openList[minCostIdx] # index of the current node
openList.pop(minCostIdx) # delete currentNode from openList
closeList.append(currentNode) # add currentNode to closeList
neighbours = self.getNeighbours(currentNode) # get the neighbours
# working on the neighbours of the current node
for neighbour in neighbours:
neighbourStr = self.mazeStr[neighbour]
if (neighbourStr.gCost is not None and
np.isinf(neighbourStr.gCost)) or \
neighbourStr in closeList:
continue
# verifying if some neighbour is the goal point
if neighbour == self.goalNode:
self.mazeStr[self.goalNode].father = currentNode.pos
self.mazeStr[neighbour].getGcost(
fatherNode=self.mazeStr[currentNode.pos])
self.mazeStr[neighbour].getHcost(
goalNode=self.mazeStr[self.goalNode],
heuristic=self.heuristic,
preferredPath=self.preferredPath)
continueWhile = False
break
# If any neighbour is the goal node, A Star continues.
else:
# If the neighbour is in openList: if true, compare the
# old gCost (from the previous father) and new gCost (from
# the current point which could be the new father) if new
# cost is bigger than the old one, leave the original
# father, else, change it to the new one.
if neighbourStr in openList:
oldGcost = neighbourStr.gCost
oldFather = neighbourStr.father
# changing the attributes: father is current node.
self.mazeStr[neighbour].father = currentNode.pos
newGcost = self.mazeStr[neighbour].getGcost(
fatherNode=self.mazeStr[currentNode.pos])
if newGcost > oldGcost:
# Coming back to the previous attributes.
self.mazeStr[neighbour].father = oldFather
self.mazeStr[neighbour].getGcost(
fatherNode=self.mazeStr[oldFather])
# If the neighbour is not in the closeList, put it in the
# openList.
elif neighbourStr not in closeList:
self.mazeStr[neighbour].father = currentNode.pos
self.mazeStr[neighbour].getGcost(
fatherNode=self.mazeStr[currentNode.pos])
self.mazeStr[neighbour].getHcost(
goalNode=self.mazeStr[self.goalNode],
heuristic=self.heuristic,
preferredPath=self.preferredPath)
openList.append(self.mazeStr[neighbour])
optimumPath = self.getWayBack(self.mazeStr[self.goalNode])
setattr(self, 'optimumPath', optimumPath)
return optimumPath
[docs] def plot(self, plotPreferredPath=False):
'''Method for generating a graphic of the optimum path returned from
the ``astar`` method.
Args:
plotForcedPath (`bool`): logical variable to check if the forced
path exists and is wanted to plot.
Returns:
(`matplotlib.figure.Figure`): object with the matplotlib structure\
of the plot. You might use it to save the figure for example.
Examples:
>>> from numpy import array
>>> from pybimstab.astar import PreferredPath, Astar
>>> grid = array([[0, 0, 0, 0, 1, 0, 0, 0, 0, 0],
>>> [0, 0, 0, 0, 1, 1, 0, 0, 0, 0],
>>> [0, 0, 0, 1, 1, 0, 1, 0, 1, 0],
>>> [0, 1, 1, 0, 0, 0, 0, 1, 0, 0],
>>> [0, 0, 0, 1, 0, 0, 0, 1, 1, 1],
>>> [0, 0, 1, 0, 1, 1, 0, 0, 0, 0],
>>> [0, 0, 1, 0, 1, 0, 1, 0, 0, 0],
>>> [1, 1, 0, 1, 0, 0, 0, 0, 1, 1],
>>> [1, 0, 0, 0, 0, 0, 0, 0, 0, 0],
>>> [0, 0, 0, 0, 1, 0, 1, 0, 0, 1],
>>> [0, 1, 0, 0, 0, 1, 0, 0, 0, 0],
>>> [1, 0, 0, 0, 0, 0, 0, 0, 0, 0],
>>> [0, 0, 0, 0, 0, 0, 0, 0, 1, 0],
>>> [1, 0, 0, 0, 1, 0, 0, 0, 0, 0]
>>> ])
>>> coordsIdx = array(
>>> [[0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 13, 13, 13,
>>> 13, 13, 13, 13, 13, 13, 13],
>>> [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4,
>>> 5, 6, 7, 8, 9]])
>>> preferredPath = PreferredPath(coordsIdx, factor=1)
>>> for typePP in [None, preferredPath]:
>>> astar = Astar(grid, startNode=(0, 0), goalNode=(13, 9),
>>> heuristic='manhattan', reverseLeft=True,
>>> reverseUp=True, preferredPath=typePP)
>>> fig = astar.plot(plotPreferredPath=True)
.. figure:: https://rawgit.com/eamontoyaa/pybimstab/master/examples/figures/astar_example1a.svg
:alt: astar_example1b
.. figure:: https://rawgit.com/eamontoyaa/pybimstab/master/examples/figures/astar_example1b.svg
:alt: astar_example1a
.. only:: html
:download:`example script<../examples/figuresScripts/astar_example1.py>`.
>>> from numpy import array
>>> from pybimstab.astar import Astar
>>> grid = array([[0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 1, 1, 0, 0],
[1, 1, 0, 0, 1, 0, 1, 0, 0, 0],
[0, 0, 0, 1, 1, 0, 1, 0, 0, 1],
[0, 0, 1, 1, 0, 1, 1, 1, 0, 0],
[0, 0, 0, 1, 1, 0, 1, 0, 0, 1],
[0, 0, 0, 0, 1, 0, 0, 1, 0, 0],
[0, 0, 0, 1, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0, 1],
[0, 0, 1, 0, 0, 1, 0, 0, 0, 0]])
>>> for heuristic in ['manhattan', 'euclidean']:
>>> astar = Astar(grid, startNode=(0, 0), goalNode=(9, 9),
>>> heuristic=heuristic, reverseLeft=True,
>>> reverseUp=True, preferredPath=None)
>>> fig = astar.plot()
.. figure:: https://rawgit.com/eamontoyaa/pybimstab/master/examples/figures/astar_example2a.svg
:alt: astar_example1b
.. figure:: https://rawgit.com/eamontoyaa/pybimstab/master/examples/figures/astar_example2b.svg
:alt: astar_example2a
.. only:: html
:download:`example script<../examples/figuresScripts/astar_example2.py>`.
>>> from numpy import array
>>> from pybimstab.bim import BlocksInMatrix
>>> from pybimstab.astar import Astar
>>> seed = 111 # for repeatibilty
>>> boundary = array([[-5, 0, 5, 0, -5], [0, 10, 0, -10, 0]])
>>> bim = BlocksInMatrix(slopeCoords=boundary, blockProp=0.2,
>>> tileSize=0.4, seed=seed)
>>> astar = Astar(bim.grid, startNode=(0, 12), goalNode=(49, 12),
>>> heuristic='manhattan', reverseLeft=True,
>>> reverseUp=True, preferredPath=None)
>>> fig = astar.plot()
.. figure:: https://rawgit.com/eamontoyaa/pybimstab/master/examples/figures/astar_example3.svg
:alt: astar_example3
.. only:: html
:download:`example script<../examples/figuresScripts/astar_example3.py>`.
'''
from matplotlib import pyplot as plt
from matplotlib.colors import LinearSegmentedColormap as newcmap
import numpy as np
# Variables to control the color map and its legend
m, n = self.grid.shape # dimension of the grid
yCells, xCells = np.mgrid[slice(-0.5, m-0.5+1, 1), # grid coordinates
slice(-0.5, n-0.5+1, 1)]
if np.any(self.grid == -1): # colormap
cmap = newcmap.from_list(
'BIMcmap', ['white', 'lightgray', 'black'], 3)
ticks = [-1+0.333, 0, 1-0.333]
ticksLabels = ['Not allowed cells', 'Allowed cells',
'Hindered cells']
else:
cmap = newcmap.from_list('BIMcmap', ['lightgray', 'black'], 2)
ticks = [0.25, 0.75]
ticksLabels = ['Allowed cells', 'Hindered cells']
if m > 50 or n > 50:
edgecolor = 'None'
else:
edgecolor = 'k'
# Plot body
fig = plt.figure()
ax = fig.add_subplot(111)
bar = ax.pcolor(xCells, yCells, self.grid, cmap=cmap,
edgecolor=edgecolor)
ax.plot(self.startNode[1], self.startNode[0], '*r', label='Start node')
ax.plot(self.goalNode[1], self.goalNode[0], '.r', label='Goal node')
if plotPreferredPath and self.preferredPath is not None:
ax.plot(self.preferredPath.coordsIdx[1],
self.preferredPath.coordsIdx[0],
':r', lw=1.5, label='Preferred path')
ax.plot(self.optimumPath[1], self.optimumPath[0], '-r', lw=1.5,
label='Optimum path')
# Configuring the colorbar
bar = plt.colorbar(bar, ax=ax, ticks=ticks, pad=0.01,
shrink=0.15, aspect=3)
bar.ax.set_yticklabels(ticksLabels, fontsize='small')
# Plot settings
ax.set_aspect(1)
ax.legend(fontsize='small', bbox_to_anchor=(1.005, 1), loc=2)
plt.gca().invert_yaxis() # invert y-axis acording to de grid notation
fig.tight_layout()
return fig
# %%
'''
BSD 2 license.
Copyright (c) 2018, Universidad Nacional de Colombia, Exneyder Andres Montoya
Araque and Ludger O. Suarez-Burgoa.
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are
met:
1. Redistributions of source code must retain the above copyright notice,
this list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
'''