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))
