lqr_rrt.max_nodes_per_extension = 5 rrt.sample_goal = lambda: goal rrt.set_distance(lqr_rrt.distance_cache) rrt.set_same_state(lqr_rrt.same_state) rrt.set_cost(lqr_rrt.cost) rrt.set_steer(lqr_rrt.steer_cache) rrt.set_goal_test(goal_test) rrt.set_sample(sample) rrt.set_collision_free(lqr_rrt.collision_free) rrt.set_distance_from_goal(distance_from_goal) rrt.gamma_rrt = 2 rrt.eta = 1000 rrt.c = 1 rrt.max_nodes_in_ball = 30 rrt.set_start(start) rrt.init_search() rrt_int = RRT_Interactive(rrt, lqr_rrt.run_forward, plot_dims=[0, 1], slider_range=(0, max_time_horizon)) def draw(rrt, ani_ax=None): if ani_ax is None:
lqr_rrt.max_nodes_per_extension = 5 rrt.sample_goal = lambda: goal rrt.set_distance(lqr_rrt.distance_cache) rrt.set_same_state(lqr_rrt.same_state) rrt.set_cost(lqr_rrt.cost) rrt.set_steer(lqr_rrt.steer_cache) rrt.set_goal_test(goal_test) rrt.set_sample(sample) rrt.set_collision_free(lqr_rrt.collision_free) rrt.set_distance_from_goal(distance_from_goal) rrt.gamma_rrt = 1.0 rrt.eta = .2 rrt.c = 1 rrt.max_nodes_in_ball = 30 lqr_rrt.max_steer_cost = .015 rrt.set_start(start) rrt.init_search() def draw(rrt, ani_ax=None): if ani_ax is None: ani_ax = plt.figure().gca() ani_ax.cla()
rrt.sample_goal = lambda : goal rrt.set_distance(lqr_rrt.distance_cache) rrt.set_same_state(lqr_rrt.same_state) rrt.set_cost(lqr_rrt.cost) rrt.set_steer(lqr_rrt.steer_cache) rrt.set_goal_test(goal_test) rrt.set_sample(sample) rrt.set_collision_free(lqr_rrt.collision_free) rrt.set_distance_from_goal(distance_from_goal) rrt.gamma_rrt = 2 rrt.eta = 1000 rrt.c = 1 rrt.max_nodes_in_ball = 30 rrt.set_start(start) rrt.init_search() rrt_int = RRT_Interactive(rrt,lqr_rrt.run_forward,plot_dims=[0,1],slider_range=(0,max_time_horizon)) def draw(rrt,ani_ax=None): if ani_ax is None: ani_ax = plt.figure().gca()
rrt.sample_goal = lambda : goal rrt.set_distance(lqr_rrt.distance_cache) rrt.set_same_state(lqr_rrt.same_state) rrt.set_cost(lqr_rrt.cost) rrt.set_steer(lqr_rrt.steer_cache) rrt.set_goal_test(goal_test) rrt.set_sample(sample) rrt.set_collision_free(lqr_rrt.collision_free) rrt.set_distance_from_goal(distance_from_goal) rrt.gamma_rrt = 1.0 rrt.eta = .2 rrt.c = 1 rrt.max_nodes_in_ball = 30 lqr_rrt.max_steer_cost = .015 rrt.set_start(start) rrt.init_search() def draw(rrt,ani_ax=None): if ani_ax is None: ani_ax = plt.figure().gca() ani_ax.cla() ani_ax.set_xlim(-10,110)
start = np.array([-1,-1])*1 rrt = RRT(state_ndim=2,control_ndim=2) rrt.set_same_state(same_state) rrt.set_distance(distance) rrt.set_cost(cost) rrt.set_steer(steer) rrt.set_goal_test(goal_test) rrt.set_sample(sample) rrt.set_collision_check(isStateValid) rrt.set_collision_free(collision_free) 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
rrt.sample_goal = lambda: goal rrt.set_distance(lqr_rrt.distance_cache) rrt.set_same_state(lqr_rrt.same_state) rrt.set_cost(lqr_rrt.cost) rrt.set_steer(lqr_rrt.steer_cache) rrt.set_goal_test(goal_test) rrt.set_sample(sample) #rrt.set_collision_check(isStateValid) rrt.set_collision_free(lqr_rrt.collision_free) rrt.set_distance_from_goal(distance_from_goal) rrt.gamma_rrt = 4.0 rrt.eta = 0.2 rrt.c = 1 rrt.set_start(start) rrt.init_search() def plot_tree(rrt): from mayavi import mlab tree = rrt.tree leafs = [i for i in tree.nodes() if len(tree.successors(i)) == 0] accounted = set() paths = [] for leaf in leafs:
goal = np.array([100.0,100.0,0]) rrt = RRT(state_ndim=3) rrt.set_distance(distance) rrt.set_cost(cost) rrt.set_steer(steer) rrt.set_goal_test(goal_test) 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'])
rrt.sample_goal = lambda : goal rrt.set_distance(lqr_rrt.distance_cache) rrt.set_same_state(lqr_rrt.same_state) rrt.set_cost(lqr_rrt.cost) rrt.set_steer(lqr_rrt.steer_cache) rrt.set_goal_test(goal_test) rrt.set_sample(sample) #rrt.set_collision_check(isStateValid) rrt.set_collision_free(lqr_rrt.collision_free) rrt.set_distance_from_goal(distance_from_goal) rrt.gamma_rrt = 4.0 rrt.eta = 0.2 rrt.c = 1 rrt.set_start(start) rrt.init_search() def plot_tree(rrt): from mayavi import mlab tree = rrt.tree leafs = [i for i in tree.nodes() if len(tree.successors(i)) == 0] accounted = set() paths = [] for leaf in leafs: this_node = leaf
start = np.array([-1, -1]) * 1 rrt = RRT(state_ndim=2, control_ndim=2) rrt.set_same_state(same_state) rrt.set_distance(distance) rrt.set_cost(cost) rrt.set_steer(steer) rrt.set_goal_test(goal_test) rrt.set_sample(sample) rrt.set_collision_check(isStateValid) rrt.set_collision_free(collision_free) 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