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] traj = np.zeros((T,4)) utraj = np.zeros((T,2))
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] traj = np.zeros((T, 4)) utraj = np.zeros((T, 2))
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() rrt.search(iters=5e3) xpath = np.array([rrt.tree.node[i]['state'] for i in rrt.best_solution(goal)]).T T = xpath.shape[1] traj = np.zeros((T,6)) utraj = np.zeros((T,2)) traj[:,3:6] = xpath.T s = shelve.open('kin_traj.shelve') s['T'] = T s['utraj'] = utraj s['traj'] = traj s.close()