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