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