def main_heapq():
""" Works with finer grid, with inflated points for start,goal, and obstacles """
#assigning status name to color: start -> red (start) o -> black (obstacle), p -> green (path), g -> blue (goal)
p = 0
s = 1
g = 2
o = 3
#set up the real map parameters:
x_range = [-2, 5] #cln
y_range = [-6, 6] #row
cell_size = 0.1
start_goal_table = [[[2.45, -3.55], [0.95, -1.55]],
[[4.95, -0.05], [2.45, 0.25]],
[[-0.55, 1.45], [1.95, 3.95]]]
#loading landmarks
landmark_table = load_landmark_data()
#set up the data matrix parameters
row_num = int((y_range[1] - y_range[0]) / cell_size)
cln_num = int((x_range[1] - x_range[0]) / cell_size)
# fill in the map data for each path. A figure will show once the previous one is closed.
for path in start_goal_table:
# make an empty data set
data = np.ones((row_num, cln_num)) * np.nan
# [start_coord,goal_coord], getting matrix coord of start and goal
start_mat_coord = to_matrix_coord(path[0], x_range, y_range, cell_size)
goal_mat_coord = to_matrix_coord(path[1], x_range, y_range, cell_size)
inflate_pt_size = 7
# inflate landmarks
for landmark in landmark_table:
lm_mat_coord = to_matrix_coord(landmark, x_range, y_range,
cell_size)
inflated_pt_ls = inflate_point(lm_mat_coord, inflate_pt_size)
for pt in inflated_pt_ls:
if out_of_boudary(pt, row_num, cln_num) == False:
data[pt[0], pt[1]] = o
result = astar_heapq(data, start_mat_coord, goal_mat_coord)
for waypoint in result:
data[waypoint[0]][waypoint[1]] = p
#displaying the final result, after calculating the result
#inflate start and goal
inflated_start = inflate_point_end(start_mat_coord,
inflate_pt_size / 2)
inflated_goal = inflate_point_end(goal_mat_coord, inflate_pt_size / 2)
for pt in inflated_start:
if out_of_boudary(pt, row_num, cln_num) == False:
data[pt[0], pt[1]] = s
for pt in inflated_goal:
if out_of_boudary(pt, row_num, cln_num) == False:
data[pt[0], pt[1]] = g
display_grid(data, x_range, y_range, cell_size,
"World Map - A* Fine-Grid - heuristic 3")
def q11_coarse_main( show_arrows = False, add_noise = False):
#assigning status name to color: start -> red (start) o -> black (obstacle), p -> green (path), g -> blue (goal)
p = 0
s = 1
g = 2
o = 3
#set up the real map parameters:
x_range = [-2,5] #cln
y_range = [-6,6] #row
cell_size = 1
starts =[(0.5,-1.5),(4.5, 3.5),(-0.5, 5.5)]
goals = [(0.5, 1.5),(4.5,-1.5),(1.5,-3.5)]
#so start and goals are put in the form of [[start0,goal0],[start1,goal1]...].This is a 3d arra
start_goal_table = np.array([[starts[i],goals[i]] for i in range(len(starts))])
heading_init = -1*np.pi/2
#loading landmarks
landmark_table = load_landmark_data()
#set up the data matrix parameters
row_num = (y_range[1]-y_range[0])/cell_size
cln_num = (x_range[1]-x_range[0])/cell_size
# fill in the map data for each path. A figure will show once the previous one is closed.
for path in start_goal_table:
# make an empty data set
data = np.ones((row_num, cln_num)) * np.nan
# [start_coord,goal_coord], getting matrix coord of start and goal
start_mat_coord = to_matrix_coord(path[0],x_range,y_range,cell_size)
goal_mat_coord = to_matrix_coord(path[1],x_range,y_range,cell_size)
for landmark in landmark_table:
lm_mat_coord = to_matrix_coord( landmark, x_range,y_range,cell_size)
data[lm_mat_coord[0], lm_mat_coord[1]]=o
#issuing control here TODO
result, turtle_path = astar_online_control_coarse_map(data,start_mat_coord,goal_mat_coord, add_noise)
for waypoint in result:
data[waypoint[0]][waypoint[1]] = p
#displaying the final result, after calculating the result
data[ start_mat_coord[0], start_mat_coord[1]] = s
data[ goal_mat_coord[0], goal_mat_coord[1]] = g
#display_grid(data,x_range,y_range,cell_size,"World Map - Q11 Online Planning + Control")
display_grid_live(data, turtle_path, x_range,y_range,cell_size,"World Map - Q11 Online Planning + Control",show_arrows)
def main_offline():
#assigning status name to color: start -> red (start) o -> black (obstacle), p -> green (path), g -> blue (goal)
p = 0
s = 1
g = 2
o = 3
#set up the real map parameters:
x_range = [-2, 5] #cln
y_range = [-6, 6] #row
cell_size = 1
starts = [(0.5, -1.5), (4.5, 3.5), (-0.5, 5.5)]
goals = [(0.5, 1.5), (4.5, -1.5), (1.5, -3.5)]
#so start and goals are put in the form of [[start0,goal0],[start1,goal1]...].This is a 3d array.
start_goal_table = np.array([[starts[i], goals[i]]
for i in range(len(starts))])
#loading landmarks
landmark_table = load_landmark_data()
#set up the data matrix parameters
row_num = (y_range[1] - y_range[0]) / cell_size
cln_num = (x_range[1] - x_range[0]) / cell_size
# fill in the map data for each path. A figure will show once the previous one is closed.
for path in start_goal_table:
# make an empty data set
data = np.ones((row_num, cln_num)) * np.nan
# [start_coord,goal_coord], getting matrix coord of start and goal
start_mat_coord = to_matrix_coord(path[0], x_range, y_range, cell_size)
goal_mat_coord = to_matrix_coord(path[1], x_range, y_range, cell_size)
for landmark in landmark_table:
lm_mat_coord = to_matrix_coord(landmark, x_range, y_range,
cell_size)
data[lm_mat_coord[0], lm_mat_coord[1]] = o
result = astar(data, start_mat_coord, goal_mat_coord)
for waypoint in result:
data[waypoint[0]][waypoint[1]] = p
#displaying the final result, after calculating the result
data[start_mat_coord[0], start_mat_coord[1]] = s
data[goal_mat_coord[0], goal_mat_coord[1]] = g
display_grid(data, x_range, y_range, cell_size,
"World Map - A* Offline")
def astar_heapq_online_control( true_chessboard, start, end, add_noise = True):
"""
Main difference from the regular implementation: heapque is used for faster sorting.
add (s_node.f,s_node) to Olist, enter while loop to see if list is empty -> heapify Olist ->access the first element to find expanded_node ->heappop the list-> ... -> heappush (child.f,child) when adding a child to the open_list.
Inputs:
1. true_chessboard: matrix that displays the real environment
2. start, end: matrix coordinates of the start and end points of one complete path. [x,y]
3. add noise: flag for adding noise
Outputs:
1. node path -matrix coordinates of waypoints returned from the astar algorthm. [(x1,y1),(x2,y2)...]
2. turtle path- map coordinates returned from controller.
"""
# for controller, set up the real map params
cell_size = 0.1
heading_init = -1*np.pi/2
x_range = [-2,5]
y_range = [-6,6]
start_map = to_map_coord(start,x_range,y_range, cell_size)
turtle_path_start = np.append( start_map, heading_init) #[x,y,theta] in map coordinates
turtle_path_x = np.array([turtle_path_start[0]]) #1 x m np array
turtle_path_y = np.array([turtle_path_start[1]]) #1 x m np array
turtle_path_theta = np.array([turtle_path_start[2]]) #1 x m np array
s_node = Node(None, start)
s_node.g = s_node.h = s_node.f = 0
e_node = Node(None, end)
e_node.g = e_node.h = e_node.f = 0
o = 3
Olist = []
Clist = []
Olist.append((s_node.f,s_node))
num_row = len(true_chessboard)
num_cln = len(true_chessboard[0])
chessboard = np.ones((num_row,num_cln))*np.nan
while len(Olist) > 0:
# update heapq
heapq.heapify(Olist)
expanded_node = Olist[0][1]
chessboard = update_chessboard(chessboard,true_chessboard,expanded_node)
heapq.heappop(Olist)
Clist.append(expanded_node)
if expanded_node == e_node:
turtle_path = ( turtle_path_x, turtle_path_y, turtle_path_theta )
path = []
a = expanded_node
while a is not None:
path.append(a.position)
a = a.parent
return path[::-1], turtle_path
neighbors = []
for new_position in [ (0, 1), (0, -1), (1, 0), (-1, 0), (-1, 1), (-1, -1),(1, 1), (1, -1) ]:
node_position = (expanded_node.position[0] + new_position[0], expanded_node.position[1] + new_position[1])
if node_position[0] < 0 or node_position[0] > (num_row-1) or node_position[1] > (num_cln-1) or node_position[1] < 0:
continue
new_node = Node(expanded_node, node_position)
if not new_node in Clist:
# Append to valid child list
neighbors.append(new_node)
for child in neighbors:
#Please notice that here unoccupied cell is np.nan
if chessboard[child.position[0]][child.position[1]] == o:
child.g = expanded_node.g+1000
else:
child.g = 1 + expanded_node.g
hval = ((child.position[1] - e_node.position[1]) ** 2) + ((child.position[0] - e_node.position[0]) ** 2)
child.h = hval
child.f = hval + child.g
min_child = get_min_child(neighbors)
control_end = to_map_coord( min_child.position, x_range, y_range, cell_size )
control_end = np.append( control_end, 0)
x_vec,y_vec,theta_vec = control_interface( turtle_path_start, control_end, cell_size, add_noise)
turtle_path_start = np.array([x_vec[-1],y_vec[-1],theta_vec[-1]])
turtle_path_x = np.append(turtle_path_x,x_vec)#1*m np array,
turtle_path_y = np.append(turtle_path_y,y_vec)#1*m np array
turtle_path_theta = np.append(turtle_path_theta,theta_vec)#1*m np array
last_pos = turtle_path_start[:2]
last_pos_mat = to_matrix_coord( last_pos, x_range, y_range, cell_size)
if last_pos_mat != min_child.position:
min_child.position = last_pos_mat
Olist.append((min_child.f,min_child))
turtle_path = ( turtle_path_x, turtle_path_y, turtle_path_theta )
return [], turtle_path
def astar_online_control_coarse_map(true_chessboard,
start,
end,
add_noise=True):
# for controller, set up the real map params
cell_size = 1
heading_init = -1 * np.pi / 2
x_range = [-2, 5]
y_range = [-6, 6]
start_map = to_map_coord(start, x_range, y_range, cell_size)
turtle_path_start = np.append(
start_map, heading_init) #[x,y,theta] in map coordinates
turtle_path_x = np.array([turtle_path_start[0]]) #1 x m np array
turtle_path_y = np.array([turtle_path_start[1]]) #1 x m np array
turtle_path_theta = np.array([turtle_path_start[2]]) #1 x m np array
s_node = Node(None, start)
s_node.g = s_node.h = s_node.f = 0
e_node = Node(None, end)
e_node.g = e_node.h = e_node.f = 0
Olist = []
Clist = []
Olist.append(s_node)
num_row = len(true_chessboard)
num_cln = len(true_chessboard[0])
chessboard = np.ones((num_row, num_cln)) * np.nan
while len(Olist) > 0:
expanded_node = Olist[0]
c_index = 0
chessboard = update_chessboard(chessboard, true_chessboard,
expanded_node)
Olist.pop(c_index)
Clist.append(expanded_node)
# Now let's return the closed list
if expanded_node == e_node:
turtle_path = (turtle_path_x, turtle_path_y, turtle_path_theta)
path = []
a = expanded_node
while a is not None:
path.append(a.position)
a = a.parent
return path[::-1], turtle_path
neighbors = []
for new_position in [(0, -1), (0, 1), (-1, 0), (1, 0), (-1, -1),
(-1, 1), (1, -1), (1, 1)]: # Adjacent squares
node_position = (expanded_node.position[0] + new_position[0],
expanded_node.position[1] + new_position[1])
if node_position[0] > (
num_row -
1) or node_position[0] < 0 or node_position[1] > (
num_cln - 1) or node_position[1] < 0:
continue
new_node = Node(expanded_node, node_position)
if not new_node in Clist:
# Append to valid child list
neighbors.append(new_node)
for child in neighbors:
#Please notice that here unoccupied cell is np.nan
if chessboard[child.position[0]][child.position[1]] == o:
child.g = expanded_node.g + 1000
else:
child.g = 1 + expanded_node.g
hval = ((child.position[1] - e_node.position[1])**2) + (
(child.position[0] - e_node.position[0])**2)
child.h = hval
child.f = hval + child.g
# get the min child
min_child = get_min_child(neighbors)
# Issue control here. Also, update your current node.
control_end = to_map_coord(min_child.position, x_range, y_range,
cell_size)
control_end = np.append(control_end, 0)
x_vec, y_vec, theta_vec = control_interface(turtle_path_start,
control_end, cell_size,
add_noise)
#updating turtle_path_start for next iteration
turtle_path_start = np.array([x_vec[-1], y_vec[-1], theta_vec[-1]])
turtle_path_x = np.append(turtle_path_x, x_vec) #1*m np array,
turtle_path_y = np.append(turtle_path_y, y_vec) #1*m np array
turtle_path_theta = np.append(turtle_path_theta,
theta_vec) #1*m np array
#check if controls end up in a wrong cell. If so, change min_child's position, in tuple
# So we only update the location of the child, without changing the closed list.
last_pos = turtle_path_start[:2]
last_pos_mat = to_matrix_coord(last_pos, x_range, y_range, cell_size)
if last_pos_mat != min_child.position:
min_child.position = last_pos_mat
Olist.append(min_child)
turtle_path = (turtle_path_x, turtle_path_y, turtle_path_theta)
return [], turtle_path
def main_q11_fine_grid(show_arrows = False, add_noise = False):
""" Works with finer grid, with inflated points for start,goal, and obstacles """
#configuration variables
#assigning status name to color: start -> red (start) o -> black (obstacle), p -> green (path), g -> blue (goal)
p = 0
s = 1
g = 2
o = 3
#set up the real map parameters:
x_range = [-2,5] #cln
y_range = [-6,6] #row
cell_size = 0.1
starts =[(0.5,-1.5),(4.5, 3.5),(-0.5, 5.5)]
goals = [(0.5, 1.5),(4.5,-1.5),(1.5,-3.5)]
#so start and goals are put in the form of [[start0,goal0],[start1,goal1]...].This is a 3d array.
start_goal_table = np.array([[starts[i],goals[i]] for i in range(len(starts))])
heading_init = -1*np.pi/2
#loading landmarks
landmark_table = load_landmark_data()
#set up the data matrix parameters
row_num = int((y_range[1]-y_range[0])/cell_size)
cln_num = int((x_range[1]-x_range[0])/cell_size)
# fill in the map data for each path. A figure will show once the previous one is closed.
for path in start_goal_table:
# make an empty data set
data = np.ones((row_num, cln_num)) * np.nan
# [start_coord,goal_coord], getting matrix coord of start and goal
start_mat_coord = to_matrix_coord(path[0],x_range,y_range,cell_size)
goal_mat_coord = to_matrix_coord(path[1],x_range,y_range,cell_size)
inflate_pt_size = 7
# inflate landmarks
for landmark in landmark_table:
lm_mat_coord = to_matrix_coord( landmark, x_range,y_range,cell_size)
inflated_pt_ls = inflate_point( lm_mat_coord, inflate_pt_size)
for pt in inflated_pt_ls:
if out_of_boudary(pt,row_num, cln_num) == False:
data[pt[0], pt[1]]=o
result,turtle_path = astar_heapq_online_control(data,start_mat_coord,goal_mat_coord, add_noise) #list
for waypoint in result[1:]:
data[waypoint[0]][waypoint[1]] = p
#displaying the final result, after calculating the result
#inflate start and goal
inflated_start = inflate_point_end( start_mat_coord, inflate_pt_size/2)
inflated_goal = inflate_point_end( goal_mat_coord,inflate_pt_size/2)
for pt in inflated_start:
if out_of_boudary(pt,row_num, cln_num) == False:
data[pt[0],pt[1]] = s
for pt in inflated_goal:
if out_of_boudary(pt,row_num, cln_num) == False:
data[pt[0],pt[1]] = g
display_grid_live(data, turtle_path, x_range,y_range,cell_size,"World Map - Controller + A* Fine-Grid" , show_arrows)
def main_q9(show_arrows=False, add_noise=False):
""" Works with finer grid, with inflated points for start,goal, and obstacles """
#configuration variables
#assigning status name to color: start -> red (start) o -> black (obstacle), p -> green (path), g -> blue (goal)
p = 0
s = 1
g = 2
o = 3
#set up the real map parameters:
x_range = [-2, 5] #cln
y_range = [-6, 6] #row
cell_size = 0.1
start_goal_table = [[[2.45, -3.55], [0.95, -1.55]],
[[4.95, -0.05], [2.45, 0.25]],
[[-0.55, 1.45], [1.95, 3.95]]]
heading_init = -1 * np.pi / 2
#loading landmarks
landmark_table = load_landmark_data()
#set up the data matrix parameters
row_num = int((y_range[1] - y_range[0]) / cell_size)
cln_num = int((x_range[1] - x_range[0]) / cell_size)
# fill in the map data for each path. A figure will show once the previous one is closed.
for path in start_goal_table:
# make an empty data set
data = np.ones((row_num, cln_num)) * np.nan
# [start_coord,goal_coord], getting matrix coord of start and goal
start_mat_coord = to_matrix_coord(path[0], x_range, y_range, cell_size)
goal_mat_coord = to_matrix_coord(path[1], x_range, y_range, cell_size)
inflate_pt_size = 7
# inflate landmarks
for landmark in landmark_table:
lm_mat_coord = to_matrix_coord(landmark, x_range, y_range,
cell_size)
inflated_pt_ls = inflate_point(lm_mat_coord, inflate_pt_size)
for pt in inflated_pt_ls:
if out_of_boudary(pt, row_num, cln_num) == False:
data[pt[0], pt[1]] = o
result = astar_heapq(data, start_mat_coord, goal_mat_coord) #list
turtle_path_start = path[0] #start in map coord for each way point
turtle_path_start.append(heading_init)
turtle_path_x = np.array([turtle_path_start[0]]) #1 x m np array
turtle_path_y = np.array([turtle_path_start[1]]) #1 x m np array
turtle_path_theta = np.array([turtle_path_start[2]]) #1 x m np array
for waypoint in result:
data[waypoint[0]][waypoint[1]] = p
#each waypoint in data is transformed into real map coordinate, then we get a set of turtle's path
control_end = to_map_coord(waypoint, x_range, y_range, cell_size)
control_end = np.append(control_end,
0) #adding dummy 0 to fit into control
x_vec, y_vec, theta_vec = control_interface(
turtle_path_start, control_end, cell_size, add_noise)
turtle_path_start = np.array([x_vec[-1], y_vec[-1], theta_vec[-1]])
turtle_path_x = np.append(turtle_path_x, x_vec) #1*m np array,
turtle_path_y = np.append(turtle_path_y, y_vec) #1*m np array
turtle_path_theta = np.append(turtle_path_theta,
theta_vec) #1*m np array
turtle_path = (turtle_path_x, turtle_path_y, turtle_path_theta)
#displaying the final result, after calculating the result
#inflate start and goal
inflated_start = inflate_point_end(start_mat_coord,
inflate_pt_size / 2)
inflated_goal = inflate_point_end(goal_mat_coord, inflate_pt_size / 2)
for pt in inflated_start:
if out_of_boudary(pt, row_num, cln_num) == False:
data[pt[0], pt[1]] = s
for pt in inflated_goal:
if out_of_boudary(pt, row_num, cln_num) == False:
data[pt[0], pt[1]] = g
display_grid_live(data, turtle_path, x_range, y_range, cell_size,
"World Map - Controller + A* Fine-Grid",
show_arrows)