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 __init__(self, resolution=0.5):
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.settings = 'NonCollisionChecking' # 'NonCollisionChecking' or 'CollisionChecking'
self.env = env(resolution=resolution)
self.start, self.goal = tuple(self.env.start), tuple(self.env.goal)
self.g = {self.start: 0, self.goal: np.inf}
self.Parent = {}
self.CLOSED = set()
self.V = []
self.done = False
self.Path = []
self.ind = 0
self.x0, self.xt = self.start, self.goal
self.OPEN = queue.MinheapPQ() # store [point,priority]
self.OPEN.put(self.x0, self.g[self.x0] +
heuristic_fun(self, self.x0)) # item, priority = g + h
self.lastpoint = self.x0
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.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=0.5):
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.start, self.goal = tuple(self.env.start), tuple(self.env.goal)
self.g = {self.start:0,self.goal:0}
self.OPEN1 = queue.MinheapPQ() # store [point,priority]
self.OPEN2 = queue.MinheapPQ()
self.Parent1, self.Parent2 = {}, {}
self.CLOSED1, self.CLOSED2 = set(), set()
self.V = []
self.done = False
self.Path = []
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