rrt_int = RRT_Interactive(rrt, lqr_rrt.run_forward, plot_dims=[2, 3], slider_range=(0, max_time_horizon)) obstacles_patches = [PolygonPatch(poly) for poly in obstacles_polys] obstacle_patch_collection = PatchCollection(obstacles_patches) rrt_int.int_ax.add_collection(obstacle_patch_collection) if False and __name__ == '__main__': # if False: # rrt.load(shelve.open('kin_rrt.shelve')) i = 0 if i > 0: rrt.load(shelve.open('linship_rrt_%04d.shelve' % (i - 1))) while (not rrt.found_feasible_solution): rrt.search(iters=5e1) s = shelve.open('linship_rrt_%04d.shelve' % i) rrt.save(s) s.close() i += 1 #nearest_id,nearest_distance = rrt.nearest_neighbor(goal) #print 'nearest neighbor distance: %f, cost: %f'%(nearest_distance,rrt.tree.node[nearest_id]['cost']) rrt.search(iters=5e2) xpath = np.array( [rrt.tree.node[i]['state'] for i in rrt.best_solution(goal)]).T T = xpath.shape[1]
rrt.improved_solution_hook = hook rrt_int = RRT_Interactive(rrt,lqr_rrt.run_forward,plot_dims=[2,3],slider_range=(0,max_time_horizon)) obstacles_patches = [PolygonPatch(poly) for poly in obstacles_polys] obstacle_patch_collection = PatchCollection(obstacles_patches) rrt_int.int_ax.add_collection(obstacle_patch_collection) if False and __name__ == '__main__': # if False: # rrt.load(shelve.open('kin_rrt.shelve')) i = 0 if i>0: rrt.load(shelve.open('linship_rrt_%04d.shelve'%(i-1))) while (not rrt.found_feasible_solution): rrt.search(iters=5e1) s = shelve.open('linship_rrt_%04d.shelve'%i) rrt.save(s) s.close() i+=1 #nearest_id,nearest_distance = rrt.nearest_neighbor(goal) #print 'nearest neighbor distance: %f, cost: %f'%(nearest_distance,rrt.tree.node[nearest_id]['cost']) rrt.search(iters=5e2) xpath = np.array([rrt.tree.node[i]['state'] for i in rrt.best_solution(goal)]).T T = xpath.shape[1]
if ani_rrt.found_feasible_solution else "none") if info_text is not None: info_text.set_text(info) else: info_text = ani_ax.figure.text(.8, .5, info, size='small') if __name__ == '__main__': if False: import shelve import os.path p = 'di_rrt_66k.shelve' assert os.path.exists(p) load_shelve = shelve.open(p) rrt.load(load_shelve) import copy interactive_rrt = copy.deepcopy(rrt) x = np.linspace(-1, 1, 1000) X, Y = np.meshgrid(x, x) obstacle_bitmap = obstacles(X, Y) #rasterize the obstacles #ugly globalness info_text = None if True: int_fig = plt.figure(None) int_ax = int_fig.add_subplot(1, 1, 1)
rrt.set_distance_from_goal(distance_from_goal) rrt.gamma_rrt = 40.0 rrt.eta = 0.5 rrt.c = 1 rrt.goal = goal rrt.set_start(start) rrt.init_search() if False: import shelve #load_shelve = shelve.open('examplets/rrt_2d_example.shelve') load_shelve = shelve.open('rrt_0950.shelve') rrt.load(load_shelve) import copy interactive_rrt = copy.deepcopy(rrt) x = np.linspace(-1,1,1000) X,Y = np.meshgrid(x,x) obstacle_bitmap = obstacles(X,Y) #rasterize the obstacles #ugly globalness info_text = None def draw_voronoi(ax,rrt): xr = ax.get_xlim() yr = ax.get_xlim()
rrt.set_distance_from_goal(distance_from_goal) rrt.set_sample(sample) rrt.set_collision_check(collision_check) rrt.set_collision_free(collision_free) rrt.gamma_rrt = 100.0 rrt.eta = 50.0 rrt.c = 1 rrt.set_start(start) rrt.init_search() if __name__ == '__main__': if False: rrt.load(shelve.open('kin_rrt.shelve')) while (not rrt.found_feasible_solution): rrt.search(iters=5e1) nearest_id,nearest_distance = rrt.nearest_neighbor(goal) print 'nearest neighbor distance: %f, cost: %f'%(nearest_distance,rrt.tree.node[nearest_id]['cost']) s = shelve.open('kin_rrt.shelve') rrt.save(s) s.close() s = shelve.open('kin_rrt.shelve') assert set(s.keys()) == set(rrt.save_vars) s.close()