00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 from math import sqrt
00040 from functools import partial
00041
00042 try:
00043
00044
00045
00046
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
00059
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
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
00079 def edgeWeight(space, v1, v2, edge):
00080 return space.distance(v1.getState(), v2.getState())
00081
00082 def useGraphTool(pd, space):
00083
00084 graphml = pd.printGraphML()
00085 f = open("graph.xml", 'w')
00086 f.write(graphml)
00087 f.close()
00088
00089
00090 graph = gt.load_graph("graph.xml")
00091 edgeweights = graph.edge_properties["weight"]
00092
00093
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)
00106 comps, hist = gt.label_components(graph)
00107 print "Weakly connected components: " + str(len(hist))
00108
00109
00110 gt.remove_parallel_edges(graph)
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
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
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
00142 if start != -1 and goal != -1:
00143 dist, pred = gt.astar_search(graph, graph.vertex(start), edgeweights)
00144
00145
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
00156
00157
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
00166 space = ob.SE3StateSpace()
00167
00168
00169 bounds = ob.RealVectorBounds(3)
00170 bounds.setLow(-10)
00171 bounds.setHigh(10)
00172 space.setBounds(bounds)
00173
00174
00175 ss = og.SimpleSetup(space)
00176
00177
00178 start = ob.State(space)
00179 start().setX(-9)
00180 start().setY(-9)
00181 start().setZ(-9)
00182 start().rotation().setIdentity()
00183
00184
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
00194 ss.setStartAndGoalStates(start, goal, 0.05)
00195
00196
00197 planner = og.PRM(ss.getSpaceInformation())
00198 ss.setPlanner(planner)
00199 ss.setup()
00200
00201
00202 solved = ss.solve(20.0)
00203
00204 if solved:
00205
00206 print("Found solution:\n%s" % ss.getSolutionPath())
00207
00208
00209 pd = ob.PlannerData(ss.getSpaceInformation())
00210 ss.getPlannerData(pd)
00211
00212
00213 pd.computeEdgeWeights(ob.EdgeWeightFn(partial(edgeWeight, space)))
00214
00215 useGraphTool(pd, space)
00216
00217 if __name__ == "__main__":
00218 plan()