def generate_full_path(self, steps):
"""
Creates a list of nodes associated with each time step
:return:
"""
path = [] # place holder, value will be removed
current_step = self.path_log[-1].get_time()
if not self.waypoints:
# No waypoints provided, robot will just sit still
pos = self.state.get_position()
i = self.get_information_available(pos)
self.waypoints.append(pos)
path.append(Node.init_without_cost(pos, i, current_step))
elif self.state.get_position() != self.waypoints[0]:
# The first state is not the first waypoint, add it in
self.waypoints.insert(0, self.state.get_position())
elif len(self.waypoints) == 1:
pos = self.waypoints[0]
i = self.get_information_available(pos)
path.append(Node.init_without_cost(pos, i, current_step))
for point in range(0, len(self.waypoints) - 1):
temp_path = self.generate_straight_line_path(point, current_step)
temp_path.reverse()
temp_path.pop()
temp_path.extend(path)
path = temp_path
current_step = path[0].get_time()
# temp_path.reverse()
# self.path_log.append(temp_path.pop())
if steps and len(path) < steps:
add_n = steps - len(path)
temp_path = self.generate_stationary_path(path[0], add_n)
temp_path.extend(path)
path = temp_path
self.path = path
def generate_stationary_path(self, node, steps):
k = node.get_time()
pos = node.get_position()
temp_path = []
for _ in range(0, steps):
k += 1
i = self.get_information_available(pos)
temp_path.append(Node.init_without_cost(pos, i, k))
return temp_path
def start(self, workspace):
"""
Set initial conditions for the robot in a specified workspace
:param workspace: Workspace object that describes the environment the agent is working in
:return:
"""
self.set_workspace(workspace)
self.initialize_pdf()
I_0 = self.get_information_available(self.state)
start_node = Node.init_without_cost(self.state.get_position(), I_0, 0)
self.path_log.append(start_node)
self.state_log.append(self.state)
def start(self, workspace):
"""
Set initial conditions for the robot in a specified workspace
:param workspace: Workspace object that describes the environment the agent is working in
:return:
"""
self.set_workspace(workspace)
# self.set_map()
self.set_initial_pdf()
self.update_pdf()
current_step = self.workspace.get_time_step()
i = self.get_information_available(self.state)
start_node = Node.init_without_cost(self.state.get_position(), i, current_step)
self.path_log.append(start_node)
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 generate_straight_line_path(self, w_0, k=0):
"""
Could use any type of path planner here. This is just a straight line style implementation
Assume waypoint 1 is the starting location
:return:
"""
start = self.waypoints[w_0]
goal = self.waypoints[w_0 + 1]
# --- Straight line assumption ---
i = self.get_information_available(start)
path = [Node.init_without_cost(start, i, k)]
x = start[0]
y = start[1]
goal_found = False
# North
if y < goal[1]:
while not goal_found:
y += self.cfg["step_size"]
k += 1
i = self.get_information_available((x, y))
path.append(Node.init_without_cost((x, y), i, k))
if (x, y) == goal:
goal_found = True
# East
elif x < goal[0]:
while not goal_found:
x += self.cfg["step_size"]
k += 1
i = self.get_information_available((x, y))
path.append(Node.init_without_cost((x, y), i, k))
if (x, y) == goal:
goal_found = True
# South
elif y > goal[1]:
while not goal_found:
y -= self.cfg["step_size"]
k += 1
i = self.get_information_available((x, y))
path.append(Node.init_without_cost((x, y), i, k))
if (x, y) == goal:
goal_found = True
# West
elif x > goal[0]:
while not goal_found:
x -= self.cfg["step_size"]
k += 1
i = self.get_information_available((x, y))
path.append(Node.init_without_cost((x, y), i, k))
if (x, y) == goal:
goal_found = True
# Error
else:
print("ERROR: Something's wrong with the start or goal position for {}".format(self.name))
return path
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