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.g = g_Space(self)
self.start, self.goal = getNearest(self.g, self.env.start), getNearest(self.g, self.env.goal)
self.x0, self.xt = self.start, self.goal
self.rhs = g_Space(self) # rhs(.) = g(.) = inf
self.rhs[self.start] = 0 # rhs(x0) = 0
self.h = Heuristic(self.g, self.goal)
self.OPEN = queue.MinheapPQ() # store [point,priority]
self.OPEN.put(self.x0, [self.h[self.x0],0])
self.CLOSED = set()
# used for A*
self.done = False
self.Path = []
self.V = []
self.ind = 0
# initialize children list
self.CHILDREN = {}
self.getCHILDRENset()
# initialize cost list
self.COST = {}
_ = self.costset()
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.x0, self.xt = tuple(self.env.start), tuple(self.env.goal)
self.b = defaultdict(lambda: defaultdict(dict)
) # back pointers every state has one except xt.
self.OPEN = {
} # OPEN list, here use a hashmap implementation. hash is point, key is value
self.h = {} # estimate from a point to the end point
self.tag = {} # set all states to new
self.V = set() # vertice in closed
# for visualization
self.ind = 0
self.Path = []
self.done = False
self.Obstaclemap = {}
def reset(self, xj):
self.g = g_Space(self) # key is the point, store g value
self.start = xj
self.g[getNearest(self.g, self.start)] = 0 # set g(x0) = 0
self.x0 = xj
self.OPEN.put(self.x0, self.g[self.x0] + heuristic_fun(self,self.x0)) # item, priority = g + h
self.CLOSED = set()