def plot_problem(self): plot_line_segments(self.obstacles, color="red", linewidth=2, label="obstacles") plt.scatter([self.x_init[0], self.x_goal[0]], [self.x_init[1], self.x_goal[1]], color="green", s=30, zorder=10) plt.annotate(r"$x_{init}$", self.x_init[:2] + [.2, 0], fontsize=16) plt.annotate(r"$x_{goal}$", self.x_goal[:2] + [.2, 0], fontsize=16) plt.legend(loc='upper center', bbox_to_anchor=(0.5, -0.03), fancybox=True, ncol=3) plt.axis('scaled')
def solve(self, eps, max_iters = 1000, goal_bias = 0.05): state_dim = len(self.x_init) # V stores the states that have been added to the RRT (pre-allocated at its maximum size # since numpy doesn't play that well with appending/extending) V = np.zeros((max_iters, state_dim)) V[0,:] = self.x_init # RRT is rooted at self.x_init n = 1 # the current size of the RRT (states accessible as V[range(n),:]) # P stores the parent of each state in the RRT. P[0] = -1 since the root has no parent, # P[1] = 0 since the parent of the first additional state added to the RRT must have been # extended from the root, in general 0 <= P[i] < i for all i < n P = -np.ones(max_iters, dtype=int) ## Intermediate Outputs # You must update and/or populate: # - V, P, n: the represention of the planning tree # - succcess: whether or not you've found a solution within max_iters RRT iterations # - solution_path: if success is True, then must contain list of states (tree nodes) # [x_init, ..., x_goal] such that the global trajectory made by linking steering # trajectories connecting the states in order is obstacle-free. #Iteration Limit K = max_iters p = goal_bias # TODO: fill me in! # fork=1 to K do for i in range(K): z = np.random.random() #if z<pthen if z < p: x_rand = x_goal else: #Get Random State and assign to x_rand x_rand = np.random.uniform(self.statespace_lo,self.statespace_hi) x_near = V[find_nearest(V,x_rand)] X_new = steer_towards(x_near,x_rand,eps) #if COLLISION FREE(xnear,xnew) then if self.is_free_motion(self.obstacles,x_near,x_new): #T .ADD VERTEX(xnew) np.vstack((V,x_new)) #T .ADD EDGE(xnear , xnew ) np.vstack((P,find_nearest(V,x_rand))) #if xnew = xgoal then if x_new == self.x_goal: #return T.PATH(xinit,xgoal) success = True path = [] for j in range(np.shape(V)[0]): path.append(V[j]) solution_path = (np.asarray(path)) plt.figure() plot_line_segments(self.obstacles, color="red", linewidth=2, label="obstacles") self.plot_tree(V, P, color="blue", linewidth=.5, label="RRT tree") if success: self.plot_path(solution_path, color="green", linewidth=2, label="solution path") plt.scatter(V[:n,0], V[:n,1]) plt.scatter([self.x_init[0], self.x_goal[0]], [self.x_init[1], self.x_goal[1]], color="green", s=30, zorder=10) plt.annotate(r"$x_{init}$", self.x_init[:2] + [.2, 0], fontsize=16) plt.annotate(r"$x_{goal}$", self.x_goal[:2] + [.2, 0], fontsize=16) plt.legend(loc='upper center', bbox_to_anchor=(0.5, -0.03), fancybox=True, ncol=3)
def solve(self, eps, max_iters = 1000, goal_bias = 0.05): state_dim = len(self.x_init) # V stores the states that have been added to the RRT (pre-allocated at its maximum size # since numpy doesn't play that well with appending/extending) V = np.zeros((max_iters, state_dim)) V[0,:] = self.x_init # RRT is rooted at self.x_init n = 1 # the current size of the RRT (states accessible as V[range(n),:]) # P stores the parent of each state in the RRT. P[0] = -1 since the root has no parent, # P[1] = 0 since the parent of the first additional state added to the RRT must have been # extended from the root, in general 0 <= P[i] < i for all i < n P = -np.ones(max_iters, dtype=int) ## Intermediate Outputs # You must update and/or populate: # - V, P, n: the represention of the planning tree # - succcess: whether or not you've found a solution within max_iters RRT iterations # - solution_path: if success is True, then must contain list of states (tree nodes) # [x_init, ..., x_goal] such that the global trajectory made by linking steering # trajectories connecting the states in order is obstacle-free. # TODO: fill me in! plt.figure() plot_line_segments(self.obstacles, color="red", linewidth=2, label="obstacles") self.plot_tree(V, P, color="blue", linewidth=.5, label="RRT tree") if success: self.plot_path(solution_path, color="green", linewidth=2, label="solution path") plt.scatter(V[:n,0], V[:n,1]) plt.scatter([self.x_init[0], self.x_goal[0]], [self.x_init[1], self.x_goal[1]], color="green", s=30, zorder=10) plt.annotate(r"$x_{init}$", self.x_init[:2] + [.2, 0], fontsize=16) plt.annotate(r"$x_{goal}$", self.x_goal[:2] + [.2, 0], fontsize=16) plt.legend(loc='upper center', bbox_to_anchor=(0.5, -0.03), fancybox=True, ncol=3)
def plot_tree(self, V, P, resolution = np.pi/24, **kwargs): line_segments = [] for i in range(V.shape[0]): if P[i] >= 0: pts = path_sample(V[P[i],:], V[i,:], self.turning_radius, self.turning_radius*resolution)[0] pts.append(V[i,:]) for j in range(len(pts) - 1): line_segments.append((pts[j], pts[j+1])) plot_line_segments(line_segments, **kwargs)
def plot_tree(self, point_size=15): plot_line_segments([(x, self.came_from[x]) for x in self.open_set if x != self.x_init], linewidth=1, color="blue", alpha=0.2) plot_line_segments([(x, self.came_from[x]) for x in self.closed_set if x != self.x_init], linewidth=1, color="blue", alpha=0.2) px = [x[0] for x in self.open_set | self.closed_set if x != self.x_init and x != self.x_goal] py = [x[1] for x in self.open_set | self.closed_set if x != self.x_init and x != self.x_goal] plt.scatter(px, py, color="blue", s=point_size, zorder=10, alpha=0.2)
def plot_tree(self, V, P, **kwargs): plot_line_segments([(V[P[i], :], V[i, :]) for i in range(V.shape[0]) if P[i] >= 0], **kwargs)
def solve(self, eps, max_iters=1000, goal_bias=0.05): state_dim = len(self.x_init) # V stores the states that have been added to the RRT (pre-allocated at its maximum size # since numpy doesn't play that well with appending/extending) V = np.zeros((max_iters, state_dim)) V[0, :] = self.x_init # RRT is rooted at self.x_init n = 1 # the current size of the RRT (states accessible as V[range(n),:]) # P stores the parent of each state in the RRT. P[0] = -1 since the root has no parent, # P[1] = 0 since the parent of the first additional state added to the RRT must have been # extended from the root, in general 0 <= P[i] < i for all i < n P = -np.ones(max_iters, dtype=int) ## Intermediate Outputs # You must update and/or populate: # - V, P, n: the represention of the planning tree # - succcess: whether or not you've found a solution within max_iters RRT iterations # - solution_path: if success is True, then must contain list of states (tree nodes) # [x_init, ..., x_goal] such that the global trajectory made by linking steering # trajectories connecting the states in order is obstacle-free. # TODO: fill me in! success = False i = 0 while not success and i <= max_iters: i = i + 1 z = np.random.uniform() if z < goal_bias: x_rand = self.x_goal else: x_rand = np.random.uniform(self.statespace_lo, self.statespace_hi) x_near = V[self.find_nearest(V[:n, :], x_rand), :] x_new = self.steer_towards(x_near, x_rand, eps) if self.is_free_motion(self.obstacles, x_near, x_new): V[n, :] = x_new # print V - x_near # print np.where(V[:n,:] == x_near) P[n] = np.where(V[:n, :] == x_near)[0][0] n = n + 1 if np.all(x_new == self.x_goal): success = True P = P[:n] V = V[:n, :] for i in range(len(P)): if i < P[i]: print "Error" # print P solution_path = [self.x_goal] # print "path" while np.all(solution_path[0] != self.x_init): idx = np.where(V == solution_path[0])[0][0] solution_path = [V[P[idx], :]] + solution_path C = 0 for i in range(1, len(solution_path)): C = C + np.linalg.norm(solution_path[i] - solution_path[i - 1]) print C # print type(solution_path) plt.figure() plot_line_segments(self.obstacles, color="red", linewidth=2, label="obstacles") self.plot_tree(V, P, color="blue", linewidth=.5, label="RRT tree") if success: self.plot_path(solution_path, color="green", linewidth=2, label="solution path") plt.scatter(V[:n, 0], V[:n, 1]) plt.scatter([self.x_init[0], self.x_goal[0]], [self.x_init[1], self.x_goal[1]], color="green", s=30, zorder=10) plt.annotate(r"$x_{init}$", self.x_init[:2] + [.2, 0], fontsize=16) plt.annotate(r"$x_{goal}$", self.x_goal[:2] + [.2, 0], fontsize=16) plt.legend(loc='upper center', bbox_to_anchor=(0.5, -0.03), fancybox=True, ncol=3)
def solve(self, eps, max_iters = 1000, goal_bias = 0.05): state_dim = len(self.x_init) # V stores the states that have been added to the RRT (pre-allocated at its maximum size # since numpy doesn't play that well with appending/extending) V = np.zeros((max_iters, state_dim)) V[0,:] = self.x_init # RRT is rooted at self.x_init n = 1 # the current size of the RRT (states accessible as V[range(n),:]) # P stores the parent of each state in the RRT. P[0] = -1 since the root has no parent, # P[1] = 0 since the parent of the first additional state added to the RRT must have been # extended from the root, in general 0 <= P[i] < i for all i < n P = -np.ones(max_iters, dtype=int) ## Intermediate Outputs # You must update and/or populate: # - V, P, n: the represention of the planning tree # - success: whether or not you've found a solution within max_iters RRT iterations # - solution_path: if success is True, then must contain list of states (tree nodes) # [x_init, ..., x_goal] such that the global trajectory made by linking steering # trajectories connecting the states in order is obstacle-free. success = False for k in range(max_iters): # Steer towards random state with probability (1 - goal_bias). z = np.random.random_sample() if z < goal_bias: x_rand = self.x_goal else: x_rand = np.array([np.random.uniform(lo, hi) for lo, hi in zip(self.statespace_lo, self.statespace_hi)]) idx_near = self.find_nearest(V[:n,:], x_rand) x_near = V[idx_near,:] x_new = self.steer_towards(x_near, x_rand, eps) # Add to RRT if path from x_near to x_new is collision-free. if self.is_free_motion(self.obstacles, x_near, x_new): V[n,:] = x_new P[n] = idx_near n = n + 1 # Reconstruct solution path when goal reached. if np.array_equal(x_new, self.x_goal): idx_cur = n - 1 solution_path = [self.x_goal] while not np.array_equal(V[idx_cur,:], self.x_init): solution_path.append(V[P[idx_cur],:]) idx_cur = P[idx_cur] solution_path = list(reversed(solution_path)) success = True break plt.figure() plot_line_segments(self.obstacles, color="red", linewidth=2, label="obstacles") self.plot_tree(V, P, color="blue", linewidth=.5, label="RRT tree") if success: self.plot_path(solution_path, color="green", linewidth=2, label="solution path") plt.scatter(V[:n,0], V[:n,1]) plt.scatter([self.x_init[0], self.x_goal[0]], [self.x_init[1], self.x_goal[1]], color="green", s=30, zorder=10) plt.annotate(r"$x_{init}$", self.x_init[:2] + [.2, 0], fontsize=16) plt.annotate(r"$x_{goal}$", self.x_goal[:2] + [.2, 0], fontsize=16) plt.legend(loc='upper center', bbox_to_anchor=(0.5, -0.03), fancybox=True, ncol=3)
def solve(self, eps, max_iters = 1000, goal_bias = 0.05): state_dim = len(self.x_init) # V stores the states that have been added to the RRT (pre-allocated at its maximum size # since numpy doesn't play that well with appending/extending) V = np.zeros((max_iters, state_dim)) V[0,:] = self.x_init # RRT is rooted at self.x_init n = 1 # the current size of the RRT (states accessible as V[range(n),:]) self.tree_size = 1 # TARIQ EDIT: self.tree_size is n. It's a self-variable so it can be accessed by methods # NOTE: self.tree_size is initialized in the _init_ of the class as well # P stores the parent of each state in the RRT. P[0] = -1 since the root has no parent, # P[1] = 0 since the parent of the first additional state added to the RRT must have been # extended from the root, in general 0 <= P[i] < i for all i < n P = -np.ones(max_iters, dtype=int) ## Intermediate Outputs # You must update and/or populate: # - V, P, n: the represention of the planning tree # - succcess: whether or not you've found a solution within max_iters RRT iterations # - solution_path: if success is True, then must contain list of states (tree nodes) # [x_init, ..., x_goal] such that the global trajectory made by linking steering # trajectories connecting the states in order is obstacle-free. # TODO: fill me in! solution_path = [] for k in range(1, max_iters): #sample a random point z = np.random.random() if(z <= goal_bias): x_rand = self.x_goal else : x_rand = np.zeros(state_dim) for i in range(state_dim): x_rand[i] = np.random.uniform(self.statespace_lo[i], self.statespace_hi[i],1) x_rand = np.array(x_rand) #find the nearest node from our tree near_index = self.find_nearest(V[:self.tree_size], x_rand) x_near = V[near_index, :] #ensure that the point we're steering towards is an eps #step away x_new = self.steer_towards(x_near, x_rand, eps) #if we can connect, connect! if self.is_free_motion(self.obstacles, x_near, x_new): #Add vertex V[self.tree_size, :] = x_new #Add edge P[self.tree_size] = near_index #increment the tree length n=n+1 #catching any legacy code; self.tree_size and n are the same value self.tree_size = self.tree_size+1 #if the new point is the goal, then we have a path! if np.linalg.norm(x_new - self.x_goal) <= eps: success = True #note: V and P are initialized with max_iters size #must keep track of tree size with "n" solution_path = self.reconstructPath(V[:self.tree_size],P[:self.tree_size]) break if(k == max_iters-1): print("Failed to find a path") success = False break plt.figure() plot_line_segments(self.obstacles, color="red", linewidth=2, label="obstacles") self.plot_tree(V, P, color="blue", linewidth=.5, label="RRT tree") if success: self.plot_path(solution_path, color="green", linewidth=2, label="solution path") plt.scatter(V[:n,0], V[:n,1]) plt.scatter([self.x_init[0], self.x_goal[0]], [self.x_init[1], self.x_goal[1]], color="green", s=30, zorder=10) plt.annotate(r"$x_{init}$", self.x_init[:2] + [.2, 0], fontsize=16) plt.annotate(r"$x_{goal}$", self.x_goal[:2] + [.2, 0], fontsize=16) plt.legend(loc='upper center', bbox_to_anchor=(0.5, -0.03), fancybox=True, ncol=3)
def solve(self, eps, max_iters=1000, goal_bias=0.05): state_dim = len(self.x_init) # V stores the states that have been added to the RRT (pre-allocated at its maximum size # since numpy doesn't play that well with appending/extending) V = np.zeros((max_iters, state_dim)) V[0, :] = self.x_init # RRT is rooted at self.x_init n = 1 # the current size of the RRT (states accessible as V[range(n),:]) # P stores the parent of each state in the RRT. P[0] = -1 since the root has no parent, # P[1] = 0 since the parent of the first additional state added to the RRT must have been # extended from the root, in general 0 <= P[i] < i for all i < n P = -np.ones(max_iters, dtype=int) ## Intermediate Outputs # You must update and/or populate: # - V, P, n: the represention of the planning tree # - succcess: whether or not you've found a solution within max_iters RRT iterations # - solution_path: if success is True, then must contain list of states (tree nodes) # [x_init, ..., x_goal] such that the global trajectory made by linking steering # trajectories connecting the states in order is obstacle-free. success = False solution_path = [] for k in range(max_iters): z = np.random.uniform(0.0, 1.0, 1) if z < goal_bias: x_rand = self.x_goal else: x_rand = np.zeros(state_dim) for i in range(state_dim): x_rand[i] = np.random.uniform(self.statespace_lo[i], self.statespace_hi[i], 1) x_rand = np.array(x_rand) near_idx = self.find_nearest(V[:n], x_rand) x_near = V[near_idx, :] x_new = self.steer_towards(x_near, x_rand, eps) if self.is_free_motion(self.obstacles, x_near, x_new): V[n, :] = x_new P[n] = near_idx n += 1 if np.linalg.norm(x_new[:2] - self.x_goal[:2]) <= 0.1: success = True # reconstruct path solution_path = self.reconstruct_path(V[:n], P[:n]) break plt.figure() plot_line_segments(self.obstacles, color="red", linewidth=2, label="obstacles") self.plot_tree(V, P, color="blue", linewidth=.5, label="RRT tree") if success: self.plot_path(solution_path, color="green", linewidth=2, label="solution path") plt.scatter(V[:n, 0], V[:n, 1]) plt.scatter([self.x_init[0], self.x_goal[0]], [self.x_init[1], self.x_goal[1]], color="green", s=30, zorder=10) plt.annotate(r"$x_{init}$", self.x_init[:2] + [.2, 0], fontsize=16) plt.annotate(r"$x_{goal}$", self.x_goal[:2] + [.2, 0], fontsize=16) plt.legend(loc='upper center', bbox_to_anchor=(0.5, -0.03), fancybox=True, ncol=3)
def solve(self, eps, max_iters=1000, goal_bias=0.05): state_dim = len(self.x_init) # V stores the states that have been added to the RRT (pre-allocated at its maximum size # since numpy doesn't play that well with appending/extending) V = np.zeros((max_iters, state_dim)) V[0, :] = self.x_init # RRT is rooted at self.x_init n = 1 # the current size of the RRT (states accessible as V[range(n),:]) # P stores the parent of each state in the RRT. P[0] = -1 since the root has no parent, # P[1] = 0 since the parent of the first additional state added to the RRT must have been # extended from the root, in general 0 <= P[i] < i for all i < n P = -np.ones(max_iters, dtype=int) ## Intermediate Outputs # You must update and/or populate: # - V, P, n: the represention of the planning tree # - succcess: whether or not you've found a solution within max_iters RRT iterations # - solution_path: if success is True, then must contain list of states (tree nodes) # [x_init, ..., x_goal] such that the global trajectory made by linking steering # trajectories connecting the states in order is obstacle-free. # TODO: fill me in! for i in range(1, max_iters): print 'iteration ', i # Randomly sample z z = np.random.random() # Bias the sample towards the goal state if z <= goal_bias: x_rand = self.x_goal else: x_rand = self.getRandomState() # Find the nearest point in the list of past points we've seen near_idx = self.find_nearest(V, x_rand) x_near = V[near_idx] # Move in the direction of the point by epsilon, unless it's closer than epsilon x_new = self.steer_towards(x_near, x_rand, eps) # Check whether new point is collision free if self.is_free_motion(self.obstacles, x_near, x_new): V[self.V_size, :] = np.asarray(x_new) P[self.V_size] = near_idx self.V_size += 1 if np.array_equal(x_new, self.x_goal): success = True solution_path = self.reconstructPath(V, P) break if i == max_iters - 1: print 'Was not able to find a path in ', max_iters, ' iterations' success = False break plt.figure() plot_line_segments(self.obstacles, color="red", linewidth=2, label="obstacles") self.plot_tree(V, P, color="blue", linewidth=.5, label="RRT tree") if success: self.plot_path(solution_path, color="green", linewidth=2, label="solution path") plt.scatter(V[:n, 0], V[:n, 1]) plt.scatter([self.x_init[0], self.x_goal[0]], [self.x_init[1], self.x_goal[1]], color="green", s=30, zorder=10) plt.annotate(r"$x_{init}$", self.x_init[:2] + [.2, 0], fontsize=16) plt.annotate(r"$x_{goal}$", self.x_goal[:2] + [.2, 0], fontsize=16) plt.legend(loc='upper center', bbox_to_anchor=(0.5, -0.03), fancybox=True, ncol=3)
def solve(self, eps, max_iters=1000, goal_bias=0.05): state_dim = len(self.x_init) # V stores the states that have been added to the RRT (pre-allocated at its maximum size # since numpy doesn't play that well with appending/extending) x_init = node(self.x_init) x_init.setP(None) x_init.setT(0) V = [x_init] n = 1 # the current size of the RRT (states accessible as V[range(n),:]) # P stores the parent of each state in the RRT. P[0] = -1 since the root has no parent, # P[1] = 0 since the parent of the first additional state added to the RRT must have been # extended from the root, in general 0 <= P[i] < i for all i < n ## Intermediate Outputs # You must update and/or populate: # - V, P, n: the represention of the planning tree # - succcess: whether or not you've found a solution within max_iters RRT iterations # - solution_path: if success is True, then must contain list of states (tree nodes) # [x_init, ..., x_goal] such that the global trajectory made by linking steering # trajectories connecting the states in order is obstacle-free. # TODO: fill me in! success = False i = 0 while not success and i <= max_iters: i = i + 1 z = np.random.uniform() if z < goal_bias: x_rand = self.x_goal else: x_rand = np.random.uniform(self.statespace_lo, self.statespace_hi) success, V = self.extend(V, x_rand, eps) # print P # print type(solution_path) plt.figure() plot_line_segments(self.obstacles, color="red", linewidth=2, label="obstacles") self.plot_tree(V, color="blue", linewidth=.5, label="RRT tree") nodes = np.zeros((self.n, 2)) for i in range(len(V)): nodes[i, :] = V[i].loc if np.all(V[i].loc == self.x_goal): goalnode = V[i] if success: solution_path_node = [goalnode] solution_path = [goalnode.loc] while np.all(solution_path[0] != self.x_init): parent = solution_path_node[0].parent solution_path_node = [parent] + solution_path solution_path = [parent.loc] + solution_path self.plot_path(solution_path, color="green", linewidth=2, label="solution path") print goalnode.T plt.scatter(nodes[:, 0], nodes[:, 1]) plt.scatter([self.x_init[0], self.x_goal[0]], [self.x_init[1], self.x_goal[1]], color="green", s=30, zorder=10) plt.annotate(r"$x_{init}$", self.x_init[:2] + [.2, 0], fontsize=16) plt.annotate(r"$x_{goal}$", self.x_goal[:2] + [.2, 0], fontsize=16) plt.legend(loc='upper center', bbox_to_anchor=(0.5, -0.03), fancybox=True, ncol=3)
def plot_tree(self, V, **kwargs): plot_line_segments([(V[i].parent.loc, V[i].loc) for i in range(len(V)) if V[i].parent != None], **kwargs)