def test22(): obstacles = [Polygon([(2,0), (8, 0), (8, 1), (2, 1)]), Polygon([(2,10), (8,10), (8,8), (2,8)]), Polygon([(2,1), (2,8), (3,8), (3,1)])] workspace = Workspace(dim=(12,12), obstacles=obstacles) car_rrt = RRT(workspace, (1, 6, pi), (6, 6, pi), 10000, num_rand_actions=6, max_control_time=1, step_threshold=.05) results = car_rrt.build_rrt() print(results) anim = workspace.animate_results(results, car_rrt, plot_tree=True) plt.show()
def test6(): obstacles = [Polygon([(2,0), (2, 2), (3, 2), (3, 0)]), Polygon([(4,4), (4,8), (7,1)]), Polygon([(2,2), (1,8), (2,4), (3,3)])] workspace = Workspace(dim=(12,12), obstacles=obstacles) car_rrt = RRT(workspace, (1, 1, pi), (10, 10, pi), 10000, num_rand_actions=4, max_control_time=1, step_threshold=.05) results = car_rrt.build_rrt() print(results) ax = workspace.graph_results(results, car_rrt, plot_tree=False) plt.show()
def rrt_local(self, pe, start, end): dims = len(start.vertex) lims = [] for i in range(dims): if start.vertex[i] < end.vertex[i]: min = start.vertex[i] - 20 max = end.vertex[i] + 20 else: max = start.vertex[i] + 20 min = end.vertex[i] - 20 lims.append([min,max]) rrt = RRT(50, dims, self.epsilon, lims=np.array(lims), connect_prob=.15, collision_func=pe.test_collisions) plan = rrt.build_rrt(start.vertex, end.vertex, pe.robot) return plan
def is_collision(self, state): #-- Casts as a shapely point state_point = Point(state[0], state[1]) if self.obstacles is None: return False else: #-- For each obstacle checks if the point is contained in the obstacle. for obstacle in self.obstacles: if obstacle.contains(state_point): return True return False def graph_results(self, results, car_rrt, plot_tree=False): return graph_results(self, results, car_rrt, plot_tree) def animate_results(self, results, car_rrt, plot_tree=False): return animate_results(self, results, car_rrt, plot_tree) if __name__ == '__main__': random.seed(446) obstacles = [Polygon([(2,0), (2, 2), (3, 2), (3, 0)]), Polygon([(4,4), (4,8), (7,1)]), Polygon([(2,2), (1,8), (2,4), (3,3)])] workspace = Workspace(dim=(12,12), obstacles=obstacles) car_rrt = RRT(workspace, (1, 1, pi), (10, 10, pi), 10000, num_rand_actions=6, max_control_time=2, step_threshold=.05) results = car_rrt.build_rrt() print(results) ax = workspace.graph_results(results, car_rrt, plot_tree=False) plt.show()