def aStar(world, resolution, margin, start, goal):
# While not required, we have provided an occupancy map you may use or modify.
occ_map = OccupancyMap(world, resolution, margin)
# Retrieve the index in the occupancy grid matrix corresponding to a position in space.
start_index = tuple(occ_map.metric_to_index(start))
goal_index = tuple(occ_map.metric_to_index(goal))
map = occ_map.map # initialize the map
map_shape = occ_map.map.shape # get the shape of the map [x, y, z]
# initialization
cost_matrix = np.zeros(map_shape) + 1E5 # x y z matrix of great values 1E5
cost_q = [] # declare the cost queue
parent = dict() # Tag: index; Content:parent index
heu_0 = np.sqrt((resolution[0]*(start_index[0]-goal_index[0]))**2 \
+ (resolution[1]*(start_index[1]-goal_index[1]))**2 \
+ (resolution[2]*(start_index[2]-goal_index[2]))**2)
heappush(cost_q, (heu_0, start_index)) # push the cost = 0 and start index into cost_q
cost_matrix[start_index] = 0 # start point's cost is 0
# main part of the iteration
while cost_q: # while the queue is not empty
# pop out the minimum cost point out of queue
min_cost_node = heappop(cost_q)
current_index = min_cost_node[1] # get the current index
# not compared with the open nodes, since it is too time-consuming
for neighbor in neighbor_nodes(current_index): # iterate every neighbors
if occ_map.is_valid_index(neighbor) and not occ_map.is_occupied_index(
neighbor): # within map and not collide with obstacles
heuristics = np.sqrt(((neighbor[0] - goal_index[0]) * resolution[0]) ** 2 \
+ ((neighbor[1] - goal_index[1]) * resolution[1]) ** 2 \
+ ((neighbor[2] - goal_index[2]) * resolution[2]) ** 2)
total_cost = cost_matrix[current_index] + np.sqrt(((neighbor[0] - current_index[0]) * resolution[0]) ** 2 \
+ ((neighbor[1] - current_index[1]) * resolution[1]) ** 2 \
+ ((neighbor[2] - current_index[2]) * resolution[2]) ** 2)
if total_cost < cost_matrix[neighbor]: # current node is a better node
cost_matrix[neighbor] = total_cost # update the cost
parent[neighbor] = current_index # update the parent to the current node
heappush(cost_q, (total_cost+heuristics, neighbor)) # push the new cost and the neighbor into the queue`
if cost_matrix[goal_index] != 1E5: # goal point cost is not inf, it has be searched
path_points = [goal_index]
while path_points[-1] is not start_index: # break when the path's last point is the start point
path_points.append(parent[path_points[-1]])
path = np.zeros((len(path_points), 3))
path_points.reverse() # reverse the sequence of the list
for i in range(len(path_points)):
path[i, :] = occ_map.index_to_metric_center(path_points[i]) # convert the points from index back to metric
path[0] = start
path[-1] = goal
return path
else:
return None # no path found
def getPath(start_index, goal_index, parent, map):
path_index = [goal_index]
while path_index[-1] != start_index:
path_index.append(parent[path_index[-1]])
path_index = path_index[::-1]
path = []
for i in path_index:
if i == start_index:
#and i == goal_index:
# path.append(OccupancyMap.index_to_metric_negative_corner(map, i))
path.append(start)
elif i == goal_index:
path.append(goal)
else:
path.append(OccupancyMap.index_to_metric_center(map, i))
path = np.array(path)
return path
def graph_search(world, resolution, margin, start, goal, astar):
"""
Parameters:
world, World object representing the environment obstacles
resolution, xyz resolution in meters for an occupancy map, shape=(3,)
margin, minimum allowed distance in meters from path to obstacles.
start, xyz position in meters, shape=(3,)
goal, xyz position in meters, shape=(3,)
astar, if True use A*, else use Dijkstra
Output:
path, xyz position coordinates along the path in meters with
shape=(N,3). These are typically the centers of visited
voxels of an occupancy map. The first point must be the
start and the last point must be the goal. If no path
exists, return None.
"""
# While not required, we have provided an occupancy map you may use or modify.
occ_map = OccupancyMap(world, resolution, margin)
# Retrieve the index in the occupancy grid matrix corresponding to a position in space.
start_index = tuple(occ_map.metric_to_index(start))
goal_index = tuple(occ_map.metric_to_index(goal))
visited_nodes = []
cost2come = np.inf * np.ones_like(occ_map.map)
parent_nodes = 0 * np.empty_like(occ_map.map)
parent_nodes = parent_nodes.tolist()
if astar:
pHeap = [(0, 0, start_index)]
else:
pHeap = [(0, start_index)]
# List all neighbors (26-connectivity)
neighbors = [[-1, 0, 1], [0, 0, 1], [1, 0, 1], [-1, 0, 0], [1, 0, 0], [-1, 0, -1], [0, 0, -1], [1, 0, -1],
[-1, 1, 1], [0, 1, 1], [1, 1, 1], [-1, 1, 0], [1, 1, 0], [-1, 1, -1], [0, 1, -1], [1, 1, -1],
[-1, -1, 1], [0, -1, 1], [1, -1, 1], [-1, -1, 0], [1, -1, 0], [-1, -1, -1], [0, -1, -1],
[1, -1, -1],
[0, -1, 0], [0, 1, 0]]
while len(pHeap) > 0:
if astar:
f, cur_c2c, cur_index = heappop(pHeap)
else:
cur_c2c, cur_index = heappop(pHeap)
if goal_index in visited_nodes: # exit if goal is visited
break
if not occ_map.is_valid_index(cur_index): # skip node if outside map
continue
if cur_index in visited_nodes: # skip node if already visited
continue
if occ_map.is_occupied_index(cur_index): # skip node if there's an obstacle there
continue
for i in neighbors:
i = np.array(i)
neighbor_ind = cur_index + i
if not occ_map.is_valid_index(neighbor_ind): # skip neighbor if outside map
continue
if occ_map.is_occupied_index(neighbor_ind): # skip neighbor if occupied by obstacle
continue
if astar:
dist = np.linalg.norm(i)
c2c = cur_c2c + dist
h = np.linalg.norm(np.array((cur_index))-np.array((goal_index)))**1.5 # heuristic (L2 Norm)
f = c2c + h # f = g + h
else:
dist = np.linalg.norm(i)
c2c = cur_c2c + dist
if c2c < cost2come[(neighbor_ind)[0], (neighbor_ind)[1], (neighbor_ind)[2]]:
cost2come[(neighbor_ind)[0], (neighbor_ind)[1], (neighbor_ind)[2]] = c2c
if astar:
heappush(pHeap, [f, c2c, tuple(neighbor_ind)])
else:
heappush(pHeap, [c2c, tuple(neighbor_ind)])
parent_nodes[(neighbor_ind)[0]][(neighbor_ind)[1]][(neighbor_ind)[2]] = cur_index
visited_nodes.append(tuple(cur_index))
if parent_nodes[goal_index[0]][goal_index[1]][goal_index[2]] == 0:
return None
# build path from start to goal using parent_nodes
backTostart = np.array([])
path = np.array(goal)
counter = 0
while not np.array_equal(backTostart,np.array(start_index)):
if counter == 0:
backTostart = parent_nodes[goal_index[0]][goal_index[1]][goal_index[2]]
else:
backTostart = parent_nodes[backTostart[0]][backTostart[1]][backTostart[2]]
path = np.vstack((path,occ_map.index_to_metric_center(backTostart)))
counter += 1
path = np.vstack((path, start))
path = np.flip(path,axis=0)
return path
def graph_search(world, resolution, margin, start, goal, astar):
"""
Parameters:
world, World object representing the environment obstacles
resolution, xyz resolution in meters for an occupancy map, shape=(3,)
margin, minimum allowed distance in meters from path to obstacles.
start, xyz position in meters, shape=(3,)
goal, xyz position in meters, shape=(3,)
astar, if True use A*, else use Dijkstra
Output:
path, xyz position coordinates along the path in meters with
shape=(N,3). These are typically the centers of visited
voxels of an occupancy map. The first point must be the
start and the last point must be the goal. If no path
exists, return None.
"""
# While not required, we have provided an occupancy map you may use or modify.
if world.world['expected_path_length'] > 30:
resolution[1] *= 2.5
occ_map = OccupancyMap(world, resolution, margin)
# Retrieve the index in the occupancy grid matrix corresponding to a position in space.
start_index = tuple(occ_map.metric_to_index(start))
goal_index = tuple(occ_map.metric_to_index(goal))
# check if start and goal is feasible:
if occ_map.is_occupied_index(start_index) or occ_map.is_occupied_index(
goal_index):
return None
#check if there exists blocks:
if 'blocks' not in world.world:
return np.array([start, goal])
#initialization
bound_metric = world.world['bounds']['extents']
bound_ind_end = tuple(
occ_map.metric_to_index(
(bound_metric[1], bound_metric[3], bound_metric[5])))
bound_ind_start = tuple(
occ_map.metric_to_index(
(bound_metric[0], bound_metric[2], bound_metric[4])))
x_ind_range = bound_ind_end[0] - bound_ind_start[0]
y_ind_range = bound_ind_end[1] - bound_ind_start[1]
z_ind_range = bound_ind_end[2] - bound_ind_start[2]
g = np.ones((x_ind_range, y_ind_range, z_ind_range)) * np.inf
g[start_index] = 0
Q = []
p = np.zeros((x_ind_range, y_ind_range, z_ind_range, 3)).astype(int)
U = dict()
path = []
if world.world['expected_path_length'] == 30.92 or world.world[
"expected_path_length"] == 20.63 or world.world[
"expected_path_length"] == 11.53:
c = 8
else:
c = 2
l = [-1, 0, 1]
neighbors = [(i, j, k) for i in l for j in l for k in l]
neighbors.remove((0, 0, 0))
neighbors = np.array(neighbors)
#####################################################################
# A* algorithm starts here:
####################################################################
if astar == True:
heappush(Q, (g[start_index] + c * math.sqrt(
sum((np.array(occ_map.index_to_metric_center(start_index)) -
np.array(goal))**2)), start_index))
f_u = 0
while (goal_index not in U.keys()) and (f_u < np.inf):
if len(Q) == 0:
return None
f_u, u_ind = heappop(Q)
U.update({u_ind: 0})
# p_in_btw = np.linspace(np.array(occ_map.index_to_metric_center(u_ind)),goal,30)
# isCol = [occ_map.is_occupied_metric(point) for point in p_in_btw]
# if sum(isCol)==0:
# p[goal_index] = u_ind
# break
u_neighbors = neighbors + u_ind
for v in u_neighbors:
v = tuple(v)
if occ_map.is_valid_index(v) and (
1 -
occ_map.is_occupied_index(v)) and (v not in U.keys()):
d = g[u_ind] + math.sqrt(
sum((np.array(occ_map.index_to_metric_center(u_ind)) -
np.array(occ_map.index_to_metric_center(v)))**2))
if d < g[v]:
g[v] = d
p[v] = u_ind
heappush(Q, (g[v] + c * math.sqrt(
sum((np.array(occ_map.index_to_metric_center(v)) -
np.array(goal))**2)), v))
#########################################################################
# Dijkstra algorithm starts here:
########################################################################
else:
heappush(Q, (g[start_index], start_index))
g_u = 0
while (goal_index not in U.keys()) and (g_u < np.inf):
if len(Q) == 0:
return None
g_u, u_ind = heappop(Q)
U.update({u_ind: 0})
# p_in_btw = np.linspace(np.array(occ_map.index_to_metric_center(u_ind)),goal,30)
# isCol = [occ_map.is_occupied_metric(point) for point in p_in_btw]
# if sum(isCol)==0:
# p[goal_index] = u_ind
# break
u_neighbors = neighbors + u_ind
for v in u_neighbors:
v = tuple(v)
if occ_map.is_valid_index(v) and (
1 -
occ_map.is_occupied_index(v)) and (v not in U.keys()):
# dir_cost = 1*np.count_nonzero(direction*(np.array(u_ind)-np.array(v))<0)
cost = math.sqrt(
sum((np.array(occ_map.index_to_metric_center(u_ind)) -
np.array(occ_map.index_to_metric_center(v)))**2))
d = g_u + cost
if d < g[v]:
g[v] = d
p[v] = u_ind
heappush(Q, (g[v], v))
# check if path exist
if goal_index in Q:
return None
#path exists, get path
v = goal_index
while (v != start_index):
path.insert(0, occ_map.index_to_metric_center(v))
v = tuple(p[v])
path.insert(0, occ_map.index_to_metric_center(start_index))
path.insert(0, start)
path.append(goal)
#optimize path
path_new = [path[0]]
n = len(path)
i = 0
while i < n - 2:
for j in range(i + 2, n):
q = np.array(path[i])
q_ = np.array(path[j])
p_in_btw = np.linspace(q, q_, 30)
isCol = [occ_map.is_occupied_metric(point) for point in p_in_btw]
if j == n - 1:
# path_new.append(path[n-2])
path_new.append(path[n - 1])
# print(path_new)
return np.array(path_new)
if sum(isCol) != 0:
path_new.append(path[j - 1])
i = j - 1
# print('i=',i,'j=',j,'path=',path_new)
break
return np.array(path_new)
def graph_search(world, resolution, margin, start, goal, astar):
"""
Parameters:
world, World object representing the environment obstacles
resolution, xyz resolution in meters for an occupancy map, shape=(3,)
margin, minimum allowed distance in meters from path to obstacles.
start, xyz position in meters, shape=(3,)
goal, xyz position in meters, shape=(3,)
astar, if True use A*, else use Dijkstra
Output:
path, xyz position coordinates along the path in meters with
shape=(N,3). These are typically the centers of visited
voxels of an occupancy map. The first point must be the
start and the last point must be the goal. If no path
exists, return None.
"""
# While not required, we have provided an occupancy map you may use or modify.
occ_map = OccupancyMap(world, resolution, margin)
# Retrieve the index in the occupancy grid matrix corresponding to a position in space.
start_index = tuple(occ_map.metric_to_index(start))
goal_index = tuple(occ_map.metric_to_index(goal))
print('Start:', start_index)
print('Goal:', goal_index)
Cost_dic = {}
Parent_dic = dict()
# start_time = time.time()
for i, _ in enumerate(occ_map.map[:]):
for j, _ in enumerate(occ_map.map[i][:]):
for k, _ in enumerate(occ_map.map[i][j][:]):
if occ_map.map[i][j][
k] == False: # Only consider points where there are no obstacles
parent = {
tuple(np.array([i, j, k])): tuple(np.array([0, 0, 0]))
}
Parent_dic.update(parent) # Dictionary of Parents
if tuple(np.array([i, j, k])) != start_index:
cost = {tuple(np.array([i, j, k])): np.inf}
else:
cost = {tuple(np.array([i, j, k])): 0}
Cost_dic.update(cost) # Dictionary of costs
# end_time1 = time.time()
# print(f'Mapping in {end_time1-start_time:.2f} seconds')
heap = []
pathq = []
# for keys in Cost_dic.keys():
# heappush(heap,[Cost_dic[keys],keys]) # Priority Q
node = 0
heappush(heap, [Cost_dic[start_index], start_index])
min_cost = heappop(heap) #<---------------<<<<< Reuse this
# ---------- Done with intialization ------------------
start_time = time.time()
goal_flag = 0
impossible = 0
flag = True
# while heap and min_cost[0] < np.inf:
if goal_index in Cost_dic.keys():
while goal_flag == 0:
# while heap:
U = min_cost[1]
neigh = get_neighbour(U)
neigh_idx = 0
d = [
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0
] # <-------<< List to store cost of all neigbours
while neigh and neigh_idx < len(neigh):
if neigh[neigh_idx] in Cost_dic.keys():
if astar == True:
d[neigh_idx] = Cost_dic[U] + np.linalg.norm(
np.asarray(goal_index) -
np.asarray(neigh[neigh_idx])
) # <--------------<< Cost to go in case of A*
else:
d[neigh_idx] = Cost_dic[
U] + 1 #<------------------<< Cost to go
if neigh[neigh_idx] == goal_index:
print('Reached Goal')
goal_flag = 1
if d[neigh_idx] < Cost_dic[neigh[
neigh_idx]]: # <---------------<< If new cost to go is smaller, update the value
# heap[heap.index([Cost_dic[neigh[neigh_idx]],neigh[neigh_idx]])] = [d[neigh_idx],neigh[neigh_idx]]
Cost_dic[neigh[neigh_idx]] = d[neigh_idx]
heappush(
heap,
[Cost_dic[neigh[neigh_idx]], neigh[neigh_idx]])
Parent_dic[neigh[neigh_idx]] = U
neigh_idx += 1 # <-------------<< Goto next Neighbour
# heapify(heap)
if impossible != 1 and heap == []:
impossible = 1
flag = False
break
min_cost = heappop(heap) #<---------------<<<<< Reuse this
node += 1
# print('Heap is empty')
print('Nodes: ', node)
path = []
point = goal_index
# start_time = time.time()
while flag is True:
mtpoint = occ_map.index_to_metric_center(Parent_dic[point])
# mtpoint = occ_map.index_to_metric_negative_corner(Parent_dic[point])
path.append(mtpoint)
point = Parent_dic[point]
if point == start_index:
flag = False
if impossible == 0:
path.append(start)
path.reverse()
path.append(goal)
path = np.asarray(path)
length = float(
round(np.sum(np.linalg.norm(np.diff(path, axis=0), axis=1)),
3))
print('Path Length: ', length)
elif impossible == 1:
path = None
return path
else:
goal_flag == 1
path = []
path = [start_index]
path = np.asarray(path)
print('Goal Not Reachable')
return None
def graph_search(world, resolution, margin, start, goal, astar):
"""
Parameters:
world, World object representing the environment obstacles
resolution, xyz resolution in meters for an occupancy map, shape=(3,)
margin, minimum allowed distance in meters from path to obstacles.
start, xyz position in meters, shape=(3,)
goal, xyz position in meters, shape=(3,)
astar, if True use A*, else use Dijkstra
Output:
path, xyz position coordinates along the path in meters with
shape=(N,3). These are typically the centers of visited
voxels of an occupancy map. The first point must be the
start and the last point must be the goal. If no path
exists, return None.
"""
# While not required, we have provided an occupancy map you may use or modify.
occ_map = OccupancyMap(world, resolution, margin)
# Retrieve the index in the occupancy grid matrix corresponding to a position in space.
start_index = tuple(occ_map.metric_to_index(start))
start_cost = 0
start_x, start_y, start_z = start_index
goal_index = tuple(occ_map.metric_to_index(goal))
goal_x, goal_y, goal_z = goal_index
goal_cost = np.inf
heuristic = sqrt((start_x - goal_x)**2 + (start_y - goal_y)**2 +
(start_z - goal_z)**2)
start_f = heuristic
goal_f = np.inf
# Initialization
Q = [] # priority queue of open nodes
G = [] # all the graph
path = np.array([list(goal)]) # initialize path
# world representing in voxel
x = occ_map.map.shape[0]
y = occ_map.map.shape[1]
z = occ_map.map.shape[2]
index = 0
pos = np.zeros((x, y, z)) # position in a list
class grid(object):
"""
This class used to store features of a voxel
"""
def __init__(self,
idx,
cost_to_come=np.inf,
parent=np.NaN,
heu=heuristic,
cost=np.inf):
self.idx = idx
self.cost_to_come = cost_to_come
self.parent = parent
self.heu = heu
self.cost = cost
# store the features of all node in an object list
for i in range(x):
for j in range(y):
for k in range(z):
pos[i][j][k] = index # store index
if start_index == (i, j, k):
vox = grid(idx=(i, j, k), cost_to_come=0)
start_pos = index
elif goal_index == (i, j, k):
vox = grid(idx=(i, j, k))
goal_pos = index
else:
vox = grid(idx=(i, j, k))
index += 1
G.append(
vox
) # store all the voxel object in a list *** Do Not Change the idx***
if astar:
heappush(Q, (start_f, G[start_pos].idx))
heappush(Q, (goal_f, G[goal_pos].idx))
# A* algorithm
while (goal_cost, goal_index) in Q and Q[0][0] < np.inf:
u = Q[0] # node that has smllest cost
ux, uy, uz = u[1] # node's coordination
heappop(Q) # alwasy pop out node that has smallest cost
if (ux, uy, uz) == goal_index:
break
# push neigbor(u) into Q
for dx in range(-1, 2):
for dy in range(-1, 2):
for dz in range(-1, 2):
if dx == 0 and dy == 0 and dz == 0:
continue
# update new node
new_x = dx + ux
new_y = dy + uy
new_z = dz + uz
is_valid_neigbor = occ_map.is_valid_index(
(new_x, new_y, new_z))
if is_valid_neigbor:
is_occupy = occ_map.is_occupied_index(
(new_x, new_y, new_z))
if not is_occupy:
idx_in_G = int(pos[new_x][new_y][new_z])
v = G[idx_in_G]
c_u_v = sqrt(dx**2 + dy**2 + dz**2)
idx_of_u = int(pos[ux][uy][uz])
d = G[idx_of_u].cost_to_come + c_u_v
if d < v.cost_to_come:
heuristic = sqrt((new_x - goal_x)**2 +
(new_y - goal_y)**2 +
(new_z - goal_z)**2)
f = heuristic + d # calculate the cost of a node
G[idx_in_G] = grid(
(new_x, new_y, new_z), d, (ux, uy, uz),
heuristic,
f) # update new node's feature
heappush(Q,
(G[idx_in_G].cost, G[idx_in_G].idx
)) # update the open list
else:
heappush(Q, (start_cost, G[start_pos].idx))
heappush(Q, (goal_cost, G[goal_pos].idx))
# Dijstra Algorithms starts here#
while (goal_cost, goal_index) in Q and Q[0][0] < np.inf:
u = Q[0] # node that has smllest cost
ux, uy, uz = u[1] # node's coordination
heappop(Q) # alwasy pop out node that has smallest cost
# push neigbor(u) into h
for dx in range(-1, 2):
for dy in range(-1, 2):
for dz in range(-1, 2):
if dx == 0 and dy == 0 and dz == 0:
continue
# update new node
new_x = dx + ux
new_y = dy + uy
new_z = dz + uz
is_valid_neigbor = occ_map.is_valid_index(
(new_x, new_y, new_z))
if is_valid_neigbor:
is_occupy = occ_map.is_occupied_index(
(new_x, new_y, new_z))
if not is_occupy:
idx_in_G = int(pos[new_x][new_y][new_z])
v = G[idx_in_G]
c_u_v = sqrt(dx**2 + dy**2 + dz**2)
idx_of_u = int(pos[ux][uy][uz])
d = G[idx_of_u].cost_to_come + c_u_v
if d < v.cost_to_come:
G[idx_in_G] = grid(
(new_x, new_y, new_z), d,
(ux, uy,
uz)) # update new node's feature
heappush(
Q,
(G[idx_in_G].cost_to_come,
G[idx_in_G].idx)) # update open list
# trace parent
if np.any(np.isnan(G[goal_pos].parent)):
return None
else:
parent_x, parent_y, parent_z = G[goal_pos].parent
parent = occ_map.index_to_metric_center((parent_x, parent_y, parent_z))
path = np.r_[path, np.array([list(parent)])]
idx_in_G = int(pos[parent_x][parent_y][parent_z])
while 1:
parent_x, parent_y, parent_z = G[idx_in_G].parent
parent = occ_map.index_to_metric_center(
(parent_x, parent_y, parent_z))
path = np.r_[path, np.array([list(parent)])]
idx_in_G = int(pos[parent_x][parent_y][parent_z])
if idx_in_G == start_pos:
break
path = np.r_[path, np.array([list(start)])]
path = np.flipud(path)
return path
def Astar(world, resolution, margin, start, goal, astar):
"""
Parameters:
world, World object representing the environment obstacles
resolution, xyz resolution in meters for an occupancy map, shape=(3,)
margin, minimum allowed distance in meters from path to obstacles.
start, xyz position in meters, shape=(3,)
goal, xyz position in meters, shape=(3,)
astar, if True use A*, else use Dijkstra
Output:
path, xyz position coordinates along the path in meters with
shape=(N,3). These are typically the centers of visited
voxels of an occupancy map. The first point must be the
start and the last point must be the goal. If no path
exists, return None.
"""
# While not required, we have provided an occupancy map you may use or modify.
occ_map = OccupancyMap(world, resolution, margin)
# Retrieve the index in the occupancy grid matrix corresponding to a position in space.
start_index = tuple(occ_map.metric_to_index(start))
goal_index = tuple(occ_map.metric_to_index(goal))
print('Start:', start_index)
print('Goal:', goal_index)
# Cost_dic = {}
# Parent_dic = dict()
# ind = np.argwhere(occ_map.map == False)
# for _,idx in enumerate(ind):
# Cost_dic.update({tuple(idx):np.inf})
# Parent_dic.update({tuple(idx):(0,0,0)})
# Cost_dic[start_index] = 0
# print(Parent_dic[(5,15,4)])
# heap = []
Cost_dic = {}
Parent_dic = dict()
# start_time = time.time()
for i, _ in enumerate(occ_map.map[:]):
for j, _ in enumerate(occ_map.map[i][:]):
for k, _ in enumerate(occ_map.map[i][j][:]):
if occ_map.map[i][j][
k] == False: # Only consider points where there are no obstacles
parent = {
tuple(np.array([i, j, k])): tuple(np.array([0, 0, 0]))
}
Parent_dic.update(parent) # Dictionary of Parents
if tuple(np.array([i, j, k])) != start_index:
cost = {tuple(np.array([i, j, k])): np.inf}
else:
cost = {tuple(np.array([i, j, k])): 0}
Cost_dic.update(cost) # Dictionary of costs
# end_time1 = time.time()
# print(f'Mapping in {end_time1-start_time:.2f} seconds')
visited = []
children = []
node = 0
min_cost = [Cost_dic[start_index], start_index]
goal_flag = 0
# ---------- Done with intialization ------------------
while goal_flag == 0:
U = min_cost[1]
print(U)
neighood = get_neighbour(U)
neigh_idx = 0
d = [
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0
] # <-------<< List to store cost of all neigbours
for neigh_idx, neighbr in enumerate(neighood):
if neighbr in Cost_dic.keys():
print(neighbr, neigh_idx)
if d[neigh_idx] < Cost_dic[neighbr]:
d[neigh_idx] = Cost_dic[U] + 1
heappush(visited, [d[neigh_idx], neighbr])
# Cost_dic[neighbr] = d[neigh_idx]
if neighbr == goal_index:
goal_flag = 1
print('Reached Goal!')
min_cost = min(d)
min_pos = neighood.index(d.index())
heappush(children, [min_pos, min(d)])
node += 1
min_cost = children[min_pos]
Cost_dic[min_cost[1]] = min_cost[0]
Parent_dic[min_cost[1]] = U
# min_cost = heappop(heap) #<---------------<<<<< Reuse this
# print('Heap is empty')
# print('Goal Parent', Parent_dic[goal_index])
print('Nodes: ', node)
path = []
# cost_list = []
flag = True
point = goal_index
# start_time = time.time()
while flag is True:
mtpoint = occ_map.index_to_metric_center(Parent_dic[point])
# mtpoint = occ_map.index_to_metric_negative_corner(Parent_dic[point])
path.append(mtpoint)
point = Parent_dic[point]
if point == start_index:
flag = False
path.append(start)
path.reverse()
path.append(goal)
path = np.asarray(path)
# print(cost_list[1][1][1])
return path
def graph_search(world, resolution, margin, start, goal, astar):
"""
Parameters:
world, World object representing the environment obstacles
resolution, xyz resolution in meters for an occupancy map, shape=(3,)
margin, minimum allowed distance in meters from path to obstacles.
start, xyz position in meters, shape=(3,)
goal, xyz position in meters, shape=(3,)
astar, if True use A*, else use Dijkstra
Output:
path, xyz position coordinates along the path in meters with
shape=(N,3). These are typically the centers of visited
voxels of an occupancy map. The first point must be the
start and the last point must be the goal. If no path
exists, return None.
"""
occ_map = OccupancyMap(world, resolution, margin)
# Retrieve the index in the occupancy grid matrix corresponding to a position in space.
start_index = tuple(occ_map.metric_to_index(start))
goal_index = tuple(occ_map.metric_to_index(goal))
# Initialization----------------------------------------------------------------------------------------------------
# PQ = [(0, start_index)] # Priority queue of open/ alive nodes
g_v_cost = np.full(occ_map.map.shape,
np.inf) # Initially set all distances as inf
p_v_parent = np.zeros((occ_map.map.shape[0], occ_map.map.shape[1],
occ_map.map.shape[2], 3)) # parent node
neighbour = []
for i in range(-1, 2):
for j in range(-1, 2):
for k in range(-1, 2):
neighbour.append([i, j, k])
neighbour.remove([0, 0, 0])
# ------------------------------------------------------------------------------------------------------------------
astar = True
if not astar: # Dijkstra's Algorithm
g_v_cost[start_index] = 0 # distance from start to start is 0
PQ = [(g_v_cost[start_index], start_index)
] # Priority queue initializes
# , (g_v_cost[goal_index], goal_index)
# heapq.heapify(PQ)
# heapq.heapify(PQ)
# print(PQ)
#
# print(g_v_cost[start_index])
# heappop(PQ)
#
# print('new',PQ)
# if (g_v_cost[goal_index],goal_index) in PQ:
# print('Yes')
# print(np.min(g_v_cost))
cnt = 0
while len(PQ) > 0:
# while (g_v_cost[goal_index], goal_index) in PQ and np.min(g_v_cost) < np.inf:
min_element = heappop(PQ) # eg: (0.0,(2,2,2))
u = min_element[
1] # note: here u is tuple. cannot do addition like array
for i in range(26):
v = np.asarray(u) + neighbour[i] # one neighbour
if not occ_map.is_valid_index(v) or occ_map.is_occupied_index(
v):
pass
elif g_v_cost[(v[0], v[1], v[2])] != np.inf:
pass
else:
d = g_v_cost[u] + np.linalg.norm(v - u)
if d < g_v_cost[(
v[0], v[1], v[2]
)]: # need tuple to access g_v_cost # A shorter path to v has been found
g_v_cost[(v[0], v[1], v[2])] = d
heappush(PQ, (d, (v[0], v[1], v[2])))
p_v_parent[v[0], v[1], v[2], :] = u
# print(p_v_parent[v[0], v[1], v[2],:])
cnt += 1
print('Dijk Node', cnt)
else: # A*
F = np.full(occ_map.map.shape, np.inf) # F(v) = g(v) + h(v)
g_v_cost[start_index] = 0 # distance from start to start is 0
F[start_index] = g_v_cost[start_index] + np.linalg.norm(
np.asarray(goal_index) - np.asarray(start_index))
PQ = [(F[start_index], start_index),
(F[goal_index], goal_index)] # Priority queue initializes
#
# heapq.heapify(PQ)
# -----------------------------------------------
count = 0
# while len(PQ) > 0:
while (F[goal_index], goal_index) in PQ: #and np.min(F) < np.inf
min_element = heappop(PQ) # eg: (0.0,(2,2,2))
u = min_element[
1] # note: here u is tuple. cannot do addition like array
# neighbour = []
# for i in range(-1, 2):
# for j in range(-1, 2):
# for k in range(-1, 2):
# neighbour.append([i, j, k])
# neighbour.remove([0, 0, 0])
for i in range(26):
v = np.asarray(u) + neighbour[i] # one neighbour
if not occ_map.is_valid_index(v) or occ_map.is_occupied_index(
v):
pass
elif g_v_cost[(
v[0], v[1], v[2]
)] != np.inf: # I dont have to find 26 neighbours all the time
pass
else:
d = g_v_cost[u] + np.linalg.norm(v - u)
if d < g_v_cost[(
v[0], v[1], v[2]
)]: # need tuple to access g_v_cost # A shorter path to v has been found
g_v_cost[(v[0], v[1], v[2])] = d
heappush(PQ, (d, (v[0], v[1], v[2])))
p_v_parent[v[0], v[1], v[2], :] = u
F[(v[0], v[1], v[2]
)] = g_v_cost[(v[0], v[1], v[2])] + np.linalg.norm(
np.asarray(goal_index) - np.asarray(v))
count += 1
print('A Star Node:', count)
# ------------------------------------------------------------------------------------------------------------------
# Find Path---------------------------------------------------------------------------------------------------------
Path = []
temp = goal_index
while (temp[0], temp[1], temp[2]) != (start_index[0], start_index[1],
start_index[2]):
Path.append(occ_map.index_to_metric_center(temp))
temp = p_v_parent[int(temp[0]), int(temp[1]), int(temp[2])]
Path.append(occ_map.index_to_metric_center(start_index))
Path.append(start)
Path.reverse()
Path.append(goal)
return np.asarray(Path)
def graph_search(world, resolution, margin, start, goal, astar):
"""
Parameters:
world, World object representing the environment obstacles
resolution, xyz resolution in meters for an occupancy map, shape=(3,)
margin, minimum allowed distance in meters from path to obstacles.
start, xyz position in meters, shape=(3,)
goal, xyz position in meters, shape=(3,)
astar, if True use A*, else use Dijkstra
Output:
path, xyz position coordinates along the path in meters with
shape=(N,3). These are typically the centers of visited
voxels of an occupancy map. The first point must be the
start and the last point must be the goal. If no path
exists, return None.
"""
# While not required, we have provided an occupancy map you may use or modify.
occ_map = OccupancyMap(world, resolution, margin)
# Retrieve the index in the occupancy grid matrix corresponding to a position in space.
start_index = tuple(occ_map.metric_to_index(start))
goal_index = tuple(occ_map.metric_to_index(goal))
path = [goal]
i, j, k = occ_map.map.shape
g = np.full((i, j, k), np.inf)
p = np.zeros((i, j, k, 3))
G = []
co = 10
if astar:
h1 = max(
abs(start_index[0] - goal_index[0]) * co,
abs(start_index[1] - goal_index[1]) * co,
abs(start_index[2] - goal_index[2]) * co)
# # euclidean
# h1 = math.sqrt(
# (abs(start_index[0] - goal_index[0]) * co) ** 2 + (abs(start_index[1] - goal_index[1]) * co) ** 2 + \
# (abs(start_index[2] - goal_index[2]) * co) ** 2)
else:
h1 = 0
g[start_index] = h1
count3 = 0
count4 = 0
heappush(G, (g[start_index], start_index))
heappush(G, (g[goal_index], goal_index))
state = np.zeros((i, j, k)) # 0 is not open, 1 is open, 2 is close
state[start_index] = 1 # add start_index to open
print("enter loop")
while np.min(g) < np.inf and not state[
goal_index] == 2: # while there is node not open and the goal is not closed
u = heappop(G)[1]
state[u] = 2
count3 = count3 + 1
for i in range(u[0] - 1, u[0] + 2):
for j in range(u[1] - 1, u[1] + 2):
for k in range(u[2] - 1, u[2] + 2):
v = (i, j, k)
# determine the distance between voxel
if occ_map.is_valid_index(v):
if not occ_map.is_occupied_index(v):
if not state[v] == 2: # if neighbot is not closed
# calculate maximum distance
count4 = count4 + 1
if astar:
h = max(
abs(goal_index[0] - v[0]) * co,
abs(goal_index[1] - v[1]) * co,
abs(goal_index[2] - v[2]) * co)
# euclidean
# h = math.sqrt(
# (abs(goal_index[0] - v[0]) * co) ** 2 + (abs(goal_index[1] - v[1]) * co) ** 2 \
# + (abs(goal_index[2] - v[2]) * co) ** 2)
else:
h = 0
if abs(v[0] - u[0]) + abs(v[1] - u[1]) + abs(
v[2] - u[2]) == 1:
c = 10
elif abs(v[0] - u[0]) + abs(v[1] - u[1]) + abs(
v[2] - u[2]) == 2:
c = 14
else:
c = 17
d = g[u] + c + h
# print("in loop")
if state[
v] == 1: # this neighbor is already opened
if g[v] > d:
G.remove((g[v], v))
g[v] = d
p[v] = u
heappush(G, (g[v], v))
elif state[
v] == 0: # this neighbor haven't been touched
g[v] = d
p[v] = u
heappush(G, (g[v], v))
state[v] = 1
print("count1")
print(count3)
print("count2")
print(count4)
print("out loop")
temp = state[goal_index]
f = p[goal_index]
if (p[goal_index] == (0, 0, 0)).all(): # goal not found
path = None
else:
pointer = goal_index
while pointer != start_index:
par = p[pointer]
path.append(occ_map.index_to_metric_center(par))
ind1 = int(par[0])
ind2 = int(par[1])
ind3 = int(par[2])
pointer = (ind1, ind2, ind3)
path.append(start)
path.reverse()
print(path)
path = np.asarray(path)
print(path.shape)
print(path[1])
print(path[1][0])
print(path[1][1])
n, d = path.shape
removeList = []
for i in range(n - 2):
if path[i + 2][0] - path[i + 1][0] == path[i + 1][0] - path[i][
0] and path[i + 2][1] - path[i + 1][1] == path[
i + 1][1] - path[i][1] and path[i + 2][2] - path[
i + 1][2] == path[i + 1][2] - path[i][2]:
removeList.append(i + 1)
path = np.delete(path, removeList, axis=0)
print(path.shape)
# test for proj1_3
# row, col = path.shape
# start = path[0:row - 1, :]
# end = path[1:row, :]
# tran = end - start
# trow,tcol = tran.shape
# tstart = tran[0:trow-1, :]
# tend = tran[1:trow, :]
# dot_prod = tstart.dot(tend.T)
# se_dot = dot_prod.diagonal()
# start_abs = np.linalg.norm(tstart, axis=1)
# end_abs = np.linalg.norm(tend, axis=1)
# se_abs = np.multiply(start_abs, end_abs)
# print(se_abs)
# print(se_dot)
# se_cos = se_dot / se_abs
# print(se_cos)
# n, d = se_cos.shape
# print(se_cos.shape)
# print(se_cos)
# deg = np.degrees(se_cos)
# degrow, degcol = deg.shape
# deg_s = deg[0:degrow-1,:]
# deg_e = deg[1:degrow,:]
# deg_fin = deg_e - deg_s
# ind = deg_fin.nonz
# new_path = []
# prev = se_cos[0,:]
# curr = []
# for i in range(1:1+n):
# curr = se_cos[i,:]
return path
def graph_search(world, resolution, margin, start, goal, astar):
"""
Parameters:
world, World object representing the environment obstacles
resolution, xyz resolution in meters for an occupancy map, shape=(3,)
margin, minimum allowed distance in meters from path to obstacles.
start, xyz position in meters, shape=(3,)
goal, xyz position in meters, shape=(3,)
astar, if True use A*, else use Dijkstra
Output:
path, xyz position coordinates along the path in meters with
shape=(N,3). These are typically the centers of visited
voxels of an occupancy map. The first point must be the
start and the last point must be the goal. If no path
exists, return None.
"""
# While not required, we have provided an occupancy map you may use or modify.
occ_map = OccupancyMap(world, resolution, margin)
# Retrieve the index in the occupancy grid matrix corresponding to a position in space.
start_index = tuple(occ_map.metric_to_index(start))
goal_index = tuple(occ_map.metric_to_index(goal))
occ_map.create_map_from_world()
all_cost = np.zeros(
occ_map.map.shape
) + 10000 # add a dummy large number for representing inf.
all_cost[start_index] = 0 # add 0 cost for the start
# heapq stores the tuple with cost and index
Q = [] # heapq storing all the neighbor points
if astar is True:
h0 = np.sqrt((resolution[0] * (start_index[0] - goal_index[0]))**2 +
(resolution[1] * (start_index[1] - goal_index[1]))**2 +
(resolution[2] * (start_index[2] - goal_index[2]))**2)
else:
h0 = 0
heappush(Q, (0 + h0, start_index)) # push the start point
parent = dict() # initialize a empty dict for storing parent nodes
# finding the path
while Q:
cur_node = heappop(Q)
# if cur_node[1] is goal_index:
# print('Goal cost is: ', cur_node[0])
# break
for neighbors in find_neighbors(cur_node[1]):
if occ_map.is_valid_index(
neighbors) and not occ_map.is_occupied_index(neighbors):
if astar is True:
h = np.sqrt((resolution[0] *
(neighbors[0] - goal_index[0]))**2 +
(resolution[1] *
(neighbors[1] - goal_index[1]))**2 +
(resolution[2] *
(neighbors[2] - goal_index[2]))**2)
else:
h = 0
cost = all_cost[cur_node[1]] + np.sqrt(
(resolution[0] * (neighbors[0] - cur_node[1][0]))**2 +
(resolution[1] * (neighbors[1] - cur_node[1][1]))**2 +
(resolution[2] * (neighbors[2] - cur_node[1][2]))**2)
if cost < all_cost[neighbors]:
heappush(
Q,
(cost + h, neighbors)) # child found, push into heap
all_cost[neighbors] = cost # update the cost matrix
parent[neighbors] = cur_node[1]
# print('Is goal still in dict? ', goal_index in parent.keys())
# check if the path exists
if goal_index in parent.keys():
pass
else:
return None
# traverse the parent nodes
parent_key = goal_index # initialize the parent key as goal_index
path = list() # initialize the path list
while True: #goal_index in parent.keys():
parent_key = parent[parent_key] # update the next parent key
if parent_key is start_index:
break
parent_in_metric = occ_map.index_to_metric_center(parent_key)
path.append(tuple(parent_in_metric))
path.reverse() # reverse the order back
path.insert(0, start)
path.append(goal)
path = np.asarray(path)
# print('the path is: ', path)
return path