ProductGraph.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2012, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Matt Maly */
36 
37 #include "ompl/control/planners/ltl/ProductGraph.h"
38 #include "ompl/base/State.h"
39 #include "ompl/control/planners/ltl/Automaton.h"
40 #include "ompl/control/planners/ltl/PropositionalDecomposition.h"
41 #include "ompl/control/planners/ltl/World.h"
42 #include "ompl/util/ClassForward.h"
43 #include "ompl/util/Console.h"
44 #include "ompl/util/Hash.h"
45 #include "ompl/util/DisableCompilerWarning.h"
46 #include <algorithm>
47 #include <boost/graph/adjacency_list.hpp>
48 #include <boost/graph/dijkstra_shortest_paths.hpp>
49 #include <unordered_map>
50 #include <unordered_set>
51 #include <map>
52 #include <ostream>
53 #include <queue>
54 #include <stack>
55 #include <utility>
56 #include <vector>
57 
59 {
60  return decompRegion == s.decompRegion && cosafeState == s.cosafeState && safeState == s.safeState;
61 }
62 
64 {
65  return cosafeState != -1 && safeState != -1;
66 }
67 
68 std::size_t ompl::control::ProductGraph::HashState::operator()(const ompl::control::ProductGraph::State &s) const
69 {
70  std::size_t hash = std::hash<int>()(s.decompRegion);
71  hash_combine(hash, s.cosafeState);
72  hash_combine(hash, s.safeState);
73  return hash;
74 }
75 
76 namespace ompl
77 {
78  namespace control
79  {
80  std::ostream &operator<<(std::ostream &out, const ProductGraph::State &s)
81  {
82  out << "(" << s.decompRegion << "," << s.cosafeState << ",";
83  out << s.safeState << ")";
84  return out;
85  }
86  }
87 }
88 
90 {
91  return decompRegion;
92 }
93 
95 {
96  return cosafeState;
97 }
98 
100 {
101  return safeState;
102 }
103 
105  AutomatonPtr safetyAut)
106  : decomp_(std::move(decomp)), cosafety_(std::move(cosafetyAut)), safety_(std::move(safetyAut))
107 {
108 }
109 
111  : decomp_(decomp), cosafety_(std::move(cosafetyAut)), safety_(Automaton::AcceptingAutomaton(decomp->getNumProps()))
112 {
113 }
114 
115 ompl::control::ProductGraph::~ProductGraph()
116 {
117  clear();
118 }
119 
121 {
122  return decomp_;
123 }
124 
126 {
127  return cosafety_;
128 }
129 
131 {
132  return safety_;
133 }
134 
135 std::vector<ompl::control::ProductGraph::State *> ompl::control::ProductGraph::computeLead(
136  ProductGraph::State *start, const std::function<double(ProductGraph::State *, ProductGraph::State *)> &edgeWeight)
137 {
138  std::vector<GraphType::vertex_descriptor> parents(boost::num_vertices(graph_));
139  std::vector<double> distances(boost::num_vertices(graph_));
140 OMPL_PUSH_DISABLE_GCC_WARNING(-Wmaybe-uninitialized)
141  EdgeIter ei, eend;
142 OMPL_POP_GCC
143  // first build up the edge weights
144  for (boost::tie(ei, eend) = boost::edges(graph_); ei != eend; ++ei)
145  {
146  GraphType::vertex_descriptor src = boost::source(*ei, graph_);
147  GraphType::vertex_descriptor target = boost::target(*ei, graph_);
148  graph_[*ei].cost = edgeWeight(graph_[src], graph_[target]);
149  }
150  int startIndex = stateToIndex_[start];
151  boost::dijkstra_shortest_paths(
152  graph_, boost::vertex(startIndex, graph_),
153  boost::weight_map(get(&Edge::cost, graph_))
154  .distance_map(boost::make_iterator_property_map(distances.begin(), get(boost::vertex_index, graph_)))
155  .predecessor_map(boost::make_iterator_property_map(parents.begin(), get(boost::vertex_index, graph_))));
156  // pick state from solutionStates_ such that distance[state] is minimized
157  State *bestSoln = *solutionStates_.begin();
158  double cost = distances[boost::vertex(stateToIndex_[bestSoln], graph_)];
159  for (std::vector<State *>::const_iterator s = solutionStates_.begin() + 1; s != solutionStates_.end(); ++s)
160  {
161  if (distances[boost::vertex(stateToIndex_[*s], graph_)] < cost)
162  {
163  cost = distances[boost::vertex(stateToIndex_[*s], graph_)];
164  bestSoln = *s;
165  }
166  }
167  // build lead from bestSoln parents
168  std::stack<State *> leadStack;
169  while (!(bestSoln == start))
170  {
171  leadStack.push(bestSoln);
172  bestSoln = graph_[parents[boost::vertex(stateToIndex_[bestSoln], graph_)]];
173  }
174  leadStack.push(bestSoln);
175 
176  std::vector<State *> lead;
177  while (!leadStack.empty())
178  {
179  lead.push_back(leadStack.top());
180  leadStack.pop();
181  // Truncate the lead as early when it hits the desired automaton states
182  // \todo: more elegant way to do this?
183  if (lead.back()->cosafeState == solutionStates_.front()->cosafeState &&
184  lead.back()->safeState == solutionStates_.front()->safeState)
185  break;
186  }
187  return lead;
188 }
189 
191 {
192  solutionStates_.clear();
193  stateToIndex_.clear();
194  startState_ = nullptr;
195  graph_.clear();
196  for (auto &i : stateToPtr_)
197  delete i.second;
198  stateToPtr_.clear();
199 }
200 
201 void ompl::control::ProductGraph::buildGraph(State *start, const std::function<void(State *)> &initialize)
202 {
203  graph_.clear();
204  solutionStates_.clear();
205  std::queue<State *> q;
206  std::unordered_set<State *> processed;
207  std::vector<int> regNeighbors;
208  VertexIndexMap index = get(boost::vertex_index, graph_);
209 
210  GraphType::vertex_descriptor next = boost::add_vertex(graph_);
211  startState_ = start;
212  graph_[boost::vertex(next, graph_)] = startState_;
213  stateToIndex_[startState_] = index[next];
214  q.push(startState_);
215  processed.insert(startState_);
216 
217  OMPL_INFORM("Building graph from start state (%u,%u,%u) with index %d", startState_->decompRegion,
218  startState_->cosafeState, startState_->safeState, stateToIndex_[startState_]);
219 
220  while (!q.empty())
221  {
222  State *current = q.front();
223  // Initialize each state using the supplied state initializer function
224  initialize(current);
225  q.pop();
226 
227  if (safety_->isAccepting(current->safeState) && cosafety_->isAccepting(current->cosafeState))
228  {
229  solutionStates_.push_back(current);
230  }
231 
232  GraphType::vertex_descriptor v = boost::vertex(stateToIndex_[current], graph_);
233 
234  // enqueue each neighbor of current
235  decomp_->getNeighbors(current->decompRegion, regNeighbors);
236  for (const auto &r : regNeighbors)
237  {
238  State *nextState = getState(current, r);
239  if (!nextState->isValid())
240  continue;
241  // if this state is newly discovered,
242  // then we can dynamically allocate a copy of it
243  // and add the new pointer to the graph.
244  // either way, we need the pointer
245  if (processed.find(nextState) == processed.end())
246  {
247  const GraphType::vertex_descriptor next = boost::add_vertex(graph_);
248  stateToIndex_[nextState] = index[next];
249  graph_[boost::vertex(next, graph_)] = nextState;
250  q.push(nextState);
251  processed.insert(nextState);
252  }
253 
254  // whether or not the neighbor is newly discovered,
255  // we still need to add the edge to the graph
256  GraphType::edge_descriptor edge;
257  bool ignore;
258  boost::tie(edge, ignore) = boost::add_edge(v, boost::vertex(stateToIndex_[nextState], graph_), graph_);
259  // graph_[edge].src = index[v];
260  // graph_[edge].dest = stateToIndex_[nextState];
261  }
262  regNeighbors.clear();
263  }
264  if (solutionStates_.empty())
265  {
266  OMPL_ERROR("No solution path found in product graph.");
267  }
268 
269  OMPL_INFORM("Number of decomposition regions: %u", decomp_->getNumRegions());
270  OMPL_INFORM("Number of cosafety automaton states: %u", cosafety_->numStates());
271  OMPL_INFORM("Number of safety automaton states: %u", safety_->numStates());
272  OMPL_INFORM("Number of high-level states in abstraction graph: %u", boost::num_vertices(graph_));
273 }
274 
276 {
277  return std::find(solutionStates_.begin(), solutionStates_.end(), s) != solutionStates_.end();
278 }
279 
281 {
282  return startState_;
283 }
284 
286 {
287  return decomp_->getRegionVolume(s->decompRegion);
288 }
289 
291 {
292  return cosafety_->distFromAccepting(s->cosafeState);
293 }
294 
296 {
297  return safety_->distFromAccepting(s->safeState);
298 }
299 
301 {
302  return getState(cs, cosafety_->getStartState(), safety_->getStartState());
303 }
304 
306  int safe) const
307 {
308  State s;
309  s.decompRegion = decomp_->locateRegion(cs);
310  s.cosafeState = cosafe;
311  s.safeState = safe;
312  State *&ret = stateToPtr_[s];
313  if (ret == nullptr)
314  ret = new State(s);
315  return ret;
316 }
317 
319 {
320  State s;
321  s.decompRegion = nextRegion;
322  const World nextWorld = decomp_->worldAtRegion(nextRegion);
323  s.cosafeState = cosafety_->step(parent->cosafeState, nextWorld);
324  s.safeState = safety_->step(parent->safeState, nextWorld);
325  State *&ret = stateToPtr_[s];
326  if (ret == nullptr)
327  ret = new State(s);
328  return ret;
329 }
330 
332  const base::State *cs) const
333 {
334  return getState(parent, decomp_->locateRegion(cs));
335 }
bool isSolution(const State *s) const
Returns whether the given State is an accepting State in this ProductGraph. We call a State accepting...
bool isValid() const
Returns whether this State is valid. A State is valid if and only if none of its Automaton states are...
const AutomatonPtr & getCosafetyAutom() const
Returns the co-safe Automaton contained within this ProductGraph.
const PropositionalDecompositionPtr & getDecomp() const
Returns the PropositionalDecomposition contained within this ProductGraph.
bool operator==(const State &s) const
Returns whether this State is equivalent to a given State, by comparing their PropositionalDecomposit...
int getDecompRegion() const
Returns this State's PropositionalDecomposition region component.
int getSafeState() const
Returns this State's safe Automaton state component.
Definition of an abstract state.
Definition: State.h:113
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...
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
std::vector< State * > computeLead(State *start, const std::function< double(State *, State *)> &edgeWeight)
Returns a shortest-path sequence of ProductGraph states, beginning with a given initial State and end...
State * getState(const base::State *cs) const
Returns a ProductGraph State with initial co-safety and safety Automaton states, and the Propositiona...
A State of a ProductGraph represents a vertex in the graph-based Cartesian product represented by the...
Definition: ProductGraph.h:147
A shared pointer wrapper for ompl::control::Automaton.
ProductGraph(PropositionalDecompositionPtr decomp, AutomatonPtr cosafetyAut, AutomatonPtr safetyAut)
Initializes a ProductGraph with a given PropositionalDecomposition, co-safe Automaton,...
State * getStartState() const
Returns the initial State of this ProductGraph.
void clear()
Clears all memory belonging to this ProductGraph.
A class to represent a deterministic finite automaton, each edge of which corresponds to a World....
Definition: Automaton.h:134
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
A shared pointer wrapper for ompl::control::PropositionalDecomposition.
int getCosafeState() const
Returns this State's co-safe Automaton state component.
const AutomatonPtr & getSafetyAutom() const
Returns the safe Automaton contained within this ProductGraph.
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...
double getRegionVolume(const State *s)
Helper method to return the volume of the PropositionalDecomposition region corresponding to the give...
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
void buildGraph(State *start, const std::function< void(State *)> &initialize=[](State *){})
Constructs this ProductGraph beginning with a given initial State, using a breadth-first search....
A class to represent an assignment of boolean values to propositions. A World can be partially restri...
Definition: World.h:71