ProductGraph.cpp
1 #include "ompl/control/planners/ltl/ProductGraph.h"
2 #include "ompl/base/State.h"
3 #include "ompl/control/planners/ltl/Automaton.h"
4 #include "ompl/control/planners/ltl/PropositionalDecomposition.h"
5 #include "ompl/control/planners/ltl/World.h"
6 #include "ompl/util/ClassForward.h"
7 #include "ompl/util/Console.h"
8 #include <algorithm>
9 #include <boost/function.hpp>
10 #include <boost/functional/hash.hpp>
11 #include <boost/graph/adjacency_list.hpp>
12 #include <boost/graph/dijkstra_shortest_paths.hpp>
13 #include <boost/unordered_map.hpp>
14 #include <boost/unordered_set.hpp>
15 #include <map>
16 #include <ostream>
17 #include <queue>
18 #include <stack>
19 #include <vector>
20 
22 {
23  return decompRegion==s.decompRegion
24  && cosafeState==s.cosafeState
25  && safeState==s.safeState;
26 }
27 
29 {
30  return cosafeState != -1 && safeState != -1;
31 }
32 
33 namespace ompl
34 {
35  namespace control
36  {
37  std::size_t hash_value(const ProductGraph::State& s)
38  {
39  std::size_t hash = 0;
40  boost::hash_combine(hash, s.decompRegion);
41  boost::hash_combine(hash, s.cosafeState);
42  boost::hash_combine(hash, s.safeState);
43  return hash;
44  }
45 
46  std::ostream& operator<<(std::ostream& out, const ProductGraph::State& s)
47  {
48  out << "(" << s.decompRegion << "," << s.cosafeState << ",";
49  out << s.safeState << ")";
50  return out;
51  }
52  }
53 }
54 
56 {
57  return decompRegion;
58 }
59 
61 {
62  return cosafeState;
63 }
64 
66 {
67  return safeState;
68 }
69 
71  const AutomatonPtr& cosafetyAut, const AutomatonPtr& safetyAut) :
72  decomp_(decomp),
73  cosafety_(cosafetyAut),
74  safety_(safetyAut)
75 {
76 }
77 
79  const AutomatonPtr& cosafetyAut) :
80  decomp_(decomp),
81  cosafety_(cosafetyAut),
82  safety_(Automaton::AcceptingAutomaton(decomp->getNumProps()))
83 {
84 }
85 
86 ompl::control::ProductGraph::~ProductGraph()
87 {
88  clear();
89 }
90 
92 {
93  return decomp_;
94 }
95 
97 {
98  return cosafety_;
99 }
100 
102 {
103  return safety_;
104 }
105 
106 std::vector<ompl::control::ProductGraph::State*>
108  ProductGraph::State* start,
109  const boost::function<double(ProductGraph::State*, ProductGraph::State*)>& edgeWeight)
110 {
111  std::vector<GraphType::vertex_descriptor> parents(boost::num_vertices(graph_));
112  std::vector<double> distances(boost::num_vertices(graph_));
113  EdgeIter ei, eend;
114  //first build up the edge weights
115  for (boost::tie(ei,eend) = boost::edges(graph_); ei != eend; ++ei)
116  {
117  GraphType::vertex_descriptor src = boost::source(*ei, graph_);
118  GraphType::vertex_descriptor target = boost::target(*ei, graph_);
119  graph_[*ei].cost = edgeWeight(graph_[src], graph_[target]);
120  }
121  int startIndex = stateToIndex_[start];
122  boost::dijkstra_shortest_paths(graph_, boost::vertex(startIndex,graph_),
123  boost::weight_map(get(&Edge::cost, graph_)).distance_map(
124  boost::make_iterator_property_map(distances.begin(), get(boost::vertex_index, graph_)
125  )).predecessor_map(
126  boost::make_iterator_property_map(parents.begin(), get(boost::vertex_index, graph_))
127  )
128  );
129  //pick state from solutionStates_ such that distance[state] is minimized
130  State* bestSoln = *solutionStates_.begin();
131  double cost = distances[boost::vertex(stateToIndex_[bestSoln], graph_)];
132  for (std::vector<State*>::const_iterator s = solutionStates_.begin()+1; s != solutionStates_.end(); ++s)
133  {
134  if (distances[boost::vertex(stateToIndex_[*s], graph_)] < cost)
135  {
136  cost = distances[boost::vertex(stateToIndex_[*s], graph_)];
137  bestSoln = *s;
138  }
139  }
140  //build lead from bestSoln parents
141  std::stack<State*> leadStack;
142  while (!(bestSoln == start))
143  {
144  leadStack.push(bestSoln);
145  bestSoln = graph_[parents[boost::vertex(stateToIndex_[bestSoln], graph_)]];
146  }
147  leadStack.push(bestSoln);
148 
149  std::vector<State*> lead;
150  while (!leadStack.empty())
151  {
152  lead.push_back(leadStack.top());
153  leadStack.pop();
154  // Truncate the lead as early when it hits the desired automaton states
155  // TODO: more elegant way to do this?
156  if (lead.back()->cosafeState == solutionStates_.front()->cosafeState
157  && lead.back()->safeState == solutionStates_.front()->safeState)
158  break;
159  }
160  return lead;
161 }
162 
164 {
165  solutionStates_.clear();
166  stateToIndex_.clear();
167  startState_ = NULL;
168  graph_.clear();
169  boost::unordered_map<State,State*>::iterator i;
170  for (i = stateToPtr_.begin(); i != stateToPtr_.end(); ++i)
171  delete i->second;
172  stateToPtr_.clear();
173 }
174 
175 void ompl::control::ProductGraph::buildGraph(State* start, const boost::function<void(State*)>& initialize)
176 {
177  graph_.clear();
178  solutionStates_.clear();
179  std::queue<State*> q;
180  boost::unordered_set<State*> processed;
181  std::vector<int> regNeighbors;
182  VertexIndexMap index = get(boost::vertex_index, graph_);
183 
184  GraphType::vertex_descriptor next = boost::add_vertex(graph_);
185  startState_ = start;
186  graph_[boost::vertex(next,graph_)] = startState_;
187  stateToIndex_[startState_] = index[next];
188  q.push(startState_);
189  processed.insert(startState_);
190 
191  OMPL_INFORM("Building graph from start state (%u,%u,%u) with index %d",
192  startState_->decompRegion, startState_->cosafeState,
193  startState_->safeState, stateToIndex_[startState_]);
194 
195  while (!q.empty())
196  {
197  State* current = q.front();
198  //Initialize each state using the supplied state initializer function
199  initialize(current);
200  q.pop();
201 
202  if (safety_->isAccepting(current->safeState) && cosafety_->isAccepting(current->cosafeState))
203  {
204  solutionStates_.push_back(current);
205  }
206 
207  GraphType::vertex_descriptor v = boost::vertex(stateToIndex_[current], graph_);
208 
209  //enqueue each neighbor of current
210  decomp_->getNeighbors(current->decompRegion, regNeighbors);
211  for (std::vector<int>::const_iterator r = regNeighbors.begin(); r != regNeighbors.end(); ++r)
212  {
213  State* nextState = getState(current, *r);
214  if (!nextState->isValid())
215  continue;
216  //if this state is newly discovered,
217  //then we can dynamically allocate a copy of it
218  //and add the new pointer to the graph.
219  //either way, we need the pointer
220  if (processed.find(nextState) == processed.end())
221  {
222  const GraphType::vertex_descriptor next = boost::add_vertex(graph_);
223  stateToIndex_[nextState] = index[next];
224  graph_[boost::vertex(next,graph_)] = nextState;
225  q.push(nextState);
226  processed.insert(nextState);
227  }
228 
229  //whether or not the neighbor is newly discovered,
230  //we still need to add the edge to the graph
231  GraphType::edge_descriptor edge;
232  bool ignore;
233  boost::tie(edge,ignore) = boost::add_edge(v, boost::vertex(stateToIndex_[nextState],graph_), graph_);
234  //graph_[edge].src = index[v];
235  //graph_[edge].dest = stateToIndex_[nextState];
236  }
237  regNeighbors.clear();
238  }
239  if (solutionStates_.empty())
240  {
241  OMPL_ERROR("No solution path found in product graph.");
242  }
243 
244  OMPL_INFORM("Number of decomposition regions: %u", decomp_->getNumRegions());
245  OMPL_INFORM("Number of cosafety automaton states: %u", cosafety_->numStates());
246  OMPL_INFORM("Number of safety automaton states: %u", safety_->numStates());
247  OMPL_INFORM("Number of high-level states in abstraction graph: %u", boost::num_vertices(graph_));
248 }
249 
251 {
252  return std::find(solutionStates_.begin(), solutionStates_.end(), s)
253  != solutionStates_.end();
254 }
255 
257 {
258  return startState_;
259 }
260 
262 {
263  return decomp_->getRegionVolume(s->decompRegion);
264 }
265 
267 {
268  return cosafety_->distFromAccepting(s->cosafeState);
269 }
270 
272 {
273  return safety_->distFromAccepting(s->safeState);
274 }
275 
277 {
278  return getState(cs, cosafety_->getStartState(), safety_->getStartState());
279 }
280 
282 {
283  State s;
284  s.decompRegion = decomp_->locateRegion(cs);
285  s.cosafeState = cosafe;
286  s.safeState = safe;
287  State*& ret = stateToPtr_[s];
288  if (ret == NULL)
289  ret = new State(s);
290  return ret;
291 }
292 
294 {
295  State s;
296  s.decompRegion = nextRegion;
297  const World nextWorld = decomp_->worldAtRegion(nextRegion);
298  s.cosafeState = cosafety_->step(parent->cosafeState, nextWorld);
299  s.safeState = safety_->step(parent->safeState, nextWorld);
300  State*& ret = stateToPtr_[s];
301  if (ret == NULL)
302  ret = new State(s);
303  return ret;
304 }
305 
307 {
308  return getState(parent, decomp_->locateRegion(cs));
309 }
double getRegionVolume(const State *s)
Helper method to return the volume of the PropositionalDecomposition region corresponding to the give...
A class to represent an assignment of boolean values to propositions. A World can be partially restri...
Definition: World.h:51
A boost shared pointer wrapper for ompl::control::PropositionalDecomposition.
void buildGraph(State *start, const boost::function< void(State *)> &initialize=ProductGraph::noInit)
Constructs this ProductGraph beginning with a given initial State, using a breadth-first search...
State * getState(const base::State *cs) const
Returns a ProductGraph State with initial co-safety and safety Automaton states, and the Propositiona...
int getSafeAutDistance(const State *s) const
Helper method to return the distance from a given State's safety state to an accepting state in the s...
State * getStartState(void) const
Returns the initial State of this ProductGraph.
std::vector< State * > computeLead(State *start, const boost::function< double(State *, State *)> &edgeWeight)
Returns a shortest-path sequence of ProductGraph states, beginning with a given initial State and end...
Main namespace. Contains everything in this library.
Definition: Cost.h:42
const PropositionalDecompositionPtr & getDecomp() const
Returns the PropositionalDecomposition contained within this ProductGraph.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
int getCosafeAutDistance(const State *s) const
Helper method to return the distance from a given State's co-safety state to an accepting state in th...
const AutomatonPtr & getSafetyAutom() const
Returns the safe Automaton contained within this ProductGraph.
A State of a ProductGraph represents a vertex in the graph-based Cartesian product represented by the...
Definition: ProductGraph.h:74
bool isValid(void) const
Returns whether this State is valid. A State is valid if and only if none of its Automaton states are...
Definition of an abstract state.
Definition: State.h:50
void clear()
Clears all memory belonging to this ProductGraph.
int getSafeState(void) const
Returns this State's safe Automaton state component.
bool operator==(const State &s) const
Returns whether this State is equivalent to a given State, by comparing their PropositionalDecomposit...
A boost shared pointer wrapper for ompl::control::Automaton.
bool isSolution(const State *s) const
Returns whether the given State is an accepting State in this ProductGraph. We call a State accepting...
int getDecompRegion(void) const
Returns this State's PropositionalDecomposition region component.
A class to represent a deterministic finite automaton, each edge of which corresponds to a World...
Definition: Automaton.h:69
int getCosafeState(void) const
Returns this State's co-safe Automaton state component.
ProductGraph(const PropositionalDecompositionPtr &decomp, const AutomatonPtr &cosafetyAut, const AutomatonPtr &safetyAut)
Initializes a ProductGraph with a given PropositionalDecomposition, co-safe Automaton, and safe Automaton.
const AutomatonPtr & getCosafetyAutom() const
Returns the co-safe Automaton contained within this ProductGraph.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68