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()