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