A* ALGORITHM DEVELOPMENT

A* Algorithm Development: Service

OVERVIEW

A* is a type of search algorithm that is commonly used for robotic path planning. As a final project for our Robotic Manipulation class for the MSR Program, we were supposed to implement this algorithm in any language of our choice (I chose Python) and demonstrate it with given robot radius, starting coordinates, obstacle coordinates, and goal coordinates during our final exam time.

The final algorithm takes into account the above specifications as well as the coordinates for potential nodes, the radii of the obstacles, and the radii of the boundary.

A* Algorithm Development: Text

CODE

import numpy as np
import matplotlib.pyplot as plt
import time
import math
import itertools
import random

def collision_check(robot_rad,nodes_coord,obs_radii,obs_coord,region_length,region_width):
#Using this function, check to see whether the nodes are within obstacles, or too close to obstacles for the robot to actually go to; this includes walls near the location of the nodes

b= nodes_coord
a = []

   #Here, we need to take into account the planar region within which the robot is allowed to travel
   x_region_min = 0
   y_region_min = 0

   x_region_max = region_width
   y_region_max = region_length

   for i in range(len(nodes_coord)):
       #Find the coordinates of the node
       xcoordn = nodes_coord[i][0]
       ycoordn = nodes_coord[i][1]

       for j in range(len(obs_radii)):
           #Create the minimum allowable distance between the center of the obstacle and the center of the robot such that the robot can actually travel past it
          clearance = robot_rad+obs_radii[j]

          #Find the center of the obstacle
           x_obscent = obs_coord[j][0]
           y_obscent = obs_coord[j][1]

#Find the distance between the obstacle center and the node center
dist = ((xcoordn-x_obscent)**2 + (ycoordn-y_obscent)**2)**0.5

           if dist < clearance:
               #If the distance between the node and the obstacle is less than the minimum allowable area the robot can travel between, then it is allowed to passs
               a.append(i)
               break

   for k in reversed(range(len(nodes_coord))):
       xcoordn = nodes_coord[k][0]
       ycoordn = nodes_coord[k][1]

#Check here to see if the robot can travel to a node that is close to the wall

x_min_region_check = xcoordn - robot_rad
y_min_region_check = ycoordn - robot_rad

x_max_region_check = xcoordn + robot_rad
y_max_region_check = ycoordn + robot_rad

       if x_min_region_check < x_region_min or x_max_region_check > x_region_max or y_min_region_check < y_region_min or y_max_region_check > y_region_max:
           # print "if statement pop"
           # print "pre pop b", b
           b.pop(k)
           # print "b", b
           # nodes_coord.pop(k)
           # print "nodes_coord", nodes_coord
       elif k in a:
           b.pop(k)
           # nodes_coord.pop(k)

# print "b", b
return b

def edges(robot_rad,nodes_coord,obs_radii,obs_coord):
#Here, we are finding all of the possible paths between nodes
obs_number = len(obs_radii)

collision = []
edgelist = []

l = -1

   for i in itertools.permutations(nodes_coord,2):
       #Here, we are finding all of the different combinations of the nodes, taking into account the proximity with which the robot will be traveling next to obstacles on the way to that node
       xcoordn1 = i[0][0]
       xcoordn2 = i[1][0]

ycoordn1 = i[0][1]
ycoordn2 = i[1][1]

l = l+1

       #Add every node combination to the edgelist as we go, then we can pop them out at the end
       edgelist.append([[xcoordn1,ycoordn1],[xcoordn2,ycoordn2]])

       for j in range(obs_number):
           #Find the x and y coordinates of the center of the obstacle
           x_obscent = obs_coord[j][0]
           y_obscent = obs_coord[j][1]

           #Need to check whether the line between the two nodes is a horizontal or verticle line, so that can be taken into account for the slope
           if ycoordn1==ycoordn2:
               xint = x_obscent
               yint = ycoordn1
           elif xcoordn1==xcoordn2:
               yint = y_obscent
               xint = xcoordn1
           else:
           #Find the equation of the line between the two nodes and the equation of the perpendicular line that goes through the center of the obstacle and intersects with the line between the nodes; find this point of intersection
               m = (ycoordn2-ycoordn1)/((xcoordn2-xcoordn1)*1.0)
               b1 = ycoordn1 - m*xcoordn1

               #The slope of the intersecting line is negative 1 divided by the slope of the of the line that it's intersecting
               b2 = y_obscent + (1/m)*x_obscent
               xint = (b2-b1)/(m+(1/m))
               yint = m*xint + b1

           #Calculate the minimum distance the robot can travel between obstacles (the clearance)
           clearance = robot_rad+obs_radii[j]
           dist_ro = ((xint-x_obscent)**2 + (yint-y_obscent)**2)**0.5

#Checking the distance between the intersection point and each of the nodes to check whether the obstacle is sitting outside of the range of the two nodes or not

           dist1 = ((xint-xcoordn1)**2 + (yint-ycoordn1)**2)**0.5
           dist2 = ((xint-xcoordn2)**2 + (yint-ycoordn2)**2)**0.5
           distn = ((xcoordn1-xcoordn2)**2 + (ycoordn1-ycoordn2)**2)**0.5

           #Finding collisions when the distance that the robot has to travel is actually less than the allowable range that the robot has to travel between
           if dist_ro < clearance and dist1 < distn and dist2 < distn:
               collision.append(l)

# print "COLLISION o_O with:",obs_coord[j],"between:",i

   for k in reversed(range(len(edgelist))):
       if k in collision:
           #Taking the node combinations between which there is a collision out since the robot can't actually travel between them- this will give us our paths possible
           edgelist.pop(k)

return edgelist

def create_graph(paths_possible,nodes_coord):
   #Creating a graph from the possible paths and the node coordinates... this was back calculated since I started with making the astar for question two.
   nodes_to = []
   nodes_to_ind = []
   nodes_from = []
   nodes_from_ind = []
   dist= []
   nodes_total = len(nodes_coord)

   for i in range(len(paths_possible)):
       #Create an array that has the nodes that we're leaving from and the ones that we're going to
       nodes_to.append(paths_possible[i][0])
       nodes_from.append(paths_possible[i][1])
   for j in range(len(nodes_to)):
       #We need to know which node numbers the paths possible elements correspond to in order to create teh graph
       nodes_to_ind.append(nodes_coord.index(nodes_to[j]))
       nodes_from_ind.append(nodes_coord.index(nodes_from[j]))
   for k in range(len(nodes_to)):
       #Need to calculate the distane between them
       node1x = nodes_to[k][0]
       node1y = nodes_to[k][1]
       node2x = nodes_from[k][0]
       node2y = nodes_from[k][1]
       distn = (((node2x-node1x)**2 + (node2y-node1y)**2))**0.5
       dist.append(distn)

#Fill a graph with zeros and add in distance values only where there is an edge between the two nodes
graph = [[0 for col in range(nodes_total)] for row in range(nodes_total)]

   for p in range(len(nodes_to_ind)):
       graph[nodes_to_ind[p]][nodes_from_ind[p]] = dist[p]
   # print "graph: ", graph
   return graph

#Fill a graph with zeros and add in distance values only where there is an edge between the two nodes
graph = [[0 for col in range(nodes_total)] for row in range(nodes_total)]

   for p in range(len(nodes_to_ind)):
       graph[nodes_to_ind[p]][nodes_from_ind[p]] = dist[p]
   # print "graph: ", graph
   return graph

def heuristic_cost(neighbor_node,goal_node):
   #An optimistic estimate of the cost that would be incurred if the the robot travelled from the current node to it's neighbor node is the straight line distance between the neighbor node and the goal
   xcoordn = neighbor_node[0]
   ycoordn = neighbor_node[1]

   xcoord_goal = goal_node[0]
   ycoord_goal = goal_node[1]

   h_cost = ((xcoordn-xcoord_goal)**2 + (ycoordn-ycoord_goal)**2)**0.5

return h_cost

def neighbor_cost(neighbor_node, current_node):
#The cost of travelling to the neighbor node from the current is the distance between the two

xcoordcn = current_node[0]
ycoordcn = current_node[1]

   xcoord_neigh = neighbor_node[0]
   ycoord_neigh = neighbor_node[1]

   cost = (((xcoordcn-xcoord_neigh)**2 + (ycoordcn-ycoord_neigh)**2))**0.5

return cost

def neighbor_nodes(nodes_coord,paths_possible):
   #This finds the neighbors possible for all of the nodes
   nodes_from = []
   nodes_to = []
   neighbor_node = []
   neighbors = []

   for i in range(len(paths_possible)):
       #Since paths possible contains a pair of nodes, need to take one column of that and make it into the nodes we're travelling from and the other into the nodes we're travelling to
       nodes_from.append(paths_possible[i][0])
       nodes_to.append(paths_possible[i][1])

   for j in range(len(nodes_coord)):
       #Find all of the parents for the nodes
       parent = nodes_coord[j]
       neighbor_temp = []
       for k in range(len(nodes_from)):
           if parent == nodes_to[k]:
               neighbor_temp.append(nodes_from[k])
       neighbors.append(neighbor_temp)
   return neighbors

def neigh_from_graph(nodes_coord,graph):
   #This finds the neighbors possible for all of the nodes if given a graph to input- use with Astar1
   neighbor_node = []
   for j in range(len(node_coord)):
       neighbor_temp = []
       for k in range(len(node_coord)):
           if graph[j][k] != 0:
               neighbor_temp.append(node_coord[k])
       neighbors.append(neighbor_temp)
   return neighbors

def new_list_open(paths_possible, nodes_coord, current, list_open, list_closed):
#This function finds the possible neighbors of the current node

##First use the neighbors function (above) to find all of the possible neihbors
neighbors = neighbor_nodes(nodes_coord,paths_possible)

##Find the index the current node in the nodes_coord array
current_ind = nodes_coord.index(current)

   for i in range(len(neighbors[current_ind])):
       ##Check if the neighbors of the current node are in the closed nodes (already explored) list and, if not, add it to the list of open neighbors
       if neighbors[current_ind][i] not in list_closed and neighbors[current_ind][i] not in list_open:
           list_open.append(neighbors[current_ind][i])

return list_open

def new_list_open_graph(list_open,list_closed,neighbors):
#This function finds the possible neighbors of the current node

   num_neighbors = len(neighbors)
   for i in range(num_neighbors):
       if neighbors[i] not in list_open and neighbors[i] not in list_closed:
           list_open.append(neighbors[i])

return list_open

def sorting_open(past_cost,goal,list_open,nodes_coord):
   est_total_cost = []
   #Sorting the possible neighbors of the current node by the lowest cost
   for i in range(len(list_open)):
       node_index = nodes_coord.index(list_open[i])
       est_total_cost.append((past_cost[node_index] +heuristic_cost(list_open[i],goal),list_open[i]))

   #Sorting the tuple, which contains the open node and the heuristic cost of that node; sorting by the heuristic cost
   cost_sorted = sorted(est_total_cost,key = lambda cost:cost[0])

   temp_list =[]
   for i in range(len(list_open)):
       temp_list.append(cost_sorted[i][1])

return temp_list

def create_inputs(nodes_number, obstacles_number):
#the A* function takes as input: (robot_rad,obs_radii,obs_coord,nodes_coord,start,goal,region_length,region_width) so need to create those randomly.

   #Initialize arrays:
   obs_coord = []
   obs_radii = []
   nodes_coord = []

   #Region inputs:
   ##Length:
   region_length = round(100*random.random(),2)
   ##Width:
   region_width = round(100*random.random(),2)

#Robot radius:
robot_rad = round(2*random.random(),2)

   #Obstacles:
   ##Coordinates:
   for i in range(obstacles_number):
       obs_coord.append([round(100*random.random(),2),round(100*random.random(),2)])
   ##Radii:
   for i in range(obstacles_number):
       obs_radii.append(round(10*random.random(),2))

   ##Nodes Coordinates:
   for i in range(nodes_number):
       nodes_coord.append([round(100*random.random(),2),round(100*random.random(),2)])

return robot_rad, obs_radii, obs_coord, nodes_coord, region_length, region_width

def plot(obs_radii,obs_coord,nodes_coord,nodes_coord_unaltered,path,start,goal,paths_possible):

   num_obs = len(obs_radii)
   num_nodes = len(nodes_coord)
   num_old_nodes = len(nodes_coord_unaltered)

   ##Finding the coordinates of the start and goal
   xstart = start[0]
   ystart = start[1]

xgoal = goal[0]
ygoal = goal[1]

#Plot the start and goal

plt.plot(xstart,ystart,'rs')
plt.plot(xgoal,ygoal,'rs')

#Plot the obstacles:

   for i in range(num_obs):
       obs = plt.Circle((obs_coord[i][0],obs_coord[i][1]), radius = obs_radii[i], fc = 'g')
       o = plt.axes()
       o.add_patch(obs)

   #Plot the nodes:

   ##There are two sets of nodes that need to be plotted- the first are the nodes that are in collision of sorts with the obstacles or the wall of the planar region that the robot is exploring
   ###Available nodes:

x = []
y = []

   for i in range(num_nodes-2):
       x.append(nodes_coord[i+1][0])
       y.append(nodes_coord[i+1][1])

plt.plot(x,y,'rd')

   ###Old nodes:
   xold = []
   yold = []

   for i in range(num_old_nodes-2):
       xold.append(nodes_coord_unaltered[i+1][0])
       yold.append(nodes_coord_unaltered[i+1][1])

plt.plot(xold,yold,'b^')

##Plot Edges:

nodes_to = []
nodes_from = []

   for i in range(len(paths_possible)):
       nodes_to.append(paths_possible[i][0])
       nodes_from.append(paths_possible[i][1])

   for k in range(len(nodes_to)):
       node1x = nodes_to[k][0]
       node1y = nodes_to[k][1]
       node2x = nodes_from[k][0]
       node2y = nodes_from[k][1]
       plt.plot([node1x,node2x],[node1y,node2y],'c--')

   ##Plotting path:
   for i in range(len(path)-1):
       x_path1 = path[i][0]
       y_path1 = path[i][1]

       x_path2 = path[i+1][0]
       y_path2 = path[i+1][1]
       plt.plot([x_path1,x_path2],[y_path1,y_path2],'k')

   ##Normalizing the axes:
   plt.axis('equal')
   plt.axis([0,110,0,110])
   plt.show()

def a_star1(graph,nodes_coord,start,goal):
   #This is just purely the a star algorithm; takes as input a graph, the node coordinates, the start, and the goal
   print 'in the program!!!!! Lets look at the graph to see if the program worked- hopefully it will have :)'

   print "Robot Radius: ", robot_rad

nodes_coord_unaltered = nodes_coord[:]
num_nodes = len(nodes_coord)

path = []

##Find all of the possible neighbors between the coordinates using the paths possible list
all_nbrs = neigh_from_graph(nodes_coord,graph)

   if start not in nodes_coord:
       print "No solution. The start conflicts with an obstacle O_o"
       plot(obs_radii,obs_coord,nodes_coord,nodes_coord_unaltered,path,start,goal,paths_possible)
       return
   elif goal not in nodes_coord:
       print "No solution. The goal conflicts with an obstacle o_O"
       plot(obs_radii,obs_coord,nodes_coord,nodes_coord_unaltered,path,start,goal,paths_possible)
       return

   #Now for the meat of A*:
   ##Initialize lists
   ###Place start as the first element in list open since that is the one that we are currently at- will update this with the neighbor list later

list_open = [start]
list_closed = []

   #The past cost of everything but the "start" node is infinity- the cost associated with start is 0 since we are already there
   past_cost = float('inf')*np.ones(num_nodes)
   past_cost[0] = 0

#Need to create a parent array that is the length of the number of nodes and has the start as its first parent since we are beginning there

   parent = [0]*num_nodes
   parent[0] = start


   while len(list_open) != 0:
       current = list_open.pop(0)
       list_closed.append(current)

       current_ind = nodes_coord.index(current)

current_nbrs = all_nbrs[current_ind]

list_open = new_list_open_graph(paths_possible, nodes_coord, current, list_open, list_closed)

       #We want to check if the current node is equal to the goal node, to see if we're done or not
       if current == goal:
           print "Path successfully found!"
           #Need to set the path to goal initially since we will keep inserting a parent element into the first index space of the path array
           path = [goal]
           node_index = nodes_coord.index(goal)
           while path[0] != start:
               #Need to take all of the nodes that we travelled to and liked from the parents index to the path
               path.insert(0,parent[node_index])
               node_index = nodes_coord.index(path[0])
           print path
           #Once path is obtained, need to plot the successful path :)
           plot(obs_radii,obs_coord,nodes_coord,nodes_coord_unaltered,path,start,goal,paths_possible)
           return

for neighbor in current_nbrs:
#Find the index of the current neighbor so we can find the cost from going from the current node to the neighbor node

neighbor_ind = nodes_coord.index(neighbor)

tentative_cost = past_cost[current_ind] + graph[current_ind][neighbor_ind]

           if tentative_cost < past_cost[neighbor_ind]:
               #If the tentative cost is below the past cost, then we should travel to that node; otherwise, it is not a good option
               past_cost[neighbor_ind] = tentative_cost
               parent[neighbor_ind] = current

#We need to get a new open list for the neighbor node once we travel to it
list_open = sorting_open(past_cost,goal,list_open,nodes_coord)

   if len(path) == 0:
       print "No solution. There are no possible paths to the goal."
       plot(obs_radii,obs_coord,nodes_coord,nodes_coord_unaltered,path,start,goal,paths_possible)

def a_star2(robot_rad,obs_radii,obs_coord,nodes_coord,start,goal,region_length,region_width):
#This is the a star algorithm, with some additions; it takes the robot radius, obstacle coordinates/radii, as well as the node coordinates, start, goal, and region length and width as an input and checks for collisions of nodes with obstacles, finds the possible paths, as well as creates the graph. THEN it goes into the a star algorithm

   print 'in the program!!!!! Lets look at the graph to see if the program worked- hopefully it will have :)'

   print "Robot Radius: ", robot_rad

   #Creating the graph:
   ##Adding the start and goal as nodes
   nodes_coord.append(goal)
   nodes_coord.insert(0,start)
   nodes_coord_unaltered = nodes_coord[:]
   num_nodes = len(nodes_coord)

##Now, check to see whether those nodes are within obstacles or too close to an obstacle for the robot to go to with it's radius; the result of collision check should return nodes that the robot should actually be allowed to go to

nodes_coord = collision_check(robot_rad,nodes_coord,obs_radii,obs_coord,region_length,region_width)

#If the start or goal node are not in the resulting nodes list, then one or the other is in conflict with an obstacle

path = []

##Find the possible paths between all of the nodes and check which ones are still possible after you check for collisions with obstacles
paths_possible = edges(robot_rad,nodes_coord,obs_radii,obs_coord)

##Find all of the possible neighbors between the coordinates using the paths possible list
all_nbrs = neighbor_nodes(nodes_coord,paths_possible)

##Create a graph using the paths possible and node coordinates
graph = create_graph(paths_possible,nodes_coord)

# print "paths_possible: ", paths_possible