def __init__(self, env_params, lattice_type, lattice_params, cost_fn, learner_params):
self.env_params = env_params
self.cost_fn = cost_fn
self.lattice_type = lattice_type
if lattice_type == "XY":
self.lattice = XYAnalyticLattice(lattice_params)
self.start_n = self.lattice.state_to_node((lattice_params['x_lims'][0], lattice_params['y_lims'][0]))
self.goal_n = self.lattice.state_to_node((lattice_params['x_lims'][1]-1, lattice_params['y_lims'][0]-1))
elif lattice_type == "XYH":
self.lattice = XYHAnalyticLattice(lattice_params)
self.start_n = self.lattice.state_to_node((lattice_params['x_lims'][0], lattice_params['y_lims'][0], 0))
self.goal_n = self.lattice.state_to_node((lattice_params['x_lims'][1]-1, lattice_params['y_lims'][0]-1, 0))
self.lattice.precalc_costs(self.cost_fn) #Enumerate and cache successors and edge costs
self.learner_policy = None #This will be set prior to running a polciy using set_learner_policy
#Data structures for planning
self.frontier = [] #Frontier is un-sorted as it is sorted on demand (using heuristic)
self.oracle_frontier = PriorityQueue() #Frontier sorted according to oracle(for mixing)
self.visited = {} #Keep track of visited cells
self.c_obs = [] #Keep track of collision checks done so far
self.cost_so_far = defaultdict(lambda: np.inf) #Keep track of cost of path to the node
self.came_from = {} #Keep track of parent during search
self.learner = SupervisedRegressionNetwork(learner_params) #learner is a part of the environment
def construct_and_test_lattice(params):
lattice = XYAnalyticLattice(params)
#test node to state conversion
node = (-10,-10)
print('Converting node to state: ', node)
state = lattice.node_to_state(node)
print('State: ', state)
#test state to node conversion
print('Converting state back to node')
node2 = lattice.state_to_node(state)
print('Got back: ', node2)
#Get id for the node
print('Node id %d'%lattice.node_to_id(node2))
#Get successors for node
print('Successsors')
print(lattice.get_successors(node2))
envfile = os.path.abspath("../../motion_planning_datasets/single_bugtrap/train/1.png")
env_params = {'x_lims': [0, 200], 'y_lims': [0, 200]}
e = Env2D()
e.initialize(envfile, env_params)
#Step 3: Create lattice to overlay on environment
lattice_params = dict()
lattice_params['x_lims'] = [0, 200] # Usefule to calculate number of cells in lattice
lattice_params['y_lims'] = [0, 200] # Useful to calculate number of cells in lattice
lattice_params['resolution'] = [1, 1] # Useful to calculate number of cells in lattice + conversion from discrete to continuous space and vice-versa
lattice_params['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)
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
lattice_params[
'x_lims'] = x_lims # Usefule to calculate number of cells in lattice
lattice_params[
'y_lims'] = y_lims # Useful to calculate number of cells in lattice
lattice_params['resolution'] = [
1, 1
] # Useful to calculate number of cells in lattice + conversion from discrete to continuous space and vice-versa
lattice_params[
'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)
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,
lattice_params = dict()
lattice_params[
'x_lims'] = x_lims # Useful to calculate number of cells in lattice
lattice_params[
'y_lims'] = y_lims # Useful to calculate number of cells in lattice
lattice_params['resolution'] = [
1, 1
] # Useful to calculate number of cells in lattice + conversion from discrete to continuous space and vice-versa
lattice_params[
'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)
cost_fn = PathLengthNoAng()
learner_params = dict()
learner_params['output_size'] = 1 #regression
learner_params['input_size'] = 17 #number of features
learner_params['learning_rate'] = 0.001 #not used during testing
learner_params['batch_size'] = 64 #Not used during test
learner_params['training_epochs'] = 20 #Not used during testing
learner_params['seed_val'] = 1234
learner_params['mode'] = "gpu"
learner_params['display_step'] = 5
sail_params = dict()
sail_params['beta0'] = 0 #Initial beta (after iter 0)
sail_params['k'] = 60 #Number of datapoitns to collect per environment
envfile = os.path.abspath("../../databases/bugtrap.png")
env_params = {'x_lims': [0, 100], 'y_lims': [0,100]}
e = Env2D()
e.initialize(envfile, env_params)
#Step 2: Create lattice to overlay on environment
lattice_params = dict()
lattice_params['x_lims'] = [0, 100] # Used to calculate number of cells in lattice (should ideally be consistent with env_params['x_lims'])
lattice_params['y_lims'] = [0, 100] # Used to calculate number of cells in lattice (should ideally be consistent with env_params['y_lims'])
lattice_params['resolution'] = [1, 1] # Used to calculate number of cells in lattice + conversion from discrete to continuous space and vice-versa
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()
from planning_python.environment_interface.env_2d import Env2D
from planning_python.state_lattices.common_lattice.xy_analytic_lattice import XYAnalyticLattice
from planning_python.cost_functions.cost_function import PathLengthNoAng
from planning_python.heuristic_functions.heuristic_function import EuclideanHeuristicNoAng, ManhattanHeuristicNoAng
from planning_python.data_structures.planning_problem import PlanningProblem
from planning_python.planners.astar import Astar
x_lims = [0, 200]
y_lims = [0, 200]
env_params = {'x_lims': x_lims, 'y_lims': y_lims}
lattice_params = {'x_lims': x_lims, 'y_lims': y_lims, 'resolution': [1, 1], 'origin': (0, 0), 'rotation': 0, 'connectivity': 'eight_connected', 'path_resolution': 1}
h_weight_list = [1] #We will run Astar by putting differen weights on the heuristic each time
cost_fn = PathLengthNoAng()
heuristic_fn = EuclideanHeuristicNoAng()
lattice = XYAnalyticLattice(lattice_params)
planner = Astar()
start_n = lattice.state_to_node((0,0))
goal_n = lattice.state_to_node((199, 199))
visualize = False
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, visualize
lattice.precalc_costs(cost_fn)
e = Env2D()
print('Running benchmark')
for folder in database_folders:
results= defaultdict(list)
x_lims = [0, 200]
y_lims = [0, 200]
env_params = {'x_lims': x_lims, 'y_lims': y_lims}
lattice_params = {
'x_lims': x_lims,
'y_lims': y_lims,
'resolution': [1, 1],
'origin': (0, 0),
'rotation': 0,
'connectivity': 'eight_connected',
'path_resolution': 1
}
cost_fn = PathLengthNoAng()
heuristic_fn = EuclideanHeuristicNoAng()
lattice = XYAnalyticLattice(lattice_params)
lattice.precalc_costs(cost_fn)
planner = Astar()
start_n = lattice.state_to_node((0, 0))
goal_n = lattice.state_to_node((199, 199))
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')