def run_benchmark(database_folders=[], num_envs=1):
global env_params, lattice_params, cost_fn, heuristic_fn, lattice, planner, start_n, goal_n
# h_weight_list = range(1, 100, 10)
# h_weight_list = [0] + h_weight_list
h_weight_list = [0]
e = Env2D()
print('Running benchmark')
for folder in database_folders:
results = defaultdict(list)
for i in range(num_envs):
curr_env_file = os.path.join(os.path.abspath(folder),
str(i) + '.png')
e.initialize(curr_env_file, env_params)
for h_weight in h_weight_list:
prob_params = {'heuristic_weight': h_weight}
prob = PlanningProblem(prob_params)
prob.initialize(e,
lattice,
cost_fn,
heuristic_fn,
start_n,
goal_n,
visualize=True)
planner.initialize(prob)
path, path_cost, num_expansions, plan_time, came_from, cost_so_far, c_obs = planner.plan(
)
results[h_weight].append((num_expansions, plan_time))
planner.clear_planner() #clear the planner in the end
env_name = os.path.split(os.path.split(os.path.abspath(folder))[0])[1]
output_file_1 = "astar_2d_benchmark.json"
json.dump(results,
open(
os.path.join(
os.path.abspath("../benchmark_results/astar/" +
env_name), output_file_1), 'w'),
sort_keys=True)
output_file_2 = "astar_2d_enchmark_averaged.json"
#Calculate average expansions
avg_results = defaultdict(list)
for k, v in results.iteritems():
avg_expansions = 0
avg_time = 0
for exp, t in v:
avg_expansions += exp
avg_time += t
avg_expansions /= num_envs
avg_time /= num_envs
avg_results[k] = (avg_expansions, avg_time)
json.dump(avg_results,
open(
os.path.join(
os.path.abspath("../benchmark_results/astar/" +
env_name), output_file_2), 'w'),
sort_keys=True)
lattice_params['connectivity'] = 'eight_connected' #Lattice connectivity (can be four or eight connected for xylattice)
lattice_params['path_resolution'] = 1 #Resolution for defining edges and doing collision checking (in meters)
l = XYAnalyticLattice(lattice_params)
#Step 4: Create cost and heuristic objects
cost_fn = PathLengthNoAng() #Penalize length of path
heuristic_fn = EuclideanHeuristicNoAng()
#(Additionally, you can precalculate edges and costs on lattice for speed-ups)
l.precalc_costs(cost_fn) #useful when lattice remains same across problems
#Step 5: Create a planning problem
prob_params = {'heuristic_weight': 1.0}
start_n = l.state_to_node(start)
goal_n = l.state_to_node(goal)
prob = PlanningProblem(prob_params)
prob.initialize(e, l, cost_fn, heuristic_fn, start_n, goal_n, visualize=visualize)
#Step 6: Create Planner object and ask it to solve the planning problem
planner = GreedyPlanner()
planner.initialize(prob)
path, path_cost, num_expansions, plan_time, came_from, cost_so_far, c_obs = planner.plan()
print('Path: ', path)
print('Path Cost: ', path_cost)
print('Number of Expansions: ', num_expansions)
print('Time taken: ', plan_time)
e.initialize_plot(start, goal)
e.plot_path(path, 'solid', 'red', 3)
'origin'] = start # Used for conversion from discrete to continuous and vice-versa.
lattice_params['rotation'] = 0 # Can rotate lattice with respect to world
lattice_params[
'connectivity'] = 'eight_connected' #Lattice connectivity (can be four or eight connected for xylattice)
lattice_params[
'path_resolution'] = 1 #Resolution for defining edges and doing collision checking (in meters)
lattice = XYAnalyticLattice(lattice_params)
# lattice.precalc_costs(cost_fn) #Precalculate costs for speedups
planner = ValueIteration()
start_n = lattice.state_to_node(start)
goal_n = lattice.state_to_node(goal)
prob_params = {
'heuristic_weight': 0.0
} #doesn't matter as VI will ignore them
prob = PlanningProblem(prob_params)
def get_json_dict(d):
new_d = dict()
for key, v in d.items():
n_k = str(key)
new_d[n_k] = v
return new_d
def generate_oracles(database_folders=[],
num_envs=1,
file_start_num=0,
file_type='json'):
global env_params, lattice_params, cost_fn, heuristic_fn, lattice, planner, start_n, goal_n, prob
lattice_params['origin'] = (0, 0) # Used for conversion from discrete to continuous and vice-versa.
lattice_params['rotation'] = 0 # Used for conversion from discrete to continuous and vice-versa (This plus origin define lattice-->world transform)
lattice_params['connectivity'] = 'eight_connected' #Lattice connectivity (can be four or eight connected for xylattice)
lattice_params['path_resolution'] = 1 #Resolution for defining edges and doing collision checking (in meters)
l = XYAnalyticLattice(lattice_params)
#Step 3: Create cost and heuristic objects
cost_fn = PathLengthNoAng() #Penalize length of path
heuristic_fn = EuclideanHeuristicNoAng()
#Step 4: Create a planning problem
prob_params = {'heuristic_weight': 1} #Planner is not greedy at all
start_n = l.state_to_node((0,0))
goal_n = l.state_to_node((100, 100))
prob = PlanningProblem(e, l, cost_fn, heuristic_fn, prob_params, start_n, goal_n, visualize=True)
#Step 4: Create Planner object and ask it to solve the planning problem
planner = Astar(prob)
plan_start_time = time.time()
path, path_cost, num_expansions, came_from, cost_so_far, c_obs = planner.plan()
planning_time = time.time() - plan_start_time
print('Path: ', path)
print('Path Cost: ', path_cost)
print('Number of Expansions: ', num_expansions)
print('Time taken: ', planning_time)
plt.show()