demos/PlannerData.py
00001 #!/usr/bin/env python 00002 00003 ###################################################################### 00004 # Software License Agreement (BSD License) 00005 # 00006 # Copyright (c) 2012, Rice University 00007 # All rights reserved. 00008 # 00009 # Redistribution and use in source and binary forms, with or without 00010 # modification, are permitted provided that the following conditions 00011 # are met: 00012 # 00013 # * Redistributions of source code must retain the above copyright 00014 # notice, this list of conditions and the following disclaimer. 00015 # * Redistributions in binary form must reproduce the above 00016 # copyright notice, this list of conditions and the following 00017 # disclaimer in the documentation and/or other materials provided 00018 # with the distribution. 00019 # * Neither the name of the Rice University nor the names of its 00020 # contributors may be used to endorse or promote products derived 00021 # from this software without specific prior written permission. 00022 # 00023 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00024 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00025 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00026 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00027 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00028 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00029 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00031 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00032 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00033 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00034 # POSSIBILITY OF SUCH DAMAGE. 00035 ###################################################################### 00036 00037 # Author: Ryan Luna 00038 00039 from math import sqrt 00040 from functools import partial 00041 00042 try: 00043 # graph-tool and py-OMPL have some minor issues coexisting with each other. Both modules 00044 # define conversions to C++ STL containers (i.e. std::vector), and the module that is imported 00045 # first will have its conversions used. Order doesn't seem to matter on Linux, 00046 # but on Apple, graph_tool will not be imported properly if OMPL comes first. 00047 import graph_tool.all as gt 00048 graphtool = True 00049 except: 00050 print('Failed to import graph-tool. PlannerData will not be analyzed or plotted') 00051 graphtool = False 00052 00053 try: 00054 from ompl import util as ou 00055 from ompl import base as ob 00056 from ompl import geometric as og 00057 except: 00058 # if the ompl module is not in the PYTHONPATH assume it is installed in a 00059 # subdirectory of the parent directory called "py-bindings." 00060 from os.path import abspath, dirname, join 00061 import sys 00062 sys.path.insert(0, join(dirname(dirname(abspath(__file__))),'py-bindings')) 00063 from ompl import util as ou 00064 from ompl import base as ob 00065 from ompl import geometric as og 00066 00067 00068 # Create a narrow passage between y=[-3,3]. Only a 6x6x6 cube will be valid, centered at origin 00069 def isStateValid(state): 00070 if (state.getY() >= -3 and state.getY() <= 3): 00071 if (state.getX() >= -3 and state.getX() <= 3 and state.getZ() >= -3 and state.getZ() <= 3): 00072 return True 00073 else: 00074 return False 00075 else: 00076 return True 00077 00078 # Returns the distance between the states contained in v1 and v2. 00079 def edgeWeight(space, v1, v2, edge): 00080 return space.distance(v1.getState(), v2.getState()) 00081 00082 def useGraphTool(pd, space): 00083 # Extract the graphml representation of the planner data 00084 graphml = pd.printGraphML() 00085 f = open("graph.graphml", 'w') 00086 f.write(graphml) 00087 f.close() 00088 00089 # Load the graphml data using graph-tool 00090 graph = gt.load_graph("graph.graphml", fmt="xml") 00091 edgeweights = graph.edge_properties["weight"] 00092 00093 # Write some interesting statistics 00094 avgdeg, stddevdeg = gt.vertex_average(graph, "total") 00095 avgwt, stddevwt = gt.edge_average(graph, edgeweights) 00096 00097 print("---- PLANNER DATA STATISTICS ----") 00098 print(str(graph.num_vertices()) + " vertices and " + str(graph.num_edges()) + " edges") 00099 print("Average vertex degree (in+out) = " + str(avgdeg) + " St. Dev = " + str(stddevdeg)) 00100 print("Average edge weight = " + str(avgwt) + " St. Dev = " + str(stddevwt)) 00101 00102 comps, hist = gt.label_components(graph) 00103 print("Strongly connected components: " + str(len(hist))) 00104 00105 graph.set_directed(False) # Make the graph undirected (for weak components, and a simpler drawing) 00106 comps, hist = gt.label_components(graph) 00107 print("Weakly connected components: " + str(len(hist))) 00108 00109 # Plotting the graph 00110 gt.remove_parallel_edges(graph) # Removing any superfluous edges 00111 00112 edgeweights = graph.edge_properties["weight"] 00113 colorprops = graph.new_vertex_property("string") 00114 vertexsize = graph.new_vertex_property("double") 00115 00116 start = -1 00117 goal = -1 00118 00119 for v in range(graph.num_vertices()): 00120 00121 # Color and size vertices by type: start, goal, other 00122 if (pd.isStartVertex(v)): 00123 start = v 00124 colorprops[graph.vertex(v)] = "cyan" 00125 vertexsize[graph.vertex(v)] = 10 00126 elif (pd.isGoalVertex(v)): 00127 goal = v 00128 colorprops[graph.vertex(v)] = "green" 00129 vertexsize[graph.vertex(v)] = 10 00130 else: 00131 colorprops[graph.vertex(v)] = "yellow" 00132 vertexsize[graph.vertex(v)] = 5 00133 00134 # default edge color is black with size 0.5: 00135 edgecolor = graph.new_edge_property("string") 00136 edgesize = graph.new_edge_property("double") 00137 for e in graph.edges(): 00138 edgecolor[e] = "black" 00139 edgesize[e] = 0.5 00140 00141 # using A* to find shortest path in planner data 00142 if start != -1 and goal != -1: 00143 dist, pred = gt.astar_search(graph, graph.vertex(start), edgeweights) 00144 00145 # Color edges along shortest path red with size 3.0 00146 v = graph.vertex(goal) 00147 while v != graph.vertex(start): 00148 p = graph.vertex(pred[v]) 00149 for e in p.out_edges(): 00150 if e.target() == v: 00151 edgecolor[e] = "red" 00152 edgesize[e] = 2.0 00153 v = p 00154 00155 # Writing graph to file: 00156 # pos indicates the desired vertex positions, and pin=True says that we 00157 # really REALLY want the vertices at those positions 00158 gt.graph_draw (graph, vertex_size=vertexsize, vertex_fill_color=colorprops, 00159 edge_pen_width=edgesize, edge_color=edgecolor, 00160 output="graph.png") 00161 print('\nGraph written to graph.png') 00162 00163 def plan(): 00164 # construct the state space we are planning in 00165 space = ob.SE3StateSpace() 00166 00167 # set the bounds for R^3 portion of SE(3) 00168 bounds = ob.RealVectorBounds(3) 00169 bounds.setLow(-10) 00170 bounds.setHigh(10) 00171 space.setBounds(bounds) 00172 00173 # define a simple setup class 00174 ss = og.SimpleSetup(space) 00175 00176 # create a start state 00177 start = ob.State(space) 00178 start().setX(-9) 00179 start().setY(-9) 00180 start().setZ(-9) 00181 start().rotation().setIdentity() 00182 00183 # create a goal state 00184 goal = ob.State(space) 00185 goal().setX(-9) 00186 goal().setY(9) 00187 goal().setZ(-9) 00188 goal().rotation().setIdentity() 00189 00190 ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid)) 00191 00192 # set the start and goal states 00193 ss.setStartAndGoalStates(start, goal, 0.05) 00194 00195 # Lets use PRM. It will have interesting PlannerData 00196 planner = og.PRM(ss.getSpaceInformation()) 00197 ss.setPlanner(planner) 00198 ss.setup() 00199 00200 # attempt to solve the problem 00201 solved = ss.solve(20.0) 00202 00203 if solved: 00204 # print the path to screen 00205 print("Found solution:\n%s" % ss.getSolutionPath()) 00206 00207 # Extracting planner data from most recent solve attempt 00208 pd = ob.PlannerData(ss.getSpaceInformation()) 00209 ss.getPlannerData(pd) 00210 00211 # Computing weights of all edges based on state space distance 00212 pd.computeEdgeWeights() 00213 00214 if graphtool: 00215 useGraphTool(pd, space) 00216 00217 if __name__ == "__main__": 00218 plan()