All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
PlannerData.py
1 #!/usr/bin/env DYLD_FALLBACK_LIBRARY_PATH=/opt/local/lib python
2 
3 ######################################################################
4 # Software License Agreement (BSD License)
5 #
6 # Copyright (c) 2012, Rice University
7 # All rights reserved.
8 #
9 # Redistribution and use in source and binary forms, with or without
10 # modification, are permitted provided that the following conditions
11 # are met:
12 #
13 # * Redistributions of source code must retain the above copyright
14 # notice, this list of conditions and the following disclaimer.
15 # * Redistributions in binary form must reproduce the above
16 # copyright notice, this list of conditions and the following
17 # disclaimer in the documentation and/or other materials provided
18 # with the distribution.
19 # * Neither the name of the Rice University nor the names of its
20 # contributors may be used to endorse or promote products derived
21 # from this software without specific prior written permission.
22 #
23 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 # POSSIBILITY OF SUCH DAMAGE.
35 ######################################################################
36 
37 # Author: Ryan Luna
38 
39 from math import sqrt
40 from functools import partial
41 
42 try:
43  # graph-tool and py-OMPL have some minor issues coexisting with each other. Both modules
44  # define conversions to C++ STL containers (i.e. std::vector), and the module that is imported
45  # first will have its conversions used. Order doesn't seem to matter on Linux,
46  # but on Apple, graph_tool will not be imported properly if OMPL comes first.
47  import graph_tool.all as gt
48  graphtool = True
49 except:
50  print 'Failed to import graph-tool. PlannerData will not be analyzed or plotted'
51  graphtool = False
52 
53 try:
54  from ompl import util as ou
55  from ompl import base as ob
56  from ompl import geometric as og
57 except:
58  # if the ompl module is not in the PYTHONPATH assume it is installed in a
59  # subdirectory of the parent directory called "py-bindings."
60  from os.path import basename, abspath, dirname, join
61  import sys
62  sys.path.insert(0, join(dirname(dirname(abspath(__file__))),'py-bindings'))
63  from ompl import util as ou
64  from ompl import base as ob
65  from ompl import geometric as og
66 
67 
68 # Create a narrow passage between y=[-3,3]. Only a 6x6x6 cube will be valid, centered at origin
69 def isStateValid(state):
70  if (state.getY() >= -3 and state.getY() <= 3):
71  if (state.getX() >= -3 and state.getX() <= 3 and state.getZ() >= -3 and state.getZ() <= 3):
72  return True
73  else:
74  return False
75  else:
76  return True
77 
78 # Returns the distance between the states contained in v1 and v2.
79 def edgeWeight(space, v1, v2, edge):
80  return space.distance(v1.getState(), v2.getState())
81 
82 def useGraphTool(pd, space):
83  # Extract the graphml representation of the planner data
84  graphml = pd.printGraphML()
85  f = open("graph.xml", 'w')
86  f.write(graphml)
87  f.close()
88 
89  # Load the graphml data using graph-tool
90  graph = gt.load_graph("graph.xml")
91  edgeweights = graph.edge_properties["weight"]
92 
93  # Write some interesting statistics
94  avgdeg, stddevdeg = gt.vertex_average(graph, "total")
95  avgwt, stddevwt = gt.edge_average(graph, edgeweights)
96 
97  print "---- PLANNER DATA STATISTICS ----"
98  print str(graph.num_vertices()) + " vertices and " + str(graph.num_edges()) + " edges"
99  print "Average vertex degree (in+out) = " + str(avgdeg) + " St. Dev = " + str(stddevdeg)
100  print "Average edge weight = " + str(avgwt) + " St. Dev = " + str(stddevwt)
101 
102  comps, hist = gt.label_components(graph)
103  print "Strongly connected components: " + str(len(hist))
104 
105  graph.set_directed(False) # Make the graph undirected (for weak components, and a simpler drawing)
106  comps, hist = gt.label_components(graph)
107  print "Weakly connected components: " + str(len(hist))
108 
109  # Plotting the graph
110  gt.remove_parallel_edges(graph) # Removing any superfluous edges
111 
112  edgeweights = graph.edge_properties["weight"]
113  colorprops = graph.new_vertex_property("string")
114  vertexsize = graph.new_vertex_property("double")
115 
116  start = -1
117  goal = -1
118 
119  for v in range(graph.num_vertices()):
120 
121  # Color and size vertices by type: start, goal, other
122  if (pd.isStartVertex(v)):
123  start = v
124  colorprops[graph.vertex(v)] = "cyan"
125  vertexsize[graph.vertex(v)] = 10
126  elif (pd.isGoalVertex(v)):
127  goal = v
128  colorprops[graph.vertex(v)] = "green"
129  vertexsize[graph.vertex(v)] = 10
130  else:
131  colorprops[graph.vertex(v)] = "yellow"
132  vertexsize[graph.vertex(v)] = 5
133 
134  # default edge color is black with size 0.5:
135  edgecolor = graph.new_edge_property("string")
136  edgesize = graph.new_edge_property("double")
137  for e in graph.edges():
138  edgecolor[e] = "black"
139  edgesize[e] = 0.5
140 
141  # using A* to find shortest path in planner data
142  if start != -1 and goal != -1:
143  dist, pred = gt.astar_search(graph, graph.vertex(start), edgeweights)
144 
145  # Color edges along shortest path red with size 3.0
146  v = graph.vertex(goal)
147  while v != graph.vertex(start):
148  p = graph.vertex(pred[v])
149  for e in p.out_edges():
150  if e.target() == v:
151  edgecolor[e] = "red"
152  edgesize[e] = 2.0
153  v = p
154 
155  # Writing graph to file:
156  # pos indicates the desired vertex positions, and pin=True says that we
157  # really REALLY want the vertices at those positions
158  gt.graph_draw (graph, vertex_size=vertexsize, vertex_fill_color=colorprops,
159  edge_pen_width=edgesize, edge_color=edgecolor,
160  output="graph.png")
161  print
162  print 'Graph written to graph.png'
163 
164 def plan():
165  # construct the state space we are planning in
166  space = ob.SE3StateSpace()
167 
168  # set the bounds for R^3 portion of SE(3)
169  bounds = ob.RealVectorBounds(3)
170  bounds.setLow(-10)
171  bounds.setHigh(10)
172  space.setBounds(bounds)
173 
174  # define a simple setup class
175  ss = og.SimpleSetup(space)
176 
177  # create a start state
178  start = ob.State(space)
179  start().setX(-9)
180  start().setY(-9)
181  start().setZ(-9)
182  start().rotation().setIdentity()
183 
184  # create a goal state
185  goal = ob.State(space)
186  goal().setX(-9)
187  goal().setY(9)
188  goal().setZ(-9)
189  goal().rotation().setIdentity()
190 
191  ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
192 
193  # set the start and goal states
194  ss.setStartAndGoalStates(start, goal, 0.05)
195 
196  # Lets use PRM. It will have interesting PlannerData
197  planner = og.PRM(ss.getSpaceInformation())
198  ss.setPlanner(planner)
199  ss.setup()
200 
201  # attempt to solve the problem
202  solved = ss.solve(20.0)
203 
204  if solved:
205  # print the path to screen
206  print("Found solution:\n%s" % ss.getSolutionPath())
207 
208  # Extracting planner data from most recent solve attempt
209  pd = ob.PlannerData(ss.getSpaceInformation())
210  ss.getPlannerData(pd)
211 
212  # Computing weights of all edges based on distance
213  pd.computeEdgeWeights(ob.EdgeWeightFn(partial(edgeWeight, space)))
214 
215  useGraphTool(pd, space)
216 
217 if __name__ == "__main__":
218  plan()