def __init__(self, resolution=1):
self.Alldirec = {(1, 0, 0): 1, (0, 1, 0): 1, (0, 0, 1): 1, \
(-1, 0, 0): 1, (0, -1, 0): 1, (0, 0, -1): 1, \
(1, 1, 0): np.sqrt(2), (1, 0, 1): np.sqrt(2), (0, 1, 1): np.sqrt(2), \
(-1, -1, 0): np.sqrt(2), (-1, 0, -1): np.sqrt(2), (0, -1, -1): np.sqrt(2), \
(1, -1, 0): np.sqrt(2), (-1, 1, 0): np.sqrt(2), (1, 0, -1): np.sqrt(2), \
(-1, 0, 1): np.sqrt(2), (0, 1, -1): np.sqrt(2), (0, -1, 1): np.sqrt(2), \
(1, 1, 1): np.sqrt(3), (-1, -1, -1) : np.sqrt(3), \
(1, -1, -1): np.sqrt(3), (-1, 1, -1): np.sqrt(3), (-1, -1, 1): np.sqrt(3), \
(1, 1, -1): np.sqrt(3), (1, -1, 1): np.sqrt(3), (-1, 1, 1): np.sqrt(3)}
self.env = env(resolution=resolution)
#self.X = StateSpace(self.env)
#self.x0, self.xt = getNearest(self.X, self.env.start), getNearest(self.X, self.env.goal)
self.settings = 'CollisionChecking' # for collision checking
self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal)
# self.OPEN = queue.QueuePrior()
self.OPEN = queue.MinheapPQ()
self.km = 0
self.g = {} # all g initialized at inf
self.rhs = {self.xt: 0} # rhs(x0) = 0
self.h = {}
self.OPEN.put(self.xt, self.CalculateKey(self.xt))
self.CLOSED = set()
# init children set:
self.CHILDREN = {}
# init Cost set
self.COST = defaultdict(lambda: defaultdict(dict))
# for visualization
self.V = set() # vertice in closed
self.ind = 0
self.Path = []
self.done = False
def __init__(self, resolution=1):
self.Alldirec = {(1, 0, 0): 1, (0, 1, 0): 1, (0, 0, 1): 1, \
(-1, 0, 0): 1, (0, -1, 0): 1, (0, 0, -1): 1, \
(1, 1, 0): np.sqrt(2), (1, 0, 1): np.sqrt(2), (0, 1, 1): np.sqrt(2), \
(-1, -1, 0): np.sqrt(2), (-1, 0, -1): np.sqrt(2), (0, -1, -1): np.sqrt(2), \
(1, -1, 0): np.sqrt(2), (-1, 1, 0): np.sqrt(2), (1, 0, -1): np.sqrt(2), \
(-1, 0, 1): np.sqrt(2), (0, 1, -1): np.sqrt(2), (0, -1, 1): np.sqrt(2), \
(1, 1, 1): np.sqrt(3), (-1, -1, -1): np.sqrt(3), \
(1, -1, -1): np.sqrt(3), (-1, 1, -1): np.sqrt(3), (-1, -1, 1): np.sqrt(3), \
(1, 1, -1): np.sqrt(3), (1, -1, 1): np.sqrt(3), (-1, 1, 1): np.sqrt(3)}
self.env = env(resolution=resolution)
self.settings = 'CollisionChecking' # for collision checking
self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal)
self.OPEN = queue.MinheapPQ()
self.g = {} # all g initialized at inf
self.h = {}
self.rhs = {self.xt: 0} # rhs(x0) = 0
self.OPEN.put(self.xt, self.key(self.xt))
self.INCONS = set()
self.CLOSED = set()
# init children set:
self.CHILDREN = {}
# init Cost set
self.COST = defaultdict(lambda: defaultdict(dict))
# for visualization
self.V = set() # vertice in closed
self.ind = 0
self.Path = []
self.done = False
# epsilon in the key caculation
self.epsilon = 1
self.increment = 0.1
self.decrement = 0.2