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