A* (A star) algorithm, fully functional at last.

[SciTech02]

After some heavy research/tinkering, I have fixed all the bugs in my A* (A-star) algorithm.  I solved the dreaded parent cell problem by creating a second, 2-dimensional array that stores the parent cell of a given cell, for later analysis.  I also changed how the next cell in the open list is found.  Instead of choosing a cell with an F-score lower than the current cell, it chooses the cell in the list with the lowest F-score (regardless of the current cell's F-score).  This keeps the program from crashing when no cell has an F-score lower than the current cell.

Well, enough discourse.  I have attached the program to this post and inserted the code below:

Code: [Select]

#Version 2 is nearly the same as version 1, but all the bugs are gone.  Now a
#clear and readable path is found.
#
#This is an example of the A* pathfinding algorithm I wrote in Python.  This
#program is primarly based on Abraham L. Howell's ER1 Mapper made specifically
#for the ER1 robot.  His program was written in Visual Basic, so I tried to
#"translate" it to Python as best I can.  All credit goes to him for the basic
#code I used as a general guide to create this.  You are free to use, edit and
#distribute this as long as credit is given where appropriate.
#
#The code is very basic right now.  You create a map in the grid below; it can
#be any size as long as if it stays a rectangle or square (it does not have to
#be one, but it is significantly easier if it is).  This grid is basically a
#list within a list: lists a through e are the rows and they are in list "map".
#The bottom-left cell is reached by typing "map[0][0]".  The first x and y
#coordinates are 0 (so if you have 5 rows, the bottom is row 0 and the top is
#row 4).  You enter the x and y coordinates of the start and goal cells to the
#"start" and "target" variables appropriately (see below).  You then run the
#program and it finds a path to the start to goal cell.  It then prints the
#entire map again with the path marked.  That is all it does for now; it finds
#a path.  Other details about this code can be discovered by reading this code
#or Howell's original code/documentation.

a = [0, 0, 0, 0, 0, 0, 0, 0]
b = [1, 1, 1, 1, 1, 1, 1, 0]   #0 means cell is blocked and 1 means cell is
c = [1, 1, 1, 1, 1, 1, 1, 0]   #unblocked.  When accessing a cell, type y-
d = [0, 0, 1, 1, 1, 1, 1, 0]   #coordinate first, then the x-coordinate.  like:
e = [0, 0, 0, 0, 0, 0, 0, 0]   #"map[4][0]" to access the top left cell in a.
map = [e,                   #list 0
       d,                   #list 1
       c,                   #list 2
       b,                   #list 3
       a]                   #list 4

openlist = []                           #open list is empty
closedlist = []                         #closed list is empty
targetX = 6                             #goal cell X coordinate
targetY = 1                             #goal cell Y coordinate
startX = 0                              #start X coordiante
startY = 3                              #start Y coordinate
numpasses = 0           #Used to count how many times the while-loop looped.

#These are the heuristic constants.  If Dg > Dh, more cells are analyzed and a
#more ideal path is found.  If Dh >= Dg, less cells are analyzed and a path is
#found quicker.
Dg = 1
Dh = 1

maxValX = 7                             #x value of a cell cannot be greater
maxValY = 4                             #y value of a cell cannot be greater

#--------------------------------------------------------------------
#This part of the code creates a 2-dimensional array for storing parent cells.
#For example: Pcell[2][3] = [2, 2].  It has the same dimensions and size as the
#above map, so no "cell is out of range" errors can occur.  It works the same as
#the map above; y value first and x second.  Example: Pcell[y][x] = [NewX, NewY]

ap = [0, 0, 0, 0, 0, 0, 0, 0]
bp = [0, 0, 0, 0, 0, 0, 0, 0]
cp = [0, 0, 0, 0, 0, 0, 0, 0]
dp = [0, 0, 0, 0, 0, 0, 0, 0]
ep = [0, 0, 0, 0, 0, 0, 0, 0]
Pcell = [ep, dp, cp, bp, ap]

#--------------------------------------------------------------------
#this part of the code establishes the telnet connection to the RCC and is
#independent and unrelated to the A* algorithm or finding a path (it is only
#used to physically move the robot after the path is found).

import telnetlib
import time
import string

HOST = "localhost"

print "\nOpening Telnet session to ER1 GUI."
tn = telnetlib.Telnet(HOST, 9000)
print "Opened session.\n"

def evocon (cmd, timeout=1):
          a=''
          if cmd=='events':
                 tn.write("%s\n" % cmd)
                 time.sleep(.1)
                 a=tn.read_until("\n", timeout)
                 print a
                 if a != '':
                        while a!= '':
                               a=tn.read_until("\n", timeout)
                               print a
          tn.write("%s\n" % cmd)
          return tn.read_until("\n", timeout)

#---------------------------------------------------------------------
#These are the mathamatical heuristics, used to calculate the so-called G,
#H and F scores of a cell.

def G(a):
    answerG = Dg * (abs(a[0] - startX) + abs(a[1] - startY))
    return answerG

def H(a):
    Hscore = Dh * (abs(a[0] - targetX) + abs(a[1] - targetY))
    return Hscore

def F(a):
    Fscore = G(a) + H(a)
    return Fscore

#----------------------------------------------------------------------
#When the goal cell is found, print map with marked cells.
def ShowPath():
    #Display that a path was found and how many loops were made to find it.
    print "Path found! ", "Number of loops =", numpasses

    map[targetY][targetX] = 5        #mark goal cell with integer 5, so you
                                     #can tell where the path ends.
    curCellX = targetX               #Start with target cell
    curCellY = targetY

    intPathLength = 0           #Path length is 0 (it has not been found yet)

    #Find the parent of the goal cell, then find that cell's parent and so on,
    #until, the current cell is the start cell.  This trail of parent cells is
    #the found path.
    while curCellX != startX or curCellY != startY:
        intPathLength = intPathLength + 1       #Record the length of the path
        P = Pcell[curCellY][curCellX]
        curCellX = P[0]
        curCellY = P[1]
        map[curCellY][curCellX] = 4  #Mark each parent cell with a 4

    print "Path length =", intPathLength

    map.reverse()                    #reverse list "map", so when printed in the
    for i in map:                    #for: loop, it comes out right-side up
        print i

def NoSolutionFound():
    print "No path found, the goal cell is unreachable"

#----------------------------------------------------------------------
#This part of the code is the actual A* algorithm.

curCellX = startX                             #start cell is the current cell
curCellY = startY

closedlist.append([startX, startY])           #add start cell to closed list

rest = 0                              #start search.  I set up a dummy variable,
while not rest:                       #so it would loop.

      numpasses = numpasses + 1       #Counts how many times it has looped.

      NcellX = curCellX
      NcellY = curCellY + 1
      ScellX = curCellX
      ScellY = curCellY - 1
      EcellX = curCellX + 1
      EcellY = curCellY
      WcellX = curCellX -1
      WcellY = curCellY

      #check if north cell is in range, that is, not under 0 or over max value
      if NcellX >= 0 and NcellY >= 0 and NcellX <= maxValX and NcellY <= maxValY:
         #check if it's on the closed list or blocked.  Its blocked if the cell
         #NcellX and NcellY represent is = 0, open is = 1   
         if map[NcellY][NcellX] != 0 and closedlist.count([NcellX, NcellY]) == 0:
             #if it's not on the openlist, add it and set parent.
             if openlist.count([NcellX, NcellY]) == 0:
                 openlist.append([NcellX, NcellY])
                 Pcell[NcellY][NcellX] = [curCellX, curCellY]
             else:
                 #check if path from current cell has a lower G value
                 if G([curCellX, curCellY]) + Dg < G([NcellX, NcellY]):
                     #if it is, set parent to current cell and recalculate G&F scores
                     #for this cell
                     Pcell[NcellY][NcellX] = [curCellX, curCellY]
                     G([NcellX, NcellY]) == G([curCellX, curCellY]) + Dg
                     F([NcellX, NcellY]) == G([NcellX, NcellY]) + H([NcellX, NcellY])

      #repeat above, only for south, east and west cells.
      if ScellX >= 0 and ScellY >= 0 and ScellX <= maxValX and ScellY <= maxValY:
         if map[ScellY][ScellX] != 0 and closedlist.count([ScellX, ScellY]) == 0:
              if openlist.count([ScellX, ScellY]) == 0:
                  openlist.append([ScellX, ScellY])
                  Pcell[ScellY][ScellX] = [curCellX, curCellY]
              else:
                  if G([curCellX, curCellY]) + Dg < G([ScellX, ScellY]):
                      Pcell[ScellY][ScellX] = [curCellX, curCellY]
                      G([ScellX, ScellY]) == G([curCellX, curCellY]) + Dg
                      F([ScellX, ScellY]) == G([ScellX, ScellY]) + H([ScellX, ScellY])

      if EcellX >= 0 and EcellY >= 0 and EcellX <= maxValX and EcellY <= maxValY:
         if map[EcellY][EcellX] != 0 and closedlist.count([EcellX, EcellY]) == 0:
              if openlist.count([EcellX, EcellY]) == 0:
                  openlist.append([EcellX, EcellY])
                  Pcell[EcellY][EcellX] = [curCellX, curCellY]
              else:
                  if G([curCellX, curCellY]) + Dg < G([EcellX, EcellY]):
                      Pcell[EcellY][EcellX] = [curCellX, curCellY]
                      G([EcellX, EcellY]) == G([curCellX, curCellY]) + Dg
                      F([EcellX, EcellY]) == G([EcellX, EcellY]) + H([EcellX, EcellY])

      if WcellX >= 0 and WcellY >= 0 and WcellX <= maxValX and WcellY <= maxValY:
         if map[WcellY][WcellX] != 0 and closedlist.count([WcellX, WcellY]) == 0:
              if openlist.count([WcellX, WcellY]) == 0:
                  openlist.append([WcellX, WcellY])
                  Pcell[WcellY][WcellX] = [curCellX, curCellY]
              else:
                  if G([curCellX, curCellY]) + Dg < G([WcellX, WcellY]):
                      Pcell[WcellY][WcellX] = [curCellX, curCellY]
                      G([WcellX, WcellY]) == G([curCellX, curCellY]) + Dg
                      F([WcellX, WcellY]) == G([WcellX, WcellY]) + H([WcellX, WcellY])

      if len(openlist) == 0:                         #if openlist is empty, exit
          NoSolutionFound()                          #loop; there is no solution.
          break

      #now we find the cell on the openlist with the lowest F score and set it
      #as the current cell.
      smallest = F(openlist[0])         #Assume the first member is the smallest.
      for r in openlist:
          if F(r) <= smallest:          #If an item on the list is smaller than
              smallest = F(r)           #the first item, set it as the new smallest
              NewX = r[0]
              NewY = r[1]           #Keep looping until a smaller value is found

      #add the cell you just selected to the closed list and remove it from the
      #open list.       
      closedlist.append([NewX, NewY])
      openlist.remove([NewX, NewY])

      #Find out if the cell you put in the closed list is the target cell
      #else, mark the current cell, so when you print "map", you can see it
      #was analyzed (analyzed cells are marked with the number 3)
      if NewX == targetX and NewY == targetY:
          ShowPath()
          break

      if NewX > -1 and NewY > -1 and NewX < maxValX and NewY < maxValY:
          map[NewY][NewX] = 3

      curCellX = NewX
      curCellY = NewY

I've been thinking that this may be the first time ever that the A* (A-star) algorithm has been written in Python.  I don't know for sure, but I've looked everywhere for one on the net without success, so it's possible.  Also, to Admin: I've heard you're creating an A* tutorial and that links to certain forum topics were in it.  Please feel free to use this code (Actually, everyone feel free to use it).  I know it's not written in C/C++, but it might be useful.

Thank you to everyone on this forum; I could not have done this without your guy's help.

[Ro-Bot-X]

Thank you for taking the time and effort to write this down. I wish I'd have the time to dust off my ER1 to play around with your code... Eh, maybe during the winter, when I'll have less work. Right now it's hard to even follow through with my current projects.

What do you think it will be the next step? Making a list of places coordinates so the robot can go where you tell him to go? Something like Couch = Map[3][2], Coffe table = Map[4][7] and so on. And a grammar list for the speech recognition engine that contains the commands (Go to, Bring, Come here, Rub my back, Vacoom, Walk the Dog, etc.), places to go (Couch, Fridge, Table, ...) objects to bring (Beer, Coffe, Socks, Newspaper, ...) and so on... Oh, I've got ideeas, just don't know how to make them work (yet). I've got to stop now, this is allready steering me away from my projects and... oh well.

By the way, the coolest thing I read about what other people did with their ER1 was about a guy that had his robot walk his dog around the block! He was watching remotely on his computer what the robot would see and even had the robot saying Good morning to the neighbours! He was saying that nobody dared to touch the robot because of the dog, hehe.

[szhang]

I didn't carefully read through all of the code, but from the comments I see some thing fishy...

Code: [Select]

#These are the heuristic constants.  If Dg > Dh, more cells are analyzed and a
#more ideal path is found.  If Dh >= Dg, less cells are analyzed and a path is
#found quicker.
A* should be provably optimal for an admissible heuristic.  Different heuristics might affect how long it takes to find the solution, but not whether if finds the optimal one.

Also, if speed is important to you, you should implement A* with heaps.

There are python implementations on the net, you have to google "A-star" instead b/c google interprets * as a special character.  I had a lot of trouble finding code for AO* a while back, because of that same problem.

[SciTech02]

Yeah, the Dg and Dh constants were part of the original program and I carried them over to my script.  What is meant by "optimal" is that the path is shorter.  For example, I noticed if Dg < Dh, the path will zig-zag towards the goal cell, if the map is very open.  However, if Dg > Dh, the path will be much more direct and more cells are analyzed (which, in theory, takes longer).  But I agree with you; every other A* (A-star) code I've seen does not use these constants.  Also, I planed on creating a binary heap sometime soon, but speed is not an issue for me, plus, I don't know how to yet.  My goal right now is to have my robot actually traverse the path it found (should see results soon).  Once that is fully working, I'll make it better.

[szhang]

Hm, where *exactly* did you get the code from?  It clearly isn't A* if it can't find the shortest path (assuming the heuristic is admissible, and the manhattan distance is an admissible heuristic).  If all you want to do is to find a path, DFS (with node marking) is faster and works just fine.

I looked at the code more, and it raised more questions:

Code: [Select]

#---------------------------------------------------------------------
#These are the mathamatical heuristics, used to calculate the so-called G,
#H and F scores of a cell.

def G(a):
    answerG = Dg * (abs(a[0] - startX) + abs(a[1] - startY))
    return answerG

def H(a):
    Hscore = Dh * (abs(a[0] - targetX) + abs(a[1] - targetY))
    return Hscore

def F(a):
    Fscore = G(a) + H(a)
    return Fscore
What is this? g(x) shouldn't be an estimate!!!

g(x): the actual shortest distance traveled from initial node to current node

I would suggest looking at another (correct) reference implementation.  Also, how big are your mazes going to be?  You might want to just do BFS instead.

[SciTech02]

Hmm...  Technically, G(x) isn't really an estimate, it calculates the actual distance between the current cell and the start cell (of course, it may refer to the actual distance traveled from the start cell following the path it created).    If you're curious, here's the link to the site with the original program:

http://www.abotics.com/buildit_yourself.htm (go to the very bottom of the page).  He has the program for download and two PDF files related to it.  He says it's an A* (A-star) mapping program.

Suffice to say, it works well and should work alright for me.  I plan on creating larger maps (about the size of an average household) and if it, for some reason, does not work, I'll change it.

[szhang]

G(x) IS an estimate in the program you had.

G(x) in the A* algorithm is the length of the path it took to get to x.  The manhanntan distance (abs(dx)+abs(dy)) is only an estimate that doesn't take into account blocked cells.

I am pointing this out because Admin linked to this post from his A* demo page, and I think it would be bad for people who never learned A* to learn the wrong one.