def high_plan(self, start, end):
"""
Create a high level plan to bais sampling
Inputs:
start = start position of the sub
end = goal position of the sub
"""
start = self.locate_region(start)
end = self.locate_region(end)
count = 0
for k in range(len(self.z) - 1):
for i in range(len(self.x) - 1):
for j in range(len(self.y) - 1):
# go to the right
if not count % self.dis_size == self.dis_size - 1:
index_count = self.get_connection_index(
count, count + 1)
if index_count:
# compute the cost of transition for each region
self.W[index_count] = self.cost(
count, count + 1, index_count)
# go to the bottom
if count % self.dis_size_big not in range(
self.dis_size_big - self.dis_size,
self.dis_size_big):
index_count = self.get_connection_index(
count, count + self.dis_size)
if index_count:
self.W[index_count] = self.cost(
count, count + self.dis_size, index_count)
#go to the back
if not count > (self.dis_size**3 - self.dis_size_big - 1):
index_count = self.get_connection_index(
count, count + self.dis_size_big)
if index_count:
self.W[index_count] = self.cost(
count, count + self.dis_size_big, index_count)
count += 1
astar = A_star()
# occationally choose a random path that will go to goal but not optimal
path_type = np.random.choice([0, 1], p=[0.05, 0.95])
if path_type:
h = [0] * len(self.R)
else:
# make random costs
h = np.random.uniform(0, max(self.W), len(self.R))
# use A* to compute the high level path
self.high_path = astar.construct_path_fast(
[self.R, self.edges, self.W], start, end, h)
def high_plan_task(self, start, end, place):
"""
Create a high level plan to bais sampling
Inputs:
start = start position of the sub
end = goal position of the sub
"""
closest_start = self.locate_region(start)
self.closest_start = 'x' + str(closest_start) + 'q' + str(place)
closest_end = self.locate_region(end)
self.closest_end = 'x' + str(closest_end) + 'q' + str(place + 1)
count = 0
for k in range(len(self.z) - 1):
for i in range(len(self.x) - 1):
for j in range(len(self.y) - 1):
# go to the right
if not count % self.dis_size == self.dis_size - 1:
index_count = self.get_connection_index(
count, count + 1)
if index_count:
# compute the cost of transition for each region
if index_count in self.edges_task_index.keys():
indicies = self.edges_task_index[index_count]
current_cost = self.cost(
count, count + 1, index_count)
for ind in indicies:
self.W_task[ind] = current_cost
# go to the bottom
if count % self.dis_size_big not in range(
self.dis_size_big - self.dis_size,
self.dis_size_big):
index_count = self.get_connection_index(
count, count + self.dis_size)
if index_count:
if index_count in self.edges_task_index.keys():
indicies = self.edges_task_index[index_count]
current_cost = self.cost(
count, count + self.dis_size, index_count)
for ind in indicies:
self.W_task[ind] = current_cost
#go to the back
if not count > (self.dis_size**3 - self.dis_size_big - 1):
index_count = self.get_connection_index(
count, count + self.dis_size_big)
if index_count:
if index_count in self.edges_task_index.keys():
indicies = self.edges_task_index[index_count]
current_cost = self.cost(
count, count + self.dis_size_big,
index_count)
for ind in indicies:
self.W_task[ind] = current_cost
count += 1
astar = A_star()
# occationally choose a random path that will go to goal but not optimal
path_type = np.random.choice([0, 1], p=[0.05, 0.95])
if path_type:
h = [0] * len(self.R_task)
else:
# make random costs
h = np.random.uniform(0, max(self.W_task), len(self.R_task))
# use A* to compute the high level path
self.high_path = astar.construct_path_fast(
[self.R_task, self.edges_task, self.W_task], self.closest_start,
self.closest_end, h)
# print('path',self.high_path)
self.high_path = [int(x[1:-2]) for x in self.high_path]
