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 }
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:71
const AutomatonPtr & getSafetyAutom() const
Returns the safe Automaton contained within this ProductGraph.
bool isSolution(const State *s) const
Returns whether the given State is an accepting State in this ProductGraph. We call a State accepting...
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...
A shared pointer wrapper for ompl::control::PropositionalDecomposition.
STL namespace.
int getSafeAutDistance(const State *s) const
Helper method to return the distance from a given State&#39;s safety state to an accepting state in the s...
State * getState(const base::State *cs) const
Returns a ProductGraph State with initial co-safety and safety Automaton states, and the Propositiona...
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
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...
int getCosafeState() const
Returns this State&#39;s co-safe Automaton state component.
A State of a ProductGraph represents a vertex in the graph-based Cartesian product represented by the...
Definition: ProductGraph.h:83
Definition of an abstract state.
Definition: State.h:49
State * getStartState() const
Returns the initial State of this ProductGraph.
void clear()
Clears all memory belonging to this ProductGraph.
A shared pointer wrapper for ompl::control::Automaton.
int getCosafeAutDistance(const State *s) const
Helper method to return the distance from a given State&#39;s co-safety state to an accepting state in th...
bool isValid() const
Returns whether this State is valid. A State is valid if and only if none of its Automaton states are...
ProductGraph(PropositionalDecompositionPtr decomp, AutomatonPtr cosafetyAut, AutomatonPtr safetyAut)
Initializes a ProductGraph with a given PropositionalDecomposition, co-safe Automaton, and safe Automaton.
A class to represent a deterministic finite automaton, each edge of which corresponds to a World...
Definition: Automaton.h:70
int getSafeState() const
Returns this State&#39;s safe Automaton state component.
void buildGraph(State *start, const std::function< void(State *)> &initialize=ProductGraph::noInit)
Constructs this ProductGraph beginning with a given initial State, using a breadth-first search...
int getDecompRegion() const
Returns this State&#39;s PropositionalDecomposition region component.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68