def sample(c_max,c_min,x_center,C): global previous_diff if c_max < float('inf'): previous_diff_temp = c_max**2 - c_min**2 if previous_diff_temp < 0: previous_diff_temp = previous_diff previous_diff = previous_diff_temp r = [c_max /2.0, np.sqrt(previous_diff)/2.0, np.sqrt(previous_diff)/2.0] L = np.diag(r) # The transformation matrix x_ball = sample_unit_ball() # The random point gets sampled from the heuristic space (ellipsoid) random_point = np.dot(np.dot(C,L), x_ball) + x_center random_point = Node(random_point[(0,0)], random_point[(1,0)]) return random_point random_point = Node(np.random.uniform(0,10), np.random.uniform(0,10)) return random_point
def RRT(x_init, K, step_size, obstacles, goal_area): V = [x_init] T = [x_init.coordinates] E = [] path_found = [] goal_found = False for k in range(K): # sample from the ellipse or a uniform distribution x_rand = Node(np.random.uniform(0, 10), np.random.uniform(0, 10)) x_nearest = kd_tree(x_rand, V, T) path_new = select_input(x_rand, x_nearest, step_size, partition=True) # Check if there is a collision if (not collision_rectangle(path_new, obstacles)): x_new = path_new[-1] V.append(x_new) T.append(x_new.coordinates) x_new.parent = x_nearest x_new.cost = x_nearest.cost + line(x_nearest, x_new) E.append((x_nearest, x_new)) # These if statements are used for the goal area and cost plotting if collision_rectangle([x_new], goal_area) and not goal_found: goal_found = True elif goal_found: path_cost = least_cost(V, goal_area)[1] cost_solutions.append(path_cost) else: cost_solutions.append(999) else: cost_solutions.append(cost_solutions[-1]) return (V, E, path_found)
def start_generate(step_size,max_nodes,obstacles,origin=Node(5,5),goal=((8,8),0.5,0.5)): print("\n[======] - Starting Informed RRT* generation - [======]\n") genStart = time.time() Informed = informed_RRT_star(origin, max_nodes, step_size,obstacles,goal_area=goal) genEnd = time.time() path_to_goal,cost = least_cost(Informed[0],goal) INFORMED = (Informed[0],Informed[1],path_to_goal) print("Amount of nodes Expanded: ", len(Informed[0])) if cost == 999: print("No path to the goal was found") else: print("Best path cost to the goal: ", cost) print("\n[======] - Informed RRT* Generation Complete...[======] - ") print("\nElapsed time for Informed RRT* Generation: ", genEnd - genStart) print("") # Plot the vertices, edges, obstacles and the goal plot(INFORMED,goal,obstacles,'#00A241') print("")
def start_generate(step_size, max_nodes, obstacles, origin=Node(5, 5), goal=((8, 8), 0.5, 0.5)): print("\n[======] - Starting RRT generation - [======]\n") genStart = time.time() RRt = RRT(origin, max_nodes, step_size, obstacles, goal_area=goal) genEnd = time.time() path_to_goal, cost = least_cost(RRt[0], goal) RRt = (RRt[0], RRt[1], path_to_goal) print("Amount of nodes Expanded: ", len(RRt[0])) if cost == 999: print("No path to the goal was found") else: print("Best path cost to the goal: ", cost) print("\n[======] - RRT Generation Complete...[======] - ") print("\nElapsed time for RRT Generation: ", genEnd - genStart) print("") # Plot the vertices, edges, obstacles and the goal plot(RRt, goal, obstacles, 'black') print("")
def informed_RRT_star(x_init, K, step_size,obstacles,goal_area): V = [x_init] T = [x_init.coordinates] E = [] path_length = float('inf') c_best = float('inf') path_found = [] # The Initialisation of the Ellipse given the theory in the paper # For Informed RRT* #------------------ X_sol = [] goal_center = Node(goal_area[0][0][0]+0.2,goal_area[0][0][1]+0.2) # Euclidean distance to the goal c_min = line(goal_center,x_init) # the centre of the ellipse x_center = np.matrix([[(x_init.x + goal_center.x) / 2.0],[(x_init.y + goal_center.y) / 2.0],[0]]) a_1 = np.matrix([[(goal_center.x - x_init.x) / c_min],[(goal_center.y - x_init.y) / c_min],[0]]) # first row of the identity matrix id1_t = np.matrix([1.0,0,0]) M = np.dot(a_1, id1_t) # Singular Value Decomposition, S denotes here the Sigma matrix U,S,Vh = np.linalg.svd(M, 1,1) # The Rotation Matrix C = np.dot(np.dot(U, np.diag([1.0,1.0, np.linalg.det(U) * np.linalg.det(np.transpose(Vh))])), Vh) #------------------ goal_found = False for k in range(K): # sample from the ellipse or a uniform distribution x_rand = sample(c_best,c_min,x_center,C) x_nearest = kd_tree(x_rand,V,T) path_new = select_input(x_rand,x_nearest,step_size,partition=True) # Check if there is a collision if (not collision_rectangle(path_new,obstacles)): x_new = path_new[-1] V.append(x_new) T.append(x_new.coordinates) x_new.parent = x_nearest x_new.cost = x_nearest.cost + line(x_nearest,x_new) # nearest neighbours X_near = near(V,x_new,T,len(V)) # Choose a parent from the nearest neighbours x_nearest = choose_parent(X_near,x_nearest,x_new,step_size,obstacles) x_new.parent = x_nearest x_new.cost = x_nearest.cost + line(x_nearest,x_new) E.append((x_nearest,x_new)) # Check the tree and rewire if there are improving edges E = rewire(X_near,E,x_new,step_size,obstacles) # goal check for Informed RRT* if collision_rectangle([x_new],goal_area): X_sol.append(x_new) path_to_goal = trace_path(x_new,[]) if x_new.cost < path_length: path_length = x_new.cost path_found = path_to_goal c_best = x_new.cost # These if statements are for the goal area and cost plotting if collision_rectangle([x_new],goal_area) and not goal_found: goal_found = True elif goal_found: path_cost = least_cost(V,goal_area)[1] c_best = path_cost cost_solutions.append(path_cost) else: cost_solutions.append(999) else: cost_solutions.append(cost_solutions[-1]) return (V,E,path_found)
# Plot the vertices, edges, obstacles and the goal plot(INFORMED,goal,obstacles,'#00A241') print("") ''' The Informed RRT* is run with epsilon: 0.15, K = 2300 and with a set of 13 obstacles, 5 of which are generated randomly The initial node originates at (5.5,1.0) and the goal is a square in the upper right corner ''' cost_solutions = [999] K = 1000 obs = 5 # Three chosen obstacles rect = [((2,2),2.5,1),((5,2),2,1),((7.1,3),1,4)] # Introduce random obstacles for i in range(obs): rect.append(((np.random.uniform(1,6),np.random.uniform(3.2,10)), np.random.uniform(0.25,1.8),np.random.uniform(0.25,2))) x_goal = [((6.4,3.5),0.5,0.5)] x_init = Node(5.5,1) # epsilon = 0.15, K = 1000, change these parameters for different tree start_generate(0.15,K,rect,x_init,goal=x_goal) costs = np.asarray(cost_solutions) # plot the cost of the best path found plot_cost(costs,(4,7),K)