def main():
pl.ion()
np.random.seed(4)
env = environment_2d.Environment(10, 6, 5)
pl.clf()
env.plot()
q = env.random_query()
if q is not None:
x_start, y_start, x_goal, y_goal = q
env.plot_query(x_start, y_start, x_goal, y_goal)
graph = PRM(env, NUM_NODES, MAX_NEIGHBOURS, RADIUS)
graph.random_sampling()
graph.find_closest_neighbours()
path = graph.solve_query()
graph.plot_nodes()
graph.plot_edges()
graph.plot_path(path)
def main():
pl.ion()
np.random.seed(4)
env = environment_2d.Environment(10, 6, 5)
pl.clf()
env.plot()
q = env.random_query()
if q is not None:
x_start, y_start, x_goal, y_goal = q
env.plot_query(x_start, y_start, x_goal, y_goal)
start = Treenode(env.start[0], env.start[1])
goal = Treenode(env.goal[0], env.goal[1])
start_tree = RRT(env, start, MAX_RADIUS)
goal_tree = RRT(env, goal, MAX_RADIUS)
midpoint = start_tree.bidirectional_RRT(goal_tree, MAXREP)
start_tree.plot_tree()
goal_tree.plot_tree()
if midpoint is not None:
goal_tree.plot_path(midpoint)
start_tree.plot_path(midpoint)
self.graph = nx.Graph()
for node in nodes:
distances, indexes = self.kdtree.query(node, k=self.N_KNN)
for distance, index in zip(distances, indexes):
neighbor = self.kdtree.data[index]
if self._local_planner(node, neighbor):
self.graph.add_edge(tuple(node), tuple(neighbor), weight=distance)
if __name__ == '__main__':
pl.ion()
pl.clf()
# np.random.seed(4)
# Initialize environment
env = environment_2d.Environment(10, 6, 5)
env.plot()
# Build probabilistic road map
prm = ProbablisticRoadmap(env)
# Plot the nodes in graph
nodes = prm.graph.nodes
pl.scatter([node[0] for node in nodes], [node[1] for node in nodes], s=3)
# Run multiple queries
for i in range(2):
q = env.random_query()
env.plot_query(*q)
if q is None: exit(0)
x_start, y_start, x_goal, y_goal = q
import time
start_time = time.time()
# import libraries and add path to dependancies
import numpy as np
import pylab as pl
import sys
sys.path.append('osr_examples/scripts/')
import environment_2d
# generate 2D environment, obstacles, goal and start
pl.ion() # interactive mode on
np.random.seed(
4
) # seed the generator(same seed number(int64) will always produce same environment)
env = environment_2d.Environment(
10, 6, 5
) # create a 10x6 environment with 5 triangular obstacles. If this is changed, arguments to vertex generator in function main() should also be changed
pl.clf() # what does this do? irrelevant.
env.plot() # plot the environment
q = env.random_query() # assigns start and end positions (randomly?)
if q is not None:
x_start, y_start, x_goal, y_goal = q
env.plot_query(x_start, y_start, x_goal,
y_goal) # plots start and goal positions
# create a vertex class with coordinates, a function to print coordinates and the set of closest vertices
class vertex():
def __init__(self, x, y):
self.x = x
self.y = y