def phi_star(start, goal, grid_obs, newBlockedCells=[]):
print(' Computing Phi* algorithm...')
durations = []
lengths = []
paths = []
x, y = np.mgrid[0:grid_obs.shape[0] + 1, 0:grid_obs.shape[1] + 1]
grid = np.vectorize(Node)(x, y)
start, goal = grid[start], grid[goal]
goal.H, start.G, start.H = 0, 0, dist(start, goal)
newBlockedCells = iter(newBlockedCells)
openset = set()
closedset = set()
t1 = time.time()
duration = 0
i = 0
while True:
print(' Planning #{}'.format(i))
i += 1
path = find_path(start, goal, grid, grid_obs, openset, closedset)
if not DISPLAY:
duration = abs(time.time() - t1)
durations.append(duration)
lengths.append(pathLength(path))
paths.append(list(map(lambda n: n.pos, path)))
if DISPLAY_END:
plot.display(start,
goal,
grid,
grid_obs,
nodes=openset.union(closedset),
path=path)
if not REPLANNING:
break
if DISPLAY_END and WAIT_INPUT:
blockedCells = plot.waitForInput(
grid_obs, lambda: plot.display(start, goal, grid, grid_obs))
else:
try:
blockedCells = next(newBlockedCells)
updateGridBlockedCells(blockedCells, grid_obs)
except StopIteration:
break
t1 = time.time()
for pt in corners(blockedCells):
if (grid[pt] in openset
or grid[pt] in closedset) and grid[pt] != start:
clearSubtree(grid[pt], grid, grid_obs, openset, closedset)
return path, openset.union(closedset), np.array(durations), np.array(
lengths), paths
def find_path(start, goal, grid, obs, openset=set(), closedset=set()):
startTime = time.time()
if len(openset) == 0:
openset.add(start)
i = 0
while openset and min(
map(lambda o: o.G + H_COST_WEIGHT * o.H, openset)
) < goal.G + H_COST_WEIGHT * goal.H and time.time() - startTime < TIME_OUT:
i = i + 1
current = min(openset, key=lambda o: o.G + H_COST_WEIGHT * o.H)
openset.remove(current)
closedset.add(current)
# Loop through the node's children/siblings
for node in children(current, grid, obs, crossbar=False):
# If it is already in the closed set, skip it
if node in closedset:
continue
if node not in openset:
node.G = float('inf')
node.H = dist(node, goal)
node.parent = None
node.ub = float('inf')
node.lb = -float('inf')
openset.add(node)
showPath2 = updateVertex(current, node, grid, obs)
if DISPLAY and i % DISPLAY_FREQ == 0:
plot.display(start,
goal,
grid,
obs,
nodes=openset.union(closedset),
point=current,
point2=node,
showPath2=showPath2)
if not goal.parent:
print(' No path found !')
raise NoPathFound
path = []
current = goal
while current.parent:
path.append(current)
current = current.parent
path.append(current)
return path[::-1]
def cb(xk):
optim[startOptim:startOptim + OPTIMIZED_POINTS, :] = xk.reshape(
-1, 2)
g = df(xk)
print('grad norm', np.linalg.norm(g))
plot.display(None,
None,
grid_obs,
path,
optim,
delta_t=delta_t,
currentOptimIdx=startOptim,
grad=g.reshape(-1, 2),
hold=.01)
def optimTrajectory(path, distObs, grid_obs, trajDuration):
path = np.pad(path, ((6, 6), (0, 0)), mode='edge')
optim = np.copy(path)
delta_t = trajDuration / len(path)
# For each iteration, we optimize between [startOptim, startOptim + OPTIMIZED_POINTS]
startOptim = INITIAL_OPTIM_OFFSET
# Optimization configuration
def objFun(optim, globalPath, startOptim, distObs, delta_t):
def E(x):
inp = np.vstack(
(optim[:startOptim], x.reshape(OPTIMIZED_POINTS, 2),
optim[startOptim + OPTIMIZED_POINTS:]))
return cost(inp, globalPath, startOptim - (6 - OPTIMIZED_POINTS),
distObs, delta_t)
gradE = autograd.grad(E)
return E, gradE
epochs = 20
losses = np.zeros((len(path) - 6 - startOptim - OPTIMIZED_POINTS, epochs))
while startOptim + OPTIMIZED_POINTS <= len(path) - 6:
f, df = objFun(optim, path, startOptim, distObs, delta_t)
x0 = optim[startOptim:startOptim + OPTIMIZED_POINTS].reshape(-1)
def cb(xk):
optim[startOptim:startOptim + OPTIMIZED_POINTS, :] = xk.reshape(
-1, 2)
g = df(xk)
print('grad norm', np.linalg.norm(g))
plot.display(None,
None,
grid_obs,
path,
optim,
delta_t=delta_t,
currentOptimIdx=startOptim,
grad=g.reshape(-1, 2),
hold=.01)
result = scipy.optimize.minimize(f,
x0,
method='BFGS',
jac=df,
callback=cb,
options={'gtol': 1e-4})
print('*' * 30)
print('Optimization result ({} steps):'.format(result.nit),
result.success, result.message)
print('*' * 30)
optim[startOptim:startOptim + OPTIMIZED_POINTS, :] = result.x.reshape(
-1, 2)
plot.display(None,
None,
grid_obs,
path,
optim,
delta_t=delta_t,
currentOptimIdx=startOptim,
hold=.1)
startOptim += 1
return optim
def main():
path, grid_obs, start, goal = phi_star_gen()
# path = np.array(over_sampling([p.pos for p in path], max_length=1))
# path = np.array([p.pos for p in path])
path = np.concatenate((path, np.array([2] * len(path)).reshape(-1, 1)),
axis=1)
edt = euclideanDistanceTransform(grid_obs)
print('EDT done')
pos0 = [0, 0, 2]
vel0 = [.1, .1, 0]
acc0 = [0, 0, 0]
localGoalExtractor = LocalGoalExtractor(path, initPos=pos0, initVel=vel0)
previousTraj = []
try:
for i, goalLocal in enumerate(localGoalExtractor):
trajs = generateTrajLibrary(pos0, vel0, acc0)
for traj in trajs:
traj.compute_cost(goalLocal, edt)
trajSelected = min(trajs, key=lambda t: t._cost)
if trajSelected._cost == np.inf:
plot.display(start,
goal,
grid_obs,
edt=edt,
globalPath=path,
trajLibrary=trajs,
trajHistory=previousTraj,
point=goalLocal,
tf=Tf)
raise ValueError('Cannot find an appropriate trajectory')
pos0 = trajSelected.get_position(Tf)
vel0 = trajSelected.get_velocity(Tf)
acc0 = trajSelected.get_acceleration(Tf)
localGoalExtractor.setPosition(pos0)
localGoalExtractor.setVelocity(vel0)
# Test input feasibility
# inputsFeasible = traj.check_input_feasibility(fmin, fmax, wmax, minTimeSec)
if i % 10 == 0:
plot.display(start,
goal,
grid_obs,
edt=edt,
globalPath=path,
trajLibrary=trajs,
trajSelected=trajSelected,
trajHistory=previousTraj,
point=goalLocal,
tf=Tf)
previousTraj.append(trajSelected)
except:
plot.display(start,
goal,
grid_obs,
edt=edt,
globalPath=path,
trajLibrary=trajs,
trajSelected=trajSelected,
trajHistory=previousTraj,
point=goalLocal,
tf=Tf,
hold=True)