40 from functools
import partial
47 import graph_tool.all
as gt
50 print 'Failed to import graph-tool. PlannerData will not be analyzed or plotted'
54 from ompl
import util
as ou
55 from ompl
import base
as ob
56 from ompl
import geometric
as og
60 from os.path
import basename, abspath, dirname, join
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
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):
79 def edgeWeight(space, v1, v2, edge):
80 return space.distance(v1.getState(), v2.getState())
82 def useGraphTool(pd, space):
84 graphml = pd.printGraphML()
85 f = open(
"graph.xml",
'w')
90 graph = gt.load_graph(
"graph.xml")
91 edgeweights = graph.edge_properties[
"weight"]
94 avgdeg, stddevdeg = gt.vertex_average(graph,
"total")
95 avgwt, stddevwt = gt.edge_average(graph, edgeweights)
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)
102 comps, hist = gt.label_components(graph)
103 print "Strongly connected components: " + str(len(hist))
105 graph.set_directed(
False)
106 comps, hist = gt.label_components(graph)
107 print "Weakly connected components: " + str(len(hist))
110 gt.remove_parallel_edges(graph)
112 edgeweights = graph.edge_properties[
"weight"]
113 colorprops = graph.new_vertex_property(
"string")
114 vertexsize = graph.new_vertex_property(
"double")
119 for v
in range(graph.num_vertices()):
122 if (pd.isStartVertex(v)):
124 colorprops[graph.vertex(v)] =
"cyan"
125 vertexsize[graph.vertex(v)] = 10
126 elif (pd.isGoalVertex(v)):
128 colorprops[graph.vertex(v)] =
"green"
129 vertexsize[graph.vertex(v)] = 10
131 colorprops[graph.vertex(v)] =
"yellow"
132 vertexsize[graph.vertex(v)] = 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"
142 if start != -1
and goal != -1:
143 dist, pred = gt.astar_search(graph, graph.vertex(start), edgeweights)
146 v = graph.vertex(goal)
147 while v != graph.vertex(start):
148 p = graph.vertex(pred[v])
149 for e
in p.out_edges():
158 gt.graph_draw (graph, vertex_size=vertexsize, vertex_fill_color=colorprops,
159 edge_pen_width=edgesize, edge_color=edgecolor,
162 print 'Graph written to graph.png'
172 space.setBounds(bounds)
182 start().rotation().setIdentity()
189 goal().rotation().setIdentity()
194 ss.setStartAndGoalStates(start, goal, 0.05)
197 planner =
og.PRM(ss.getSpaceInformation())
198 ss.setPlanner(planner)
202 solved = ss.solve(20.0)
206 print(
"Found solution:\n%s" % ss.getSolutionPath())
210 ss.getPlannerData(pd)
213 pd.computeEdgeWeights(ob.EdgeWeightFn(partial(edgeWeight, space)))
215 useGraphTool(pd, space)
217 if __name__ ==
"__main__":