Loading...
Searching...
No Matches
PlannerData.py
1#!/usr/bin/env python
2
3
36
37# Author: Ryan Luna
38
39try:
40 # graph-tool and py-OMPL have some minor issues coexisting with each other. Both modules
41 # define conversions to C++ STL containers (i.e. std::vector), and the module that is imported
42 # first will have its conversions used. Order doesn't seem to matter on Linux,
43 # but on Apple, graph_tool will not be imported properly if OMPL comes first.
44 import graph_tool.all as gt
45 graphtool = True
46except ImportError:
47 print('Failed to import graph-tool. PlannerData will not be analyzed or plotted')
48 graphtool = False
49
50try:
51 from ompl import base as ob
52 from ompl import geometric as og
53except ImportError:
54 # if the ompl module is not in the PYTHONPATH assume it is installed in a
55 # subdirectory of the parent directory called "py-bindings."
56 from os.path import abspath, dirname, join
57 import sys
58 sys.path.insert(0, join(dirname(dirname(abspath(__file__))), 'py-bindings'))
59 from ompl import base as ob
60 from ompl import geometric as og
61
62
63# Create a narrow passage between y=[-3,3]. Only a 6x6x6 cube will be valid, centered at origin
64def isStateValid(state):
65 if state.getY() >= -3 and state.getY() <= 3:
66 return state.getX() >= -3 and state.getX() <= 3 and \
67 state.getZ() >= -3 and state.getZ() <= 3
68 return True
69
70def useGraphTool(pd):
71 # Extract the graphml representation of the planner data
72 graphml = pd.printGraphML()
73 f = open("graph.graphml", 'w')
74 f.write(graphml)
75 f.close()
76
77 # Load the graphml data using graph-tool
78 graph = gt.load_graph("graph.graphml", fmt="xml")
79 edgeweights = graph.edge_properties["weight"]
80
81 # Write some interesting statistics
82 avgdeg, stddevdeg = gt.vertex_average(graph, "total")
83 avgwt, stddevwt = gt.edge_average(graph, edgeweights)
84
85 print("---- PLANNER DATA STATISTICS ----")
86 print(str(graph.num_vertices()) + " vertices and " + str(graph.num_edges()) + " edges")
87 print("Average vertex degree (in+out) = " + str(avgdeg) + " St. Dev = " + str(stddevdeg))
88 print("Average edge weight = " + str(avgwt) + " St. Dev = " + str(stddevwt))
89
90 _, hist = gt.label_components(graph)
91 print("Strongly connected components: " + str(len(hist)))
92
93 # Make the graph undirected (for weak components, and a simpler drawing)
94 graph.set_directed(False)
95 _, hist = gt.label_components(graph)
96 print("Weakly connected components: " + str(len(hist)))
97
98 # Plotting the graph
99 gt.remove_parallel_edges(graph) # Removing any superfluous edges
100
101 edgeweights = graph.edge_properties["weight"]
102 colorprops = graph.new_vertex_property("string")
103 vertexsize = graph.new_vertex_property("double")
104
105 start = -1
106 goal = -1
107
108 for v in range(graph.num_vertices()):
109
110 # Color and size vertices by type: start, goal, other
111 if pd.isStartVertex(v):
112 start = v
113 colorprops[graph.vertex(v)] = "cyan"
114 vertexsize[graph.vertex(v)] = 10
115 elif pd.isGoalVertex(v):
116 goal = v
117 colorprops[graph.vertex(v)] = "green"
118 vertexsize[graph.vertex(v)] = 10
119 else:
120 colorprops[graph.vertex(v)] = "yellow"
121 vertexsize[graph.vertex(v)] = 5
122
123 # default edge color is black with size 0.5:
124 edgecolor = graph.new_edge_property("string")
125 edgesize = graph.new_edge_property("double")
126 for e in graph.edges():
127 edgecolor[e] = "black"
128 edgesize[e] = 0.5
129
130 # using A* to find shortest path in planner data
131 if start != -1 and goal != -1:
132 _, pred = gt.astar_search(graph, graph.vertex(start), edgeweights)
133
134 # Color edges along shortest path red with size 3.0
135 v = graph.vertex(goal)
136 while v != graph.vertex(start):
137 p = graph.vertex(pred[v])
138 for e in p.out_edges():
139 if e.target() == v:
140 edgecolor[e] = "red"
141 edgesize[e] = 2.0
142 v = p
143
144 # Writing graph to file:
145 # pos indicates the desired vertex positions, and pin=True says that we
146 # really REALLY want the vertices at those positions
147 gt.graph_draw(graph, vertex_size=vertexsize, vertex_fill_color=colorprops,
148 edge_pen_width=edgesize, edge_color=edgecolor,
149 output="graph.png")
150 print('\nGraph written to graph.png')
151
152def plan():
153 # construct the state space we are planning in
154 space = ob.SE3StateSpace()
155
156 # set the bounds for R^3 portion of SE(3)
157 bounds = ob.RealVectorBounds(3)
158 bounds.setLow(-10)
159 bounds.setHigh(10)
160 space.setBounds(bounds)
161
162 # define a simple setup class
163 ss = og.SimpleSetup(space)
164
165 # create a start state
166 start = ob.State(space)
167 start().setX(-9)
168 start().setY(-9)
169 start().setZ(-9)
170 start().rotation().setIdentity()
171
172 # create a goal state
173 goal = ob.State(space)
174 goal().setX(-9)
175 goal().setY(9)
176 goal().setZ(-9)
177 goal().rotation().setIdentity()
178
179 ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
180
181 # set the start and goal states
182 ss.setStartAndGoalStates(start, goal, 0.05)
183
184 # Lets use PRM. It will have interesting PlannerData
185 planner = og.PRM(ss.getSpaceInformation())
186 ss.setPlanner(planner)
187 ss.setup()
188
189 # attempt to solve the problem
190 solved = ss.solve(20.0)
191
192 if solved:
193 # print the path to screen
194 print("Found solution:\n%s" % ss.getSolutionPath())
195
196 # Extracting planner data from most recent solve attempt
197 pd = ob.PlannerData(ss.getSpaceInformation())
198 ss.getPlannerData(pd)
199
200 # Computing weights of all edges based on state space distance
201 pd.computeEdgeWeights()
202
203 if graphtool:
204 useGraphTool(pd)
205
206if __name__ == "__main__":
207 plan()
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:175
The lower and upper bounds for an Rn space.
A state space representing SE(3)
Definition: SE3StateSpace.h:50
Definition of an abstract state.
Definition: State.h:50
Probabilistic RoadMap planner.
Definition: PRM.h:81
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:63
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...