def run(self): while True: if self.Astar.run(N=self.N): self.Astar.Path = self.Astar.Path + self.path self.Astar.done = True visualization(self.Astar) plt.show() break self.updateHeuristic() self.move()
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 Main(self): ischanged = False islargelychanged = False t = 0 self.ComputeorImprovePath() # TODO publish current epsilon sub-optimal solution self.done = True self.ind = 0 self.Path = self.path() visualization(self) while True: visualization(self) if t == 20: break # change environment # new2,old2 = self.env.move_block(theta = [0,0,0.1*t], mode='rotation') # new2, old2 = self.env.move_block(a=[0, 0, -0.2], mode='translation') new2, old2 = self.env.move_OBB(theta=[10 * t, 0, 0], translation=[0, 0.1 * t, 0]) mmode = 'obb' # obb or aabb ischanged = True # islargelychanged = True self.Path = [] # update Cost with changed environment if ischanged: # CHANGED = self.updatecost(True, new2, old2, mode='obb') CHANGED = self.updatecost(True, new2, old2, mode=mmode) for u in CHANGED: self.UpdateState(u) self.ComputeorImprovePath() ischanged = False if islargelychanged: self.epsilon += self.increment # or replan from scratch elif self.epsilon > 1: self.epsilon -= self.decrement # move states from the INCONS to OPEN # update priorities in OPEN Allnodes = self.INCONS.union(self.OPEN.allnodes()) for node in Allnodes: self.OPEN.put(node, self.key(node, self.epsilon)) self.INCONS = set() self.CLOSED = set() self.ComputeorImprovePath() # publish current epsilon sub optimal solution self.Path = self.path() # if epsilon == 1: # wait for change to occur t += 1
def run(self): # put G (ending state) into the OPEN list self.OPEN[self.xt] = 0 self.tag[self.x0] = 'New' # first run while True: # TODO: self.x0 = self.process_state() # visualization(self) if self.tag[self.x0] == "Closed": break self.ind += 1 self.Path = self.path() self.done = True visualization(self) plt.pause(0.2) # plt.show() # when the environemnt changes over time for i in range(5): self.env.move_block(a=[0.1, 0, 0], s=0.5, block_to_move=1, mode='translation') self.env.move_block(a=[0, 0, -0.25], s=0.5, block_to_move=0, mode='translation') # travel from end to start s = tuple(self.env.start) # self.V = set() while s != self.xt: if s == tuple(self.env.start): sparent = self.b[self.x0] else: sparent = self.b[s] # if there is a change of cost, or a collision. if cost(self, s, sparent) == np.inf: self.modify(s) continue self.ind += 1 s = sparent 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 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 ComputePath(self): print('computing path ...') while self.key(self.xt) > self.OPEN.top_key() or self.rhs[self.xt] != self.g[self.xt]: xi = self.OPEN.get() # if g > rhs, overconsistent if self.g[xi] > self.rhs[xi]: self.g[xi] = self.rhs[xi] # add xi to expanded node set if xi not in self.CLOSED: self.V.append(xi) self.CLOSED.add(xi) else: # underconsistent and consistent self.g[xi] = np.inf self.UpdateMembership(xi) for xj in self.CHILDREN[xi]: self.UpdateMembership(xj) # visualization(self) self.ind += 1 self.Path = self.path() self.done = True visualization(self) plt.pause(2)
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()