PlannerData.py
1 #!/usr/bin/env python
2 
3 
36 
37 # Author: Ryan Luna
38 
39 try:
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
46 except ImportError:
47  print('Failed to import graph-tool. PlannerData will not be analyzed or plotted')
48  graphtool = False
49 
50 try:
51  from ompl import base as ob
52  from ompl import geometric as og
53 except 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
64 def 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 
70 def 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 
152 def 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 
206 if __name__ == "__main__":
207  plan()
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
Create the set of classes typically needed to solve a geometric problem.
Definition: SimpleSetup.h:63
Definition of an abstract state.
Definition: State.h:49
The lower and upper bounds for an Rn space.
Probabilistic RoadMap planner.
Definition: PRM.h:80
A state space representing SE(3)
Definition: SE3StateSpace.h:49
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...