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 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.graphml",
'w')
90 graph = gt.load_graph(
"graph.graphml", fmt=
"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,
161 print(
'\nGraph written to graph.png')
171 space.setBounds(bounds)
181 start().rotation().setIdentity()
188 goal().rotation().setIdentity()
193 ss.setStartAndGoalStates(start, goal, 0.05)
196 planner =
og.PRM(ss.getSpaceInformation())
197 ss.setPlanner(planner)
201 solved = ss.solve(20.0)
205 print(
"Found solution:\n%s" % ss.getSolutionPath())
209 ss.getPlannerData(pd)
212 pd.computeEdgeWeights()
215 useGraphTool(pd, space)
217 if __name__ ==
"__main__":
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Create the set of classes typically needed to solve a geometric problem.
Definition of an abstract state.
The lower and upper bounds for an Rn space.
Probabilistic RoadMap planner.
A state space representing SE(3)
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...