# DEALINGS IN THE SOFTWARE. # # PolygonPRM.py # Written Ian Rankin - October 2020 # # This example 2D PRM with obstcales and a non-rectangular environment # A* algorithm finding the shortest point between two points on the PRM. import rdml_graph as gr import numpy as np import matplotlib.pyplot as plt import shapely.geometry as geo bounding = geo.Polygon([(-10,25), (0, -35), (10, 25)]) obs = [geo.Polygon([(-5,5), (0, -5), (10, 2)])] map = {'bounding': bounding, 'obs': obs} G = gr.PRM( map=map, \ num_points=200,\ r= 10.0, \ sampleF=gr.sample2DPolygon, \ collision=gr.polygonCollision) path, cost = gr.AStar(G[0], goal = G[50]) # plot the geometric 2d graph gr.plot2DGeoGraph(G, 'green') gr.plot2DGeoPath(path, 'red') plt.show()
n.addEdge(gr.HEdge(n, n2, sign, features=features)) n2.addEdge(gr.HEdge(n2, n, sign, features=features)) n.addEdge(gr.HEdge(n, n3, sign, features=features)) n3.addEdge(gr.HEdge(n3, n, sign, features=features)) n2.addEdge(gr.HEdge(n2, n4, sign, features=features)) n4.addEdge(gr.HEdge(n4, n2, sign, features=features)) n1.addEdge(gr.HEdge(n1, n5, sign, features=features)) n5.addEdge(gr.HEdge(n5, n1, sign, features=features)) n3.addEdge(gr.HEdge(n3, n5, sign, features=features)) n5.addEdge(gr.HEdge(n5, n3, sign, features=features)) n.addEdge(gr.HEdge(n, n5, sign, features=features)) n5.addEdge(gr.HEdge(n5, n, sign, features=features)) gr.plot2DGeoGraph(G, 'blue') plt.title('Original') plt.show(block=False) pickle.dump(G, open("sample.g", "wb")) loaded = pickle.load(open("sample.g", "rb")) plt.figure() gr.plot2DGeoGraph(loaded, 'red') plt.title('Loaded graph') plt.show()