Author Topic: Robot mapping, A* algorithm and implementing it directly into a script.  (Read 11160 times)

0 Members and 1 Guest are viewing this topic.

Offline SciTech02Topic starter

  • Robot Overlord
  • ****
  • Posts: 136
  • Helpful? 3
  • Life's but a green duck with soy sauce. -MegaHAL
Hello, I have some questions about robot pathfinding, more specifically, the A* (A-star) algorithm.  First, I've played with a few programs that use the A* algorithm and all of them find paths that are parallel to the X and Y axis and always make 90 degree turns (that is, the path can only go up-down and left-right).  Can you find and traverse diagonal paths with the A* algorithm (turn 45 degrees and go strait), or does this require a more complex algorithm?

Second, every time I have seen this algorithm implemented, it is done with an outside GUI program (the map is created in it and path is found using it).  However, I want to have it directly in my script.  I'm sure the pathfinding part can be written into a script, but I don't know about the actual map (does the map have to be made in a GUI?).

Finally, can you have a robot deviate for the path it's traversing?  I ask this for several reasons.  One is that certain eviroments are irregular, for example: if a hallway is 2 square cells wide, the path will be close to one of the walls.  However, if you wanted the robot to travel down the middle of the hallway, can you have it leave the path close to wall, drive down the middle of the hallway and return to its original path once it exits the hallway?  The other reason is if an obstacle is blocking its path, can the robot leave its path, go around the object and return to its original path?

Sorry if I'm asking to much questions.  I also noticed this subject has become fairly popular (2 other threads somewhat related to the A* algorithm at the time of this post's writting).  ::) Sorry if it seems like I'm "spamming" this subject.
Check out the Evolution Robotics, ER1 robot, and ERSP Resource Page: http://www.societyofrobots.com/member_tutorials/node/336

Offline pomprocker

  • Supreme Robot
  • *****
  • Posts: 1,430
  • Helpful? 16
  • Sorry miss, I was giving myself an oil-job.
Re: Robot mapping, A* algorithm and implementing it directly into a script.
« Reply #1 on: September 23, 2008, 08:14:59 PM »
I'm no expert, but I'm trying to learn also. I beleive the X,Y path has to do with your Heuristic, most likely the Manhattan Method.

The map should be stored in a data structure such as a two dimensional array or a Binary Heap for larger grids/maps

The point in the node where the robot lands on does not have to be in the center, it can be toward one of the sides or a corner.

With A* it makes life a lot easier to use even numbers for everything you can.

Robots can deviate, you just have to come up with a rule to penalize certain unwanted nodes by adding to its F score and it will be less likely to go that way.

Offline SciTech02Topic starter

  • Robot Overlord
  • ****
  • Posts: 136
  • Helpful? 3
  • Life's but a green duck with soy sauce. -MegaHAL
Re: Robot mapping, A* algorithm and implementing it directly into a script.
« Reply #2 on: September 23, 2008, 10:53:02 PM »
Okay, here is what I've learned so far: the A* algorithm is a distance+cost method uses 3 "heuristic functions":

g(x)=distance from start "node" to current node
h(x)=estimated distance from current node to goal node
f(x)=sum of the above nodes

A* works with a regular X-Y grid with positive X and Y values.

You then evaluate g(x) and h(x) by measuring their defined distances (using the distance formula from algebra, since it's a X-Y coordinate plane?).  You analyze nodes in an "open list" and store analyzed nodes in a "closed list"

This is interesting and all, but I was wondering if any software exists, where I can construct maps and integrate the algorithm directly into my script (like a python "modual" script that I can "import" into my custom script).  The reason I'm asking this is because I don't want to develop my own A* algorithm, I want to use it in a robot script I'm developing (and I'm much better at editing pre-existing scripts than building them from scratch. :-\ ).
Check out the Evolution Robotics, ER1 robot, and ERSP Resource Page: http://www.societyofrobots.com/member_tutorials/node/336

Offline benji

  • Supreme Robot
  • *****
  • Posts: 832
  • Helpful? 0
Re: Robot mapping, A* algorithm and implementing it directly into a script.
« Reply #3 on: September 24, 2008, 05:57:21 AM »
when you add the adjacent 8 squares to the open list and mark the central square as their mother then you are making diagonal paths .
many tutorial in the net anyways teach A*
check threads in SOR,,there was one created by admin and many links are posted there
good ol' BeNNy

Offline SciTech02Topic starter

  • Robot Overlord
  • ****
  • Posts: 136
  • Helpful? 3
  • Life's but a green duck with soy sauce. -MegaHAL
Re: Robot mapping, A* algorithm and implementing it directly into a script.
« Reply #4 on: September 24, 2008, 04:13:02 PM »
Alright, after some in-depth reading I think I'm starting to understand this algrithm much better.  However, I still have a few more questions: one of them is, when you create a map in your script, all you have to do is create a matrix/2-dimensional array?  For example:

Let x=empty, 0=blocked, s=start cell and g=goal cell:

Y  {0 0 0 0 0 0}
a   {0 x x g x  0}
x   {0 x x x x  0}
i    {0 s x x x  0}
s   {0 0 0 0 0 0}
   ----X axis----

Is this all you have to do, or do you then add a coordinate pair for each cell (such as cell s=[1, 1] and cell g=[3, 3]) and use these to find the F-values of the cells?  Sorry if this is an overly obvious question; I'm not accustomed with, quite literaly, visual programing.
Check out the Evolution Robotics, ER1 robot, and ERSP Resource Page: http://www.societyofrobots.com/member_tutorials/node/336

Offline pomprocker

  • Supreme Robot
  • *****
  • Posts: 1,430
  • Helpful? 16
  • Sorry miss, I was giving myself an oil-job.
Re: Robot mapping, A* algorithm and implementing it directly into a script.
« Reply #5 on: September 24, 2008, 04:20:32 PM »
I assume...

You use a two dimensional array as the simplest data structure for a small grid.

map[][]= {
                  0,0,1,...
                 0,1.0....
                                };


the you have other arrays that store its different f scores, and its status
« Last Edit: September 24, 2008, 04:21:55 PM by pomprocker »

Offline SciTech02Topic starter

  • Robot Overlord
  • ****
  • Posts: 136
  • Helpful? 3
  • Life's but a green duck with soy sauce. -MegaHAL
Re: Robot mapping, A* algorithm and implementing it directly into a script.
« Reply #6 on: September 24, 2008, 05:03:57 PM »
Yeah, I figured that's how you create a map/matrix after reading admin's wavefront tutorial more carefully (he gives an example of a 2-dimensional array).  To analyze a certain cell/member of the matrix, you basically type "map[3][4]" and it analyzes cell (3, 4) as if it were on a coordinate plane, correct?  If it is, this is all starting to make sense...
Check out the Evolution Robotics, ER1 robot, and ERSP Resource Page: http://www.societyofrobots.com/member_tutorials/node/336

Offline pomprocker

  • Supreme Robot
  • *****
  • Posts: 1,430
  • Helpful? 16
  • Sorry miss, I was giving myself an oil-job.
Re: Robot mapping, A* algorithm and implementing it directly into a script.
« Reply #7 on: September 24, 2008, 05:28:05 PM »
yes thats how you access a two dimensional array.

How about binding all the pieces of information together?

you have:

coordinates
G score
H score
F score
status (open/closed)
Passable or impassable
etc....


do you used a linked list or what?

I'm actually writing a paper on this topic right now for my Algorithm Design class.
« Last Edit: September 24, 2008, 05:28:53 PM by pomprocker »

Offline SciTech02Topic starter

  • Robot Overlord
  • ****
  • Posts: 136
  • Helpful? 3
  • Life's but a green duck with soy sauce. -MegaHAL
Re: Robot mapping, A* algorithm and implementing it directly into a script.
« Reply #8 on: September 24, 2008, 08:32:26 PM »
do you used a linked list or what?

I'm actually writing a paper on this topic right now for my Algorithm Design class.

Well, I don't really know.  The programing language I'm using is python, which does not seem to be able to use matrixes or 2-dimensional arrays (please correct me if I'm wrong).  What I have written now is a list that works only in 1 dimension, but I can write it like this:

map = [0, 1, 1, 0,
           1, 0, g, 0,
           s, 1, 0, 0]

a "pseudo matrix"  ::)

You then type "map[6]" and get the cell/list member g (the first cell is 0, and you add 1 as you move "horizontally" across the list).  This is pain in the butt, because if you want to analyze the squares around cell s, you analyze: map[9], map[4] and map[5].  This can be tolerated, since the cell directly above a given cell is equal to the value of the given cell minus 4 (the "matrix" is 4 cells wide).  As for an actual A* program, I haven't written one yet, as I'm still testing some basic functions I need to know beforehand. 
Check out the Evolution Robotics, ER1 robot, and ERSP Resource Page: http://www.societyofrobots.com/member_tutorials/node/336

Offline pomprocker

  • Supreme Robot
  • *****
  • Posts: 1,430
  • Helpful? 16
  • Sorry miss, I was giving myself an oil-job.
Re: Robot mapping, A* algorithm and implementing it directly into a script.
« Reply #9 on: September 25, 2008, 10:55:38 AM »
I've found some implementation of this in C++, I'll try to post it up when I get home tonight.

Offline SciTech02Topic starter

  • Robot Overlord
  • ****
  • Posts: 136
  • Helpful? 3
  • Life's but a green duck with soy sauce. -MegaHAL
Re: Robot mapping, A* algorithm and implementing it directly into a script.
« Reply #10 on: September 25, 2008, 02:20:06 PM »
Cool, this will be interesting.  I might be able to use it as a rough template for my algorithm in python.  :)  Now about that: as I said above, python does not have the ability to use matrices or 2-dimensional arrays, using it's own syntax.  After researching python's website, it looks like you can write a matrix in C/C++ and insert it into your python script, which would work (how to do this, I don't know).  If somebody specifically knows how, please say so.
Check out the Evolution Robotics, ER1 robot, and ERSP Resource Page: http://www.societyofrobots.com/member_tutorials/node/336

Offline pomprocker

  • Supreme Robot
  • *****
  • Posts: 1,430
  • Helpful? 16
  • Sorry miss, I was giving myself an oil-job.
Re: Robot mapping, A* algorithm and implementing it directly into a script.
« Reply #11 on: September 25, 2008, 02:24:34 PM »
hmmm C/C++ and python should'nt go together imho

Java and Python sound better....Jython

Offline pomprocker

  • Supreme Robot
  • *****
  • Posts: 1,430
  • Helpful? 16
  • Sorry miss, I was giving myself an oil-job.
Re: Robot mapping, A* algorithm and implementing it directly into a script.
« Reply #12 on: September 25, 2008, 02:30:05 PM »
I talked to someone here at work that knows a little python, he said what you do is create a list of lists and that acts as your 2d array

Offline SciTech02Topic starter

  • Robot Overlord
  • ****
  • Posts: 136
  • Helpful? 3
  • Life's but a green duck with soy sauce. -MegaHAL
Re: Robot mapping, A* algorithm and implementing it directly into a script.
« Reply #13 on: September 25, 2008, 06:12:47 PM »
I talked to someone here at work that knows a little python, he said what you do is create a list of lists and that acts as your 2d array

Of course, that makes perfect sense (why didn't I think of that?).  Live and learn I guess.  ::)  I just typed up a quick, throw-away script and it works nearly the same as the 2 dimensional array described above.  You type "map[][]" and it prints that member of a list within a list (the only major difference is the first value is the Y-axes and it has 0 as the highest Y-value).  So on a 3X3 "double list", cell (1, 0) would be (2, 1) on my script.  I could probably change this though...

Edit: I fixed the part where the highest Y-value is 0, so now I just have to write the Y-value before the X-value to access a certain cell.
« Last Edit: September 25, 2008, 06:24:16 PM by SciTech02 »
Check out the Evolution Robotics, ER1 robot, and ERSP Resource Page: http://www.societyofrobots.com/member_tutorials/node/336

Offline Ro-Bot-X

  • Contest Winner
  • Supreme Robot
  • ****
  • Posts: 1,431
  • Helpful? 25
  • Store: RoBotXDesigns.ca
Re: Robot mapping, A* algorithm and implementing it directly into a script.
« Reply #14 on: September 25, 2008, 08:17:24 PM »
How does that look like? List in a list? Ugh, I envy you programmers...
Check out the uBotino robot controller!

Offline SciTech02Topic starter

  • Robot Overlord
  • ****
  • Posts: 136
  • Helpful? 3
  • Life's but a green duck with soy sauce. -MegaHAL
Re: Robot mapping, A* algorithm and implementing it directly into a script.
« Reply #15 on: September 25, 2008, 09:19:09 PM »
Like this  :P:

a = [1, 0, 0, 1]    #first list                                               |               map = [a, b, c]    OR:     #a, b, and c represent the three other lists
b = [0, 0, 1, 1]    #second list                                          |               map = [a,
c = [1, 1, 0, 0]    #third list.                                             |                          b,
                                                                                    |                          c]
This basicly represents a 2-dimensional grid.  So, now I know how to create maps.  ;D  Now I just need to piece the rest of the info together and I may have a working algorithm soon...
Check out the Evolution Robotics, ER1 robot, and ERSP Resource Page: http://www.societyofrobots.com/member_tutorials/node/336

Offline Ro-Bot-X

  • Contest Winner
  • Supreme Robot
  • ****
  • Posts: 1,431
  • Helpful? 25
  • Store: RoBotXDesigns.ca
Re: Robot mapping, A* algorithm and implementing it directly into a script.
« Reply #16 on: September 27, 2008, 06:18:25 PM »
Thanks! I can't wait to see what you come up with!
Check out the uBotino robot controller!

Offline pomprocker

  • Supreme Robot
  • *****
  • Posts: 1,430
  • Helpful? 16
  • Sorry miss, I was giving myself an oil-job.
Re: Robot mapping, A* algorithm and implementing it directly into a script.
« Reply #17 on: September 29, 2008, 12:49:28 AM »
Here is a C++ implementation I found:
Code: [Select]
/*
;===================================================================
;A* Pathfinder (Version 1.71a) by Patrick Lester. Used by permission.
;===================================================================
;Last updated 06/16/03 -- Visual C++ version
 */

//Declare constants
const mapWidth = 80, mapHeight = 60, tileSize = 10, numberPeople = 3;
int onClosedList = 10;
const notfinished = 0, notStarted = 0;// path-related constants
const found = 1, nonexistent = 2;
const walkable = 0, unwalkable = 1;// walkability array constants

//Create needed arrays
char walkability [mapWidth][mapHeight];
int openList[mapWidth*mapHeight+2]; //1 dimensional array holding ID# of open list items
int whichList[mapWidth+1][mapHeight+1];  //2 dimensional array used to record
// whether a cell is on the open list or on the closed list.
int openX[mapWidth*mapHeight+2]; //1d array stores the x location of an item on the open list
int openY[mapWidth*mapHeight+2]; //1d array stores the y location of an item on the open list
int parentX[mapWidth+1][mapHeight+1]; //2d array to store parent of each cell (x)
int parentY[mapWidth+1][mapHeight+1]; //2d array to store parent of each cell (y)
int Fcost[mapWidth*mapHeight+2]; //1d array to store F cost of a cell on the open list
int Gcost[mapWidth+1][mapHeight+1]; //2d array to store G cost for each cell.
int Hcost[mapWidth*mapHeight+2]; //1d array to store H cost of a cell on the open list
int pathLength[numberPeople+1];     //stores length of the found path for critter
int pathLocation[numberPeople+1];   //stores current position along the chosen path for critter
int* pathBank [numberPeople+1];

//Path reading variables
int pathStatus[numberPeople+1];
int xPath[numberPeople+1];
int yPath[numberPeople+1];

//-----------------------------------------------------------------------------
// Function Prototypes: where needed
//-----------------------------------------------------------------------------
void ReadPath(int pathfinderID,int currentX,int currentY, int pixelsPerFrame);
int ReadPathX(int pathfinderID,int pathLocation);
int ReadPathY(int pathfinderID,int pathLocation);


//-----------------------------------------------------------------------------
// Name: InitializePathfinder
// Desc: Allocates memory for the pathfinder.
//-----------------------------------------------------------------------------
void InitializePathfinder (void)
{
for (int x = 0; x < numberPeople+1; x++)
pathBank [x] = (int*) malloc(4);
}


//-----------------------------------------------------------------------------
// Name: EndPathfinder
// Desc: Frees memory used by the pathfinder.
//-----------------------------------------------------------------------------
void EndPathfinder (void)
{
for (int x = 0; x < numberPeople+1; x++)
{
free (pathBank [x]);
}
}


//-----------------------------------------------------------------------------
// Name: FindPath
// Desc: Finds a path using A*
//-----------------------------------------------------------------------------
int FindPath (int pathfinderID,int startingX, int startingY,
  int targetX, int targetY)
{
int onOpenList=0, parentXval=0, parentYval=0,
a=0, b=0, m=0, u=0, v=0, temp=0, corner=0, numberOfOpenListItems=0,
addedGCost=0, tempGcost = 0, path = 0,
tempx, pathX, pathY, cellPosition,
newOpenListItemID=0;

//1. Convert location data (in pixels) to coordinates in the walkability array.
int startX = startingX/tileSize;
int startY = startingY/tileSize;
targetX = targetX/tileSize;
targetY = targetY/tileSize;

//2.Quick Path Checks: Under the some circumstances no path needs to
// be generated ...

// If starting location and target are in the same location...
if (startX == targetX && startY == targetY && pathLocation[pathfinderID] > 0)
return found;
if (startX == targetX && startY == targetY && pathLocation[pathfinderID] == 0)
return nonexistent;

// If target square is unwalkable, return that it's a nonexistent path.
if (walkability[targetX][targetY] == unwalkable)
goto noPath;

//3.Reset some variables that need to be cleared
if (onClosedList > 1000000) //reset whichList occasionally
{
for (int x = 0; x < mapWidth;x++) {
for (int y = 0; y < mapHeight;y++)
whichList [x][y] = 0;
}
onClosedList = 10;
}
onClosedList = onClosedList+2; //changing the values of onOpenList and onClosed list is faster than redimming whichList() array
onOpenList = onClosedList-1;
pathLength [pathfinderID] = notStarted;//i.e, = 0
pathLocation [pathfinderID] = notStarted;//i.e, = 0
Gcost[startX][startY] = 0; //reset starting square's G value to 0

//4.Add the starting location to the open list of squares to be checked.
numberOfOpenListItems = 1;
openList[1] = 1;//assign it as the top (and currently only) item in the open list, which is maintained as a binary heap (explained below)
openX[1] = startX ; openY[1] = startY;

//5.Do the following until a path is found or deemed nonexistent.
do
{

//6.If the open list is not empty, take the first cell off of the list.
// This is the lowest F cost cell on the open list.
if (numberOfOpenListItems != 0)
{

//7. Pop the first item off the open list.
parentXval = openX[openList[1]];
parentYval = openY[openList[1]]; //record cell coordinates of the item
whichList[parentXval][parentYval] = onClosedList;//add the item to the closed list

// Open List = Binary Heap: Delete this item from the open list, which
//  is maintained as a binary heap. For more information on binary heaps, see:
// http://www.policyalmanac.org/games/binaryHeaps.htm
numberOfOpenListItems = numberOfOpenListItems - 1;//reduce number of open list items by 1

// Delete the top item in binary heap and reorder the heap, with the lowest F cost item rising to the top.
openList[1] = openList[numberOfOpenListItems+1];//move the last item in the heap up to slot #1
v = 1;

// Repeat the following until the new item in slot #1 sinks to its proper spot in the heap.
do
{
u = v;
if (2*u+1 <= numberOfOpenListItems) //if both children exist
{
//Check if the F cost of the parent is greater than each child.
//Select the lowest of the two children.
if (Fcost[openList[u]] >= Fcost[openList[2*u]])
v = 2*u;
if (Fcost[openList[v]] >= Fcost[openList[2*u+1]])
v = 2*u+1;
}
else
{
if (2*u <= numberOfOpenListItems) //if only child #1 exists
{
//Check if the F cost of the parent is greater than child #1
if (Fcost[openList[u]] >= Fcost[openList[2*u]])
v = 2*u;
}
}

if (u != v) //if parent's F is > one of its children, swap them
{
temp = openList[u];
openList[u] = openList[v];
openList[v] = temp;
}
else
break; //otherwise, exit loop

}
while (!KeyDown(27));//reorder the binary heap


//7.Check the adjacent squares. (Its "children" -- these path children
// are similar, conceptually, to the binary heap children mentioned
// above, but don't confuse them. They are different. Path children
// are portrayed in Demo 1 with grey pointers pointing toward
// their parents.) Add these adjacent child squares to the open list
// for later consideration if appropriate (see various if statements
// below).
for (b = parentYval-1; b <= parentYval+1; b++){
for (a = parentXval-1; a <= parentXval+1; a++){

// If not off the map (do this first to avoid array out-of-bounds errors)
if (a != -1 && b != -1 && a != mapWidth && b != mapHeight){

// If not already on the closed list (items on the closed list have
// already been considered and can now be ignored).
if (whichList[a][b] != onClosedList) {

// If not a wall/obstacle square.
if (walkability [a][b] != unwalkable) {

// Don't cut across corners
corner = walkable;
if (a == parentXval-1)
{
if (b == parentYval-1)
{
if (walkability[parentXval-1][parentYval] == unwalkable
|| walkability[parentXval][parentYval-1] == unwalkable) \
corner = unwalkable;
}
else if (b == parentYval+1)
{
if (walkability[parentXval][parentYval+1] == unwalkable
|| walkability[parentXval-1][parentYval] == unwalkable)
corner = unwalkable;
}
}
else if (a == parentXval+1)
{
if (b == parentYval-1)
{
if (walkability[parentXval][parentYval-1] == unwalkable
|| walkability[parentXval+1][parentYval] == unwalkable)
corner = unwalkable;
}
else if (b == parentYval+1)
{
if (walkability[parentXval+1][parentYval] == unwalkable
|| walkability[parentXval][parentYval+1] == unwalkable)
corner = unwalkable;
}
}
if (corner == walkable) {

// If not already on the open list, add it to the open list.
if (whichList[a][b] != onOpenList)
{

//Create a new open list item in the binary heap.
newOpenListItemID = newOpenListItemID + 1; //each new item has a unique ID #
m = numberOfOpenListItems+1;
openList[m] = newOpenListItemID;//place the new open list item (actually, its ID#) at the bottom of the heap
openX[newOpenListItemID] = a;
openY[newOpenListItemID] = b;//record the x and y coordinates of the new item

//Figure out its G cost
if (abs(a-parentXval) == 1 && abs(b-parentYval) == 1)
addedGCost = 14;//cost of going to diagonal squares
else
addedGCost = 10;//cost of going to non-diagonal squares
Gcost[a][b] = Gcost[parentXval][parentYval] + addedGCost;

//Figure out its H and F costs and parent
Hcost[openList[m]] = 10*(abs(a - targetX) + abs(b - targetY));
Fcost[openList[m]] = Gcost[a][b] + Hcost[openList[m]];
parentX[a][b] = parentXval ; parentY[a][b] = parentYval;

//Move the new open list item to the proper place in the binary heap.
//Starting at the bottom, successively compare to parent items,
//swapping as needed until the item finds its place in the heap
//or bubbles all the way to the top (if it has the lowest F cost).
while (m != 1) //While item hasn't bubbled to the top (m=1)
{
//Check if child's F cost is < parent's F cost. If so, swap them.
if (Fcost[openList[m]] <= Fcost[openList[m/2]])
{
temp = openList[m/2];
openList[m/2] = openList[m];
openList[m] = temp;
m = m/2;
}
else
break;
}
numberOfOpenListItems = numberOfOpenListItems+1;//add one to the number of items in the heap

//Change whichList to show that the new item is on the open list.
whichList[a][b] = onOpenList;
}

//8.If adjacent cell is already on the open list, check to see if this
// path to that cell from the starting location is a better one.
// If so, change the parent of the cell and its G and F costs.
else //If whichList(a,b) = onOpenList
{

//Figure out the G cost of this possible new path
if (abs(a-parentXval) == 1 && abs(b-parentYval) == 1)
addedGCost = 14;//cost of going to diagonal tiles
else
addedGCost = 10;//cost of going to non-diagonal tiles
tempGcost = Gcost[parentXval][parentYval] + addedGCost;

//If this path is shorter (G cost is lower) then change
//the parent cell, G cost and F cost.
if (tempGcost < Gcost[a][b]) //if G cost is less,
{
parentX[a][b] = parentXval; //change the square's parent
parentY[a][b] = parentYval;
Gcost[a][b] = tempGcost;//change the G cost

//Because changing the G cost also changes the F cost, if
//the item is on the open list we need to change the item's
//recorded F cost and its position on the open list to make
//sure that we maintain a properly ordered open list.
for (int x = 1; x <= numberOfOpenListItems; x++) //look for the item in the heap
{
if (openX[openList[x]] == a && openY[openList[x]] == b) //item found
{
Fcost[openList[x]] = Gcost[a][b] + Hcost[openList[x]];//change the F cost

//See if changing the F score bubbles the item up from it's current location in the heap
m = x;
while (m != 1) //While item hasn't bubbled to the top (m=1)
{
//Check if child is < parent. If so, swap them.
if (Fcost[openList[m]] < Fcost[openList[m/2]])
{
temp = openList[m/2];
openList[m/2] = openList[m];
openList[m] = temp;
m = m/2;
}
else
break;
}
break; //exit for x = loop
} //If openX(openList(x)) = a
} //For x = 1 To numberOfOpenListItems
}//If tempGcost < Gcost(a,b)

}//else If whichList(a,b) = onOpenList
}//If not cutting a corner
}//If not a wall/obstacle square.
}//If not already on the closed list
}//If not off the map
}//for (a = parentXval-1; a <= parentXval+1; a++){
}//for (b = parentYval-1; b <= parentYval+1; b++){

}//if (numberOfOpenListItems != 0)

//9.If open list is empty then there is no path.
else
{
path = nonexistent; break;


//If target is added to open list then path has been found.
if (whichList[targetX][targetY] == onOpenList)
{
path = found; break;
}

}
while (1);//Do until path is found or deemed nonexistent

//10.Save the path if it exists.
if (path == found)
{

//a.Working backwards from the target to the starting location by checking
// each cell's parent, figure out the length of the path.
pathX = targetX; pathY = targetY;
do
{
//Look up the parent of the current cell.
tempx = parentX[pathX][pathY];
pathY = parentY[pathX][pathY];
pathX = tempx;

//Figure out the path length
pathLength[pathfinderID] = pathLength[pathfinderID] + 1;
}
while (pathX != startX || pathY != startY);

//b.Resize the data bank to the right size in bytes
pathBank[pathfinderID] = (int*) realloc (pathBank[pathfinderID],
pathLength[pathfinderID]*8);

//c. Now copy the path information over to the databank. Since we are
// working backwards from the target to the start location, we copy
// the information to the data bank in reverse order. The result is
// a properly ordered set of path data, from the first step to the
// last.
pathX = targetX ; pathY = targetY;
cellPosition = pathLength[pathfinderID]*2;//start at the end
do
{
cellPosition = cellPosition - 2;//work backwards 2 integers
pathBank[pathfinderID] [cellPosition] = pathX;
pathBank[pathfinderID] [cellPosition+1] = pathY;

//d.Look up the parent of the current cell.
tempx = parentX[pathX][pathY];
pathY = parentY[pathX][pathY];
pathX = tempx;

//e.If we have reached the starting square, exit the loop.
}
while (pathX != startX || pathY != startY);

//11.Read the first path step into xPath/yPath arrays
ReadPath(pathfinderID,startingX,startingY,1);

}
return path;


//13.If there is no path to the selected target, set the pathfinder's
// xPath and yPath equal to its current location and return that the
// path is nonexistent.
noPath:
xPath[pathfinderID] = startingX;
yPath[pathfinderID] = startingY;
return nonexistent;
}




//==========================================================
//READ PATH DATA: These functions read the path data and convert
//it to screen pixel coordinates.
void ReadPath(int pathfinderID,int currentX,int currentY,
  int pixelsPerFrame)
{
/*
; Note on PixelsPerFrame: The need for this parameter probably isn't
; that obvious, so a little explanation is in order. This
; parameter is used to determine if the pathfinder has gotten close
; enough to the center of a given path square to warrant looking up
; the next step on the path.
;
; This is needed because the speed of certain sprites can
; make reaching the exact center of a path square impossible.
; In Demo #2, the chaser has a velocity of 3 pixels per frame. Our
; tile size is 50 pixels, so the center of a tile will be at location
; 25, 75, 125, etc. Some of these are not evenly divisible by 3, so
; our pathfinder has to know how close is close enough to the center.
; It calculates this by seeing if the pathfinder is less than
; pixelsPerFrame # of pixels from the center of the square.

; This could conceivably cause problems if you have a *really* fast
; sprite and/or really small tiles, in which case you may need to
; adjust the formula a bit. But this should almost never be a problem
; for games with standard sized tiles and normal speeds. Our smiley
; in Demo #4 moves at a pretty fast clip and it isn't even close
; to being a problem.
*/

int ID = pathfinderID; //redundant, but makes the following easier to read

//If a path has been found for the pathfinder ...
if (pathStatus[ID] == found)
{

//If path finder is just starting a new path or has reached the
//center of the current path square (and the end of the path
//hasn't been reached), look up the next path square.
if (pathLocation[ID] < pathLength[ID])
{
//if just starting or if close enough to center of square
if (pathLocation[ID] == 0 ||
(abs(currentX - xPath[ID]) < pixelsPerFrame && abs(currentY - yPath[ID]) < pixelsPerFrame))
pathLocation[ID] = pathLocation[ID] + 1;
}

//Read the path data.
xPath[ID] = ReadPathX(ID,pathLocation[ID]);
yPath[ID] = ReadPathY(ID,pathLocation[ID]);

//If the center of the last path square on the path has been
//reached then reset.
if (pathLocation[ID] == pathLength[ID])
{
if (abs(currentX - xPath[ID]) < pixelsPerFrame
&& abs(currentY - yPath[ID]) < pixelsPerFrame) //if close enough to center of square
pathStatus[ID] = notStarted;
}
}

//If there is no path for this pathfinder, simply stay in the current
  //location.
else
{
xPath[ID] = currentX;
yPath[ID] = currentY;
}
}


//The following two functions read the raw path data from the pathBank.
//You can call these functions directly and skip the readPath function
//above if you want. Make sure you know what your current pathLocation
//is.

//-----------------------------------------------------------------------------
// Name: ReadPathX
// Desc: Reads the x coordinate of the next path step
//-----------------------------------------------------------------------------
int ReadPathX(int pathfinderID,int pathLocation)
{
int x;
if (pathLocation <= pathLength[pathfinderID])
{

//Read coordinate from bank
x = pathBank[pathfinderID] [pathLocation*2-2];

//Adjust the coordinates so they align with the center
//of the path square (optional). This assumes that you are using
//sprites that are centered -- i.e., with the midHandle command.
//Otherwise you will want to adjust this.
x = tileSize*x + .5*tileSize;

}
return x;
}


//-----------------------------------------------------------------------------
// Name: ReadPathY
// Desc: Reads the y coordinate of the next path step
//-----------------------------------------------------------------------------
int ReadPathY(int pathfinderID,int pathLocation)
{
int y;
if (pathLocation <= pathLength[pathfinderID])
{

//Read coordinate from bank
y = pathBank[pathfinderID] [pathLocation*2-1];

//Adjust the coordinates so they align with the center
//of the path square (optional). This assumes that you are using
//sprites that are centered -- i.e., with the midHandle command.
//Otherwise you will want to adjust this.
y = tileSize*y + .5*tileSize;

}
return y;
}






Offline paulstreats

  • Supreme Robot
  • *****
  • Posts: 1,381
  • Helpful? 21
Re: Robot mapping, A* algorithm and implementing it directly into a script.
« Reply #18 on: September 29, 2008, 08:10:51 AM »
Traversing diagonally on a grid system isnt all that difficult, youve just got to come up with an algorythm to do it. Think about how a computer draws a diagonal line onto the screen. The screen is made up of a grid. The bresenham algorythm was one of the first for plotting lines between one pojnt and another, it should be quite easy to convert it for robotics use.


Offline SciTech02Topic starter

  • Robot Overlord
  • ****
  • Posts: 136
  • Helpful? 3
  • Life's but a green duck with soy sauce. -MegaHAL
Re: Robot mapping, A* algorithm and implementing it directly into a script.
« Reply #19 on: September 30, 2008, 10:59:53 AM »
UPDATE:  I'm getting closer to finishing my A* algorithm (I haven't forgot about it).  I'm just taking my time to make sure I'm doing everything right (I have never wrote a program like this before  :-\).  Also, the one I'm writting is going to start out very basic; so I'll have one that just simply works, which I can modify afterward and make better (with diagonal paths and such).

About diagonal paths, paulstreats, I had this idea of using simple trigonometry/pythagoran theorem to find the hypontenuse (the diagonal distance needed to be traveled) and the angle the bot would have to turn to aim at its goal location (I would basically look at L-turns as 2 legs of a right triangle).  It seems like this method could work, right?

Thanks for all your comments so far.
Check out the Evolution Robotics, ER1 robot, and ERSP Resource Page: http://www.societyofrobots.com/member_tutorials/node/336

 


Get Your Ad Here