def RIG_tree(d, B, X_all, X_free, epsilon, x_0, R, t_limit=0.1):
"""
:param d: single dynamic step
:param B: budget
:param X_all: workspace
:param X_free: free space
:param epsilon: environment
:param x_0: initial state
:param R: nearest neighbor radius
:param t_limit: time expansion can run for
:return: a list of nodes and edges
"""
# Initialize cost C, information I, starting node x_0, node list V, edge list E, and tree T
input_samples = cfg["samples"]
I_init = initial_information(x_0, epsilon) # Initial node information
C_init = 0 # Initial node cost
n_0 = Node(x_0, C_init, I_init, 0) # Initial node
V = [n_0] # Node list
V_closed = [] # Closed node list
E = [] # Edge list
# Sample configuration space of vehicle and find nearest node
t_0 = process_time()
while process_time() - t_0 < t_limit:
print(process_time() - t_0)
x_sample = sample(X_all)
n_nearest = nearest(x_sample, list(set(V).difference(V_closed)))
x_feasible = steer(n_nearest.get_position(), x_sample, d, input_samples, 'y.')
# find near points to be extended
# print("Full list: {}".format(V))
# print("Closed list: {}".format(V_closed))
# print("Open list: {}".format(list(set(V).difference(V_closed))))
n_near = near(x_feasible, list(set(V).difference(V_closed)), R)
for node in n_near:
# extend towards new point
x_new = steer(node.get_position(), x_feasible, d, input_samples)
if no_collision(node.get_position(), x_new, X_free):
I_new = information(node.get_information(), x_new, epsilon)
C_x_new = evaluate_cost(node.get_position(), x_new)
C_new = node.get_cost() + C_x_new
k_new = node.get_time() + 1 # represents one time step, could be actual time
n_new = Node(x_new, C_new, I_new, k_new)
if prune(n_new):
pass
else:
# add edge and node
E.append((node, n_new))
V.append(n_new)
if n_new.get_cost() > B:
V_closed.append(n_new)
if cfg["plot_full"]:
plot_tree(E)
plot_expansion(x_sample, x_feasible, node.get_position(), x_new)
return V, E
def RIG_tree(V, E, V_closed, X_all, X_free, epsilon, x_0, parameters):
"""
:param V: node list for a prebuilt tree
:param E: edge list for a prebuilt tree
:param V_closed: list of nodes that can't be expanded
:param X_all: workspace
:param X_free: free space
:param epsilon: environment
:param x_0: initial state
:param parameters: dictionary of neccessary values: step size, budget, nearest neighbor radius, input samples, and
time limit for expansion
:return: a list of nodes and edges
"""
# Initialize cost C, information I, starting node x_0, node list V, edge list E, and tree T
# np.random.seed(769) # 50 with lambda = 0 shows a weird error where agents decides to ignore a good path
d = parameters["step_size"]
B = parameters["budget"]
R = parameters["radius"]
input_samples = parameters["samples"]
t_limit = parameters["t_limit"]
if not V:
I_init = initial_information(x_0, epsilon) # Initial node information
C_init = 0 # Initial node cost
n_0 = Node(x_0, C_init, I_init, 0) # Initial node
V = [n_0] # Node list
V_closed = [] # Closed node list
E = [] # Edge list
# Sample configuration space of vehicle and find nearest node
t_0 = process_time()
while process_time() - t_0 < t_limit:
x_sample = sample(X_all, parameters)
n_nearest = nearest(x_sample, list(set(V).difference(V_closed)))
x_feasible = steer(n_nearest.get_position(), x_sample, d,
input_samples, 'y.')
n_near = near(x_feasible, list(set(V).difference(V_closed)), R)
I_best = 0
node_best = None
n_best = None
for node in n_near:
if node.get_time() >= parameters.get("budget"):
continue
# extend towards new point
x_new = steer(node.get_position(), x_feasible, d, input_samples)
if cfg["plot_full"]:
plot_tree(E)
plot_expansion(x_sample, x_feasible, node.get_position(),
x_new)
# node is in collision
if collision(node.get_position(), x_new, X_free):
continue
C_x_new = evaluate_cost(node.get_position(), x_new)
C_new = node.get_cost() + C_x_new
# node is not in range of home
# TODO: make this a cost instead of a cancel
if not in_range_of_home(parameters["home"], x_new, C_new,
parameters["budget"]):
continue
I_new = information(node.get_information(), x_new, epsilon)
# C_x_new = evaluate_cost(node.get_position(), x_new)
# C_new = node.get_cost() + C_x_new
k_new = node.get_time(
) + 1 # represents one time step, could be actual time
n_new = Node(x_new, C_new, I_new, k_new)
# node is not as good as another option
if n_new.get_information() < I_best:
continue
node_best = node
n_best = n_new
if node_best and n_best:
E.append((node_best, n_best))
V.append(n_best)
if n_best.get_cost() > B:
V_closed.append(n_best)
return V, E, V_closed