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 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 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 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 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 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 move(self): st, localhvals = self.st, self.localhvals maxhval = max(localhvals) sthval = self.Astar.h[st] # find the lowest path up hill while sthval < maxhval: parentsvals , parents = [] , [] # find the max child for xi in children(self.Astar,st): if xi in self.Astar.CLOSED: parents.append(xi) parentsvals.append(self.Astar.h[xi]) lastst = st st = parents[np.argmax(parentsvals)] self.path.append([st,lastst]) # add to path sthval = self.Astar.h[st] self.Astar.reset(self.st)
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 move(self): st = self.Astar.x0 ind = 0 # find the lowest path down hill while st in self.Astar.CLOSED: # when minchild in CLOSED then continue, when minchild in OPEN, stop Children = children(self.Astar, st) minh, minchild = np.inf, None for child in Children: # check collision here, not a supper efficient collide, _ = isCollide(self.Astar, st, child) if collide: continue h = self.Astar.h[child] if h <= minh: minh, minchild = h, child self.path.append([st, minchild]) st = minchild for (_, p) in self.Astar.OPEN.enumerate(): if p == st: break ind += 1 if ind > 1000: break self.Astar.reset(st)
def getchildren(self, xi): if xi not in self.CHILDREN: allchild = children(self, xi) self.CHILDREN[xi] = set(allchild) return self.CHILDREN[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()