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()
