def evaluation(self, allchild, xi, conf): for xj in allchild: if conf == 1: if xj not in self.CLOSED1: if xj not in self.g: self.g[xj] = np.inf else: pass gi = self.g[xi] a = gi + cost(self,xi,xj) if a < self.g[xj]: self.g[xj] = a self.Parent1[xj] = xi self.OPEN1.put(xj, a+1*heuristic_fun(self,xj,self.goal)) if conf == 2: if xj not in self.CLOSED2: if xj not in self.g: self.g[xj] = np.inf else: pass gi = self.g[xi] a = gi + cost(self,xi,xj) if a < self.g[xj]: self.g[xj] = a self.Parent2[xj] = xi self.OPEN2.put(xj, a+1*heuristic_fun(self,xj,self.start))
def key(self, s, epsilon=1): if self.getg(s) > self.getrhs(s): return [ self.rhs[s] + epsilon * heuristic_fun(self, s, self.x0), self.rhs[s] ] else: return [ self.getg(s) + heuristic_fun(self, s, self.x0), self.getg(s) ]
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 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()
def main(self): s_last = self.x0 s_start = self.x0 print('first run ...') self.ComputeShortestPath() self.Path = self.path() self.done = True visualization(self) plt.pause(0.5) # plt.show() # change the environment print('running with map update ...') for i in range(100): range_changed1 = self.env.move_block(a=[0, 0, -0.1], s=0.5, block_to_move=0, mode='translation') range_changed2 = self.env.move_block(a=[0.1, 0, 0], s=0.5, block_to_move=1, mode='translation') range_changed3 = self.env.move_block(a=[0, 0.1, 0], s=0.5, block_to_move=2, mode='translation') #range_changed = self.env.move_block(a=[0.1, 0, 0], s=0.5, block_to_move=1, mode='translation') # update the edge cost of c(u,v) CHANGED1 = self.updatecost(range_changed1) CHANGED2 = self.updatecost(range_changed2) CHANGED3 = self.updatecost(range_changed3) CHANGED2 = CHANGED2.union(CHANGED1) CHANGED = CHANGED3.union(CHANGED2) while getDist(s_start, self.xt) > 2 * self.env.resolution: if s_start == self.x0: children = [ i for i in self.CLOSED if getDist(s_start, i) <= self.env.resolution * np.sqrt(3) ] else: children = list(self.CHILDREN[s_start]) s_start = children[np.argmin([ cost(self, s_start, s_p) + self.g[s_p] for s_p in children ])] # for all directed edges (u,v) with changed costs if CHANGED: self.km = self.km + heuristic_fun(self, s_start, s_last) for u in CHANGED: self.UpdateVertex(u) s_last = s_start self.ComputeShortestPath() self.Path = self.path() visualization(self) plt.show()
def run(self): x0, xt = self.start, self.goal self.OPEN1.put(x0, self.g[x0] + heuristic_fun(self,x0,xt)) # item, priority = g + h self.OPEN2.put(xt, self.g[xt] + heuristic_fun(self,xt,x0)) # item, priority = g + h self.ind = 0 while not self.CLOSED1.intersection(self.CLOSED2): # while xt not reached and open is not empty xi1, xi2 = self.OPEN1.get(), self.OPEN2.get() self.CLOSED1.add(xi1) # add the point in CLOSED set self.CLOSED2.add(xi2) self.V.append(xi1) self.V.append(xi2) # visualization(self) allchild1, allchild2 = children(self,xi1), children(self,xi2) self.evaluation(allchild1,xi1,conf=1) self.evaluation(allchild2,xi2,conf=2) if self.ind % 100 == 0: print('iteration number = '+ str(self.ind)) self.ind += 1 self.common = self.CLOSED1.intersection(self.CLOSED2) self.done = True self.Path = self.path() visualization(self) plt.show()
def run(self, N=None): xt = self.xt xi = self.x0 while self.OPEN: # while xt not reached and open is not empty xi = self.OPEN.get() if xi not in self.CLOSED: self.V.append(np.array(xi)) self.CLOSED.add(xi) # add the point in CLOSED set if getDist(xi, xt) < self.env.resolution: break # visualization(self) for xj in children(self, xi): # if xj not in self.CLOSED: if xj not in self.g: self.g[xj] = np.inf else: pass a = self.g[xi] + cost(self, xi, xj) if a < self.g[xj]: self.g[xj] = a self.Parent[xj] = xi # if (a, xj) in self.OPEN.enumerate(): # update priority of xj self.OPEN.put(xj, a + 1 * heuristic_fun(self, xj)) # else: # add xj in to OPEN set # self.OPEN.put(xj, a + 1 * heuristic_fun(self, xj)) # For specified expanded nodes, used primarily in LRTA* if N: if len(self.CLOSED) % N == 0: break if self.ind % 100 == 0: print('number node expanded = ' + str(len(self.V))) self.ind += 1 self.lastpoint = xi # if the path finding is finished if self.lastpoint in self.CLOSED: self.done = True self.Path = self.path() if N is None: #visualization(self) plt.show() return True return False
def geth(self, xi): # when the heurisitic is first calculated if xi not in self.h: self.h[xi] = heuristic_fun(self, xi, self.x0) return self.h[xi]
def main(self): s_last = self.x0 print('first run ...') self.ComputeShortestPath() self.Path = self.path() self.done = True visualization(self) plt.pause(0.5) # plt.show() print('running with map update ...') t = 0 # count time ischanged = False self.V = set() while getDist(self.x0, self.xt) > 2 * self.env.resolution: #---------------------------------- at specific times, the environment is changed and Cost is updated if t % 2 == 0: new0, old0 = self.env.move_block(a=[-0.1, 0, -0.2], s=0.5, block_to_move=1, mode='translation') new1, old1 = self.env.move_block(a=[0, 0, -0.2], s=0.5, block_to_move=0, mode='translation') new2, old2 = self.env.move_block(theta=[0, 0, 0.1 * t], mode='rotation') #new2,old2 = self.env.move_block(a=[-0.3, 0, -0.1], s=0.5, block_to_move=1, mode='translation') ischanged = True self.Path = [] #----------------------------------- traverse the route as originally planned if t == 0: children_new = [ i for i in self.CLOSED if getDist(self.x0, i) <= self.env.resolution * np.sqrt(3) ] else: children_new = list(children(self, self.x0)) self.x0 = children_new[np.argmin([ self.getcost(self.x0, s_p) + self.getg(s_p) for s_p in children_new ])] # TODO add the moving robot position codes self.env.start = self.x0 # ---------------------------------- if any Cost changed, update km, reset slast, # for all directed edgees (u,v) with chaged edge costs, # update the edge Cost cBest(u,v) and update vertex u. then replan if ischanged: self.km += heuristic_fun(self, self.x0, s_last) s_last = self.x0 CHANGED = self.updatecost(True, new0, old0) CHANGED1 = self.updatecost(True, new1, old1) CHANGED2 = self.updatecost(True, new2, old2, mode='obb') CHANGED = CHANGED.union(CHANGED1, CHANGED2) # self.V = set() for u in CHANGED: self.UpdateVertex(u) self.ComputeShortestPath() ischanged = False self.Path = self.path(self.x0) visualization(self) t += 1 plt.show()