All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
demos/PlannerData.py
00001 #!/usr/bin/env DYLD_FALLBACK_LIBRARY_PATH=/opt/local/lib 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 basename, 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.xml", 'w')
00086     f.write(graphml)
00087     f.close()
00088 
00089     # Load the graphml data using graph-tool
00090     graph = gt.load_graph("graph.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
00162     print 'Graph written to graph.png'
00163 
00164 def plan():
00165     # construct the state space we are planning in
00166     space = ob.SE3StateSpace()
00167 
00168     # set the bounds for R^3 portion of SE(3)
00169     bounds = ob.RealVectorBounds(3)
00170     bounds.setLow(-10)
00171     bounds.setHigh(10)
00172     space.setBounds(bounds)
00173 
00174     # define a simple setup class
00175     ss = og.SimpleSetup(space)
00176 
00177     # create a start state
00178     start = ob.State(space)
00179     start().setX(-9)
00180     start().setY(-9)
00181     start().setZ(-9)
00182     start().rotation().setIdentity()
00183 
00184     # create a goal state
00185     goal = ob.State(space)
00186     goal().setX(-9)
00187     goal().setY(9)
00188     goal().setZ(-9)
00189     goal().rotation().setIdentity()
00190 
00191     ss.setStateValidityChecker(ob.StateValidityCheckerFn(isStateValid))
00192 
00193     # set the start and goal states
00194     ss.setStartAndGoalStates(start, goal, 0.05)
00195 
00196     # Lets use PRM.  It will have interesting PlannerData
00197     planner = og.PRM(ss.getSpaceInformation())
00198     ss.setPlanner(planner)
00199     ss.setup()
00200 
00201     # attempt to solve the problem
00202     solved = ss.solve(20.0)
00203 
00204     if solved:
00205         # print the path to screen
00206         print("Found solution:\n%s" % ss.getSolutionPath())
00207 
00208         # Extracting planner data from most recent solve attempt
00209         pd = ob.PlannerData(ss.getSpaceInformation())
00210         ss.getPlannerData(pd)
00211 
00212         # Computing weights of all edges based on distance
00213         pd.computeEdgeWeights(ob.EdgeWeightFn(partial(edgeWeight, space)))
00214 
00215         useGraphTool(pd, space)
00216 
00217 if __name__ == "__main__":
00218     plan()
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines