def updatecost(self, range_changed=None): # TODO: update cost when the environment is changed # chaged nodes CHANGED = set() for xi in self.CLOSED: oldchildren = self.CHILDREN[xi] # A # if you don't know where the change occured: if range_changed is None: newchildren = set(children(self, xi)) # B added = newchildren.difference(oldchildren) # B-A removed = oldchildren.difference(newchildren) # A-B self.CHILDREN[xi] = newchildren if added or removed: CHANGED.add(xi) for xj in removed: self.COST[xi][xj] = cost(self, xi, xj) for xj in added: self.COST[xi][xj] = cost(self, xi, xj) # if you do know where on the map changed, only update those changed around that area else: if isinbound(range_changed, xi): newchildren = set(children(self, xi)) # B added = newchildren.difference(oldchildren) # B-A removed = oldchildren.difference(newchildren) # A-B self.CHILDREN[xi] = newchildren if added or removed: CHANGED.add(xi) for xj in removed: self.COST[xi][xj] = cost(self, xi, xj) for xj in added: self.COST[xi][xj] = cost(self, xi, xj) return CHANGED
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 updatecost(self, range_changed=None, new=None, old=None, mode=False): # scan graph for changed cost, if cost is changed update it CHANGED = set() for xi in self.CLOSED: if xi in self.CHILDREN: oldchildren = self.CHILDREN[xi] # A if isinbound(old, xi, mode) or isinbound(new, xi, mode): newchildren = set(children(self, xi)) # B removed = oldchildren.difference(newchildren) intersection = oldchildren.intersection(newchildren) added = newchildren.difference(oldchildren) self.CHILDREN[xi] = newchildren for xj in removed: self.COST[xi][xj] = cost(self, xi, xj) for xj in intersection.union(added): self.COST[xi][xj] = cost(self, xi, xj) CHANGED.add(xi) else: if isinbound(old, xi, mode) or isinbound(new, xi, mode): CHANGED.add(xi) children_added = set(children(self, xi)) self.CHILDREN[xi] = children_added for xj in children_added: self.COST[xi][xj] = cost(self, xi, xj) return CHANGED
def getcost(self, xi, xj): # use a LUT for getting the costd if xi not in self.COST: for (xj, xjcost) in children(self, xi, settings=1): self.COST[xi][xj] = cost(self, xi, xj, xjcost) # this might happen when there is a node changed. if xj not in self.COST[xi]: self.COST[xi][xj] = cost(self, xi, xj) return self.COST[xi][xj]
def path(self): '''After ComputeShortestPath() returns, one can then follow a shortest path from s_start to s_goal by always moving from the current vertex s, starting at s_start. , to any successor s' that minimizes c(s,s') + g(s') until s_goal is reached (ties can be broken arbitrarily).''' path = [] s_goal = self.xt s = self.x0 ind = 0 while s != s_goal: if s == self.x0: children = [ i for i in self.CLOSED if getDist(s, i) <= self.env.resolution * np.sqrt(3) ] else: children = list(self.CHILDREN[s]) snext = children[np.argmin( [cost(self, s, s_p) + self.g[s_p] for s_p in children])] path.append([s, snext]) s = snext if ind > 100: break ind += 1 return path
def process_state(self): # main function of the D star algorithm, perform the process state # around the old path when needed. x, kold = self.min_state() self.tag[x] = 'Closed' self.V.add(x) if x is None: return -1 # check if 1st timer x self.checkState(x) if kold < self.h[x]: # raised states for y in children(self, x): # check y self.checkState(y) a = self.h[y] + cost(self, y, x) if self.h[y] <= kold and self.h[x] > a: self.b[x], self.h[x] = y, a if kold == self.h[x]: # lower for y in children(self, x): # check y self.checkState(y) bb = self.h[x] + cost(self, x, y) if self.tag[y] == 'New' or \ (self.b[y] == x and self.h[y] != bb) or \ (self.b[y] != x and self.h[y] > bb): self.b[y] = x self.insert(y, bb) else: for y in children(self, x): # check y self.checkState(y) bb = self.h[x] + cost(self, x, y) if self.tag[y] == 'New' or \ (self.b[y] == x and self.h[y] != bb): self.b[y] = x self.insert(y, bb) else: if self.b[y] != x and self.h[y] > bb: self.insert(x, self.h[x]) else: if self.b[y] != x and self.h[y] > bb and \ self.tag[y] == 'Closed' and self.h[y] == kold: self.insert(y, self.h[y]) return self.get_kmin()
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 updatecost(self, range_changed=None, new=None, old=None, mode=False): # scan graph for changed Cost, if Cost is changed update it CHANGED = set() for xi in self.CLOSED: if isinbound(old, xi, mode) or isinbound(new, xi, mode): newchildren = set(children(self, xi)) # B self.CHILDREN[xi] = newchildren for xj in newchildren: self.COST[xi][xj] = cost(self, xi, xj) CHANGED.add(xi) return CHANGED
def updatecost(self, range_changed=None, new=None, old=None, mode=False): # scan graph for changed Cost, if Cost is changed update it CHANGED = set() for xi in self.CLOSED: if self.isinobs(old, xi, mode) or self.isinobs(new, xi, mode): # if self.isinobs(new, xi, mode): self.V.remove(xi) # self.V.difference_update({i for i in children(self, xi)}) newchildren = set(children(self, xi)) # B self.CHILDREN[xi] = newchildren for xj in newchildren: self.COST[xi][xj] = cost(self, xi, xj) CHANGED.add(xi) return CHANGED
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): # 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 updateHeuristic(self): # Initialize hvalues at infinity for xi in self.Astar.CLOSED: self.Astar.h[xi] = np.inf Diff = True while Diff: # repeat DP until converge hvals, lasthvals = [], [] for xi in self.Astar.CLOSED: lasthvals.append(self.Astar.h[xi]) # update h values if they are smaller Children = children(self.Astar, xi) minfval = min([ cost(self.Astar, xi, xj, settings=0) + self.Astar.h[xj] for xj in Children ]) # h(s) = h(s') if h(s) > c(s,s') + h(s') if self.Astar.h[xi] >= minfval: self.Astar.h[xi] = minfval hvals.append(self.Astar.h[xi]) if lasthvals == hvals: Diff = False
def modify_cost(self, x): xparent = self.b[x] if self.tag[x] == 'Closed': self.insert(x, self.h[xparent] + cost(self, x, xparent))