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)