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