def performing(self):
rrt = RRT(None, None, self.obstacle_list, self.boundary)
n_expand = math.ceil(self.plan_time / self.plot_interval)
self.bin_list = [self.plot_interval * i for i in range(1, n_expand+1)]
self.paths = [[] for i in range(n_expand)]
for i in range(self.num_space):
initial = rrt.get_random_mps()
while not rrt.check_collision_obstacle(initial, self.obstacle_list):
initial = rrt.get_random_mps()
goal = rrt.get_random_mps()
while not rrt.check_collision_obstacle(goal, self.obstacle_list):
goal = rrt.get_random_mps()
testing = self.Planning(self.plan_time, self.plot_interval)
path_list = testing.find_shortest_path(RRT, initial, goal, self.obstacle_list, self.boundary)
for i in range(len(path_list)):
if path_list[i] == float("inf"):
self.paths[i].append(0)
else:
self.paths[i].append(path_list[i])
print(initial, goal, path_list)
self.cal_performance()
self.plot_performance(show = True)
return self.mean_list[-1]
def summary_2(start, goal, obstacle_array, boundary, habitats, shark_dict,
sharkGrid, test_num, test_time, plot_interval, weights):
'''generate the average cost of optimal paths of one weight scheme'''
cost_list = [[] for _ in range(math.ceil(test_time // plot_interval))]
improvement = []
for _ in range(test_num):
rrt = RRT(start, goal, boundary, obstacle_array, habitats)
if weights[1] == "random time":
plan_time = True
if weights[2] == "trajectory time stamp":
traj_time_stamp = True
else:
traj_time_stamp = False
elif weights[1] == "random (x,y)":
plan_time = False
traj_time_stamp = False
result = rrt.exploring(shark_dict,
sharkGrid,
plot_interval,
5,
1,
50,
traj_time_stamp=traj_time_stamp,
max_plan_time=test_time,
plan_time=plan_time,
weights=weights[0])
if result:
cost = result["cost list"]
for i in range(len(cost)):
cost_list[i].append(cost[i])
cost_mean = []
for i in range(len(cost_list)):
temp_mean = statistics.mean(cost_list[i])
if i >= 1:
improvement.append("{:.0%}".format(temp_mean / cost_mean[-1]))
cost_mean.append(temp_mean)
#plot_summary_2(time_list, cost_list)
#print(cost_mean, improvement)
return cost_mean, improvement
def plot_time_stamp(start, goal, boundary, obstacle_array, habitats):
'''draw time stamp distribution of one rrt_rubins path planning algorithm'''
rrt = RRT(start, goal, boundary, obstacle_array, habitats)
result = rrt.exploring(habitats,
0.5,
5,
1,
max_plan_time=10.0,
weights=[1, -4.5, -4.5])
time_stamp_list = result["time stamp"]
bin_list = time_stamp_list.keys()
num_time_list = []
for time_bin in bin_list:
num_time_list.append(len(time_stamp_list[time_bin]))
plt.title("time stamp distribution")
plt.xlabel("time stamp bin")
plt.ylabel("number of motion_plan_states")
#plt.xticks(self.bin_list)
plt.bar(bin_list, num_time_list, color="g")
plt.show()
def find_shortest_path(self, planner, initial, goal, obstacle_list, boundary):
n_expand = math.ceil(self.plan_time / self.plot_interval)
ori_t = time.time()
for i in range(1, n_expand+1):
t_end = ori_t + i * self.plot_interval
while time.time() < t_end:
path_planning = RRT(initial, goal, obstacle_list, boundary)
result = path_planning.planning()
if result is not None:
self.get_shortest(result)
#self.plot_perf()
self.shrtpath_list.append(self.shortest_length)
self.time_list.append(time.time() - ori_t)
self.plot_perf()
#return self.shortest_length, self.shortest_path
return self.shrtpath_list
def summary_5(obstacles,
boundary,
habitats,
sharkGrid,
cell_list,
test_num=50):
nums = [0, 3, 5, 7, 8, 10, 11, 12, 13, 15, 16]
rrt = RRT(boundary, sharkGrid, cell_list)
res = []
for num in nums:
temp_obstacles = []
while len(temp_obstacles) < num:
ran = int(random.uniform(0, len(habitats)))
temp_obstacles.append(obstacles[ran])
time_list = []
for _ in range(test_num):
initial_x = random.uniform(-300, -100)
initial_y = random.uniform(-100, 100)
initial = Point(initial_x, initial_y)
while not initial.within(boundary_poly):
initial_x = random.uniform(-300, -100)
initial_y = random.uniform(-100, 100)
initial = Point(initial_x, initial_y)
initial = Motion_plan_state(initial_x, initial_y)
start = time.time()
rrt.exploring(initial, habitats, temp_obstacles, 5, 50, 1, 50,
True, 100, 500, True, [-3, -3, -4])
end = time.time()
time_list.append((end - start))
res.append(statistics.mean(time_list))
return res
def summary_1(weight,
obstacles,
boundary,
habitats,
shark_dict,
sharkGrid,
cell_list,
test_num=10):
'''
generate a summary about each term of one specific cost function, given randomly chosen environment
test_num: the number of tests to run under a specific cost function
output:
cost_avr: a dictionary summarizing the result of each term of the cost function,
key will be weight i.e. w1, w2, ...
value will be the average cost of each term
'''
# sharkOccupancyGrid = SharkOccupancyGrid(10, boundary, 50, 50, cell_list)
for comp in [4, 5, 7, 8, 9, 10]:
cost_summary_ex_whabitat_cal = []
cost_summary_ex_whabitat_plan = []
cost_summary_ex_wohabitat = []
# cost_summary_rp = []
# ran_hab = int(random.uniform(0, comp))
temp_habitats = []
# temp_shark = shark_dict
while len(temp_habitats) < comp and len(temp_habitats) < len(habitats):
temp = math.floor(random.uniform(0, len(habitats)))
while temp >= len(habitats):
temp = math.floor(random.uniform(0, len(habitats)))
temp_habitats.append(habitats[temp])
# ran_shark = comp - len(temp_habitats)
# while len(list(temp_shark.keys())) < ran_shark and len(list(temp_shark.keys())) < len(list(shark_dict.keys())):
# temp = math.floor(random.uniform(1, len(list(shark_dict.keys()))))
# while temp >= len(list(shark_dict.keys())):
# temp = math.floor(random.uniform(0, len(habitats)))
# temp_shark[temp] = shark_dict[temp]
print(len(temp_habitats))
temp_sharkGrid = sharkGrid
temp_cost_ex_whabitat_cal = []
temp_cost_ex_wohabitat = []
temp_cost_ex_whabitat_plan = []
# temp_cost_rp = []
for _ in range(test_num):
initial_x = random.uniform(-300, -100)
initial_y = random.uniform(-100, 100)
initial = Point(initial_x, initial_y)
while not initial.within(boundary_poly):
initial_x = random.uniform(-300, -100)
initial_y = random.uniform(-100, 100)
initial = Point(initial_x, initial_y)
initial = Motion_plan_state(initial_x, initial_y)
testing = RRT(boundary, obstacles, temp_sharkGrid, cell_list)
res1 = testing.exploring(initial, [],
0.5,
5,
1,
50,
True,
20.0,
500.0,
weights=weight)
path = res1['path'][0]
cost = habitat_shark_cost_func(path, path[-1].traj_time_stamp,
temp_habitats, temp_sharkGrid,
weight)
# print(res1["cost"], cost)
temp_cost_ex_wohabitat.append(res1["cost"][0])
temp_cost_ex_whabitat_cal.append(cost[0])
res3 = testing.exploring(initial,
temp_habitats,
0.5,
5,
1,
50,
True,
20.0,
500.0,
weights=weight)
# print(res3["cost"])
temp_cost_ex_whabitat_plan.append(res3["cost"][0])
# res2 = testing.replanning(initial, habitats, 10.0, 100.0, 0.1, weight)
# print(res2[2])
# temp_cost_rp.append(res2[2][0])
cost_summary_ex_whabitat_cal.append(
statistics.mean(temp_cost_ex_whabitat_cal))
cost_summary_ex_whabitat_plan.append(
statistics.mean(temp_cost_ex_whabitat_plan))
cost_summary_ex_wohabitat.append(
statistics.mean(temp_cost_ex_wohabitat))
# cost_summary_rp.append(statistics.mean(temp_cost_rp))
print(
str(comp) + "consider habitats in planning: " +
str(cost_summary_ex_whabitat_plan) + ", not consider habitats: " +
str(cost_summary_ex_wohabitat) +
", consider habitats in calculation: " +
str(cost_summary_ex_whabitat_cal))
return
def summary_1(weight,
obstacles,
boundary,
habitats,
shark_dict,
sharkGrid,
test_num=100):
'''
generate a summary about each term of one specific cost function, given randomly chosen environment
cost_func: a list of lists of weights assigned to each term in the cost function
test_num: the number of tests to run under a specific cost function
output:
cost_avr: a dictionary summarizing the result of each term of the cost function,
key will be weight i.e. w1, w2, ...
value will be the average cost of each term
'''
testing = RRT(boundary, obstacles, shark_dict, sharkGrid)
cost_summary_ex = [[] for _ in range(len(weight))]
cost_summary_rp = [[] for _ in range(len(weight))]
for _ in range(test_num):
initial_x = random.uniform(-300, -100)
initial_y = random.uniform(-100, 100)
initial = Point(initial_x, initial_y)
while not initial.within(boundary_poly):
initial_x = random.uniform(-300, -100)
initial_y = random.uniform(-100, 100)
initial = Point(initial_x, initial_y)
initial = Motion_plan_state(initial_x, initial_y)
res1 = testing.exploring(initial,
habitats,
0.5,
5,
1,
50,
True,
20.0,
500.0,
weights=weight)
print(res1["cost"])
for i in range(len(res1["cost"][1])):
cost_summary_ex[i].append(res1["cost"][1][i])
res2 = testing.replanning(initial, habitats, 10.0, 100.0, 0.1)
print(res2[2])
for i in range(len(res2[2][1])):
cost_summary_rp[i].append(res2[2][1][i])
#calculate average cost for each term
result1 = []
for cost in cost_summary_ex:
result1.append(statistics.mean(cost))
result2 = []
for cost in cost_summary_rp:
result2.append(statistics.mean(cost))
return [result2, result1]