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 <algorithm>
46 #include <boost/graph/adjacency_list.hpp>
47 #include <boost/graph/dijkstra_shortest_paths.hpp>
48 #include <unordered_map>
49 #include <unordered_set>
50 #include <map>
51 #include <ostream>
52 #include <queue>
53 #include <stack>
54 #include <utility>
55 #include <vector>
56 
58 {
59  return decompRegion == s.decompRegion && cosafeState == s.cosafeState && safeState == s.safeState;
60 }
61 
63 {
64  return cosafeState != -1 && safeState != -1;
65 }
66 
67 std::size_t ompl::control::ProductGraph::HashState::operator()(const ompl::control::ProductGraph::State &s) const
68 {
69  std::size_t hash = std::hash<int>()(s.decompRegion);
70  hash_combine(hash, s.cosafeState);
71  hash_combine(hash, s.safeState);
72  return hash;
73 }
74 
75 namespace ompl
76 {
77  namespace control
78  {
79  std::ostream &operator<<(std::ostream &out, const ProductGraph::State &s)
80  {
81  out << "(" << s.decompRegion << "," << s.cosafeState << ",";
82  out << s.safeState << ")";
83  return out;
84  }
85  }
86 }
87 
89 {
90  return decompRegion;
91 }
92 
94 {
95  return cosafeState;
96 }
97 
99 {
100  return safeState;
101 }
102 
104  AutomatonPtr safetyAut)
105  : decomp_(std::move(decomp)), cosafety_(std::move(cosafetyAut)), safety_(std::move(safetyAut))
106 {
107 }
108 
110  : decomp_(decomp), cosafety_(std::move(cosafetyAut)), safety_(Automaton::AcceptingAutomaton(decomp->getNumProps()))
111 {
112 }
113 
114 ompl::control::ProductGraph::~ProductGraph()
115 {
116  clear();
117 }
118 
120 {
121  return decomp_;
122 }
123 
125 {
126  return cosafety_;
127 }
128 
130 {
131  return safety_;
132 }
133 
134 std::vector<ompl::control::ProductGraph::State *> ompl::control::ProductGraph::computeLead(
135  ProductGraph::State *start, const std::function<double(ProductGraph::State *, ProductGraph::State *)> &edgeWeight)
136 {
137  std::vector<GraphType::vertex_descriptor> parents(boost::num_vertices(graph_));
138  std::vector<double> distances(boost::num_vertices(graph_));
139  EdgeIter ei, eend;
140  // first build up the edge weights
141  for (boost::tie(ei, eend) = boost::edges(graph_); ei != eend; ++ei)
142  {
143  GraphType::vertex_descriptor src = boost::source(*ei, graph_);
144  GraphType::vertex_descriptor target = boost::target(*ei, graph_);
145  graph_[*ei].cost = edgeWeight(graph_[src], graph_[target]);
146  }
147  int startIndex = stateToIndex_[start];
148  boost::dijkstra_shortest_paths(
149  graph_, boost::vertex(startIndex, graph_),
150  boost::weight_map(get(&Edge::cost, graph_))
151  .distance_map(boost::make_iterator_property_map(distances.begin(), get(boost::vertex_index, graph_)))
152  .predecessor_map(boost::make_iterator_property_map(parents.begin(), get(boost::vertex_index, graph_))));
153  // pick state from solutionStates_ such that distance[state] is minimized
154  State *bestSoln = *solutionStates_.begin();
155  double cost = distances[boost::vertex(stateToIndex_[bestSoln], graph_)];
156  for (std::vector<State *>::const_iterator s = solutionStates_.begin() + 1; s != solutionStates_.end(); ++s)
157  {
158  if (distances[boost::vertex(stateToIndex_[*s], graph_)] < cost)
159  {
160  cost = distances[boost::vertex(stateToIndex_[*s], graph_)];
161  bestSoln = *s;
162  }
163  }
164  // build lead from bestSoln parents
165  std::stack<State *> leadStack;
166  while (!(bestSoln == start))
167  {
168  leadStack.push(bestSoln);
169  bestSoln = graph_[parents[boost::vertex(stateToIndex_[bestSoln], graph_)]];
170  }
171  leadStack.push(bestSoln);
172 
173  std::vector<State *> lead;
174  while (!leadStack.empty())
175  {
176  lead.push_back(leadStack.top());
177  leadStack.pop();
178  // Truncate the lead as early when it hits the desired automaton states
179  // \todo: more elegant way to do this?
180  if (lead.back()->cosafeState == solutionStates_.front()->cosafeState &&
181  lead.back()->safeState == solutionStates_.front()->safeState)
182  break;
183  }
184  return lead;
185 }
186 
188 {
189  solutionStates_.clear();
190  stateToIndex_.clear();
191  startState_ = nullptr;
192  graph_.clear();
193  for (auto &i : stateToPtr_)
194  delete i.second;
195  stateToPtr_.clear();
196 }
197 
198 void ompl::control::ProductGraph::buildGraph(State *start, const std::function<void(State *)> &initialize)
199 {
200  graph_.clear();
201  solutionStates_.clear();
202  std::queue<State *> q;
203  std::unordered_set<State *> processed;
204  std::vector<int> regNeighbors;
205  VertexIndexMap index = get(boost::vertex_index, graph_);
206 
207  GraphType::vertex_descriptor next = boost::add_vertex(graph_);
208  startState_ = start;
209  graph_[boost::vertex(next, graph_)] = startState_;
210  stateToIndex_[startState_] = index[next];
211  q.push(startState_);
212  processed.insert(startState_);
213 
214  OMPL_INFORM("Building graph from start state (%u,%u,%u) with index %d", startState_->decompRegion,
215  startState_->cosafeState, startState_->safeState, stateToIndex_[startState_]);
216 
217  while (!q.empty())
218  {
219  State *current = q.front();
220  // Initialize each state using the supplied state initializer function
221  initialize(current);
222  q.pop();
223 
224  if (safety_->isAccepting(current->safeState) && cosafety_->isAccepting(current->cosafeState))
225  {
226  solutionStates_.push_back(current);
227  }
228 
229  GraphType::vertex_descriptor v = boost::vertex(stateToIndex_[current], graph_);
230 
231  // enqueue each neighbor of current
232  decomp_->getNeighbors(current->decompRegion, regNeighbors);
233  for (const auto &r : regNeighbors)
234  {
235  State *nextState = getState(current, r);
236  if (!nextState->isValid())
237  continue;
238  // if this state is newly discovered,
239  // then we can dynamically allocate a copy of it
240  // and add the new pointer to the graph.
241  // either way, we need the pointer
242  if (processed.find(nextState) == processed.end())
243  {
244  const GraphType::vertex_descriptor next = boost::add_vertex(graph_);
245  stateToIndex_[nextState] = index[next];
246  graph_[boost::vertex(next, graph_)] = nextState;
247  q.push(nextState);
248  processed.insert(nextState);
249  }
250 
251  // whether or not the neighbor is newly discovered,
252  // we still need to add the edge to the graph
253  GraphType::edge_descriptor edge;
254  bool ignore;
255  boost::tie(edge, ignore) = boost::add_edge(v, boost::vertex(stateToIndex_[nextState], graph_), graph_);
256  // graph_[edge].src = index[v];
257  // graph_[edge].dest = stateToIndex_[nextState];
258  }
259  regNeighbors.clear();
260  }
261  if (solutionStates_.empty())
262  {
263  OMPL_ERROR("No solution path found in product graph.");
264  }
265 
266  OMPL_INFORM("Number of decomposition regions: %u", decomp_->getNumRegions());
267  OMPL_INFORM("Number of cosafety automaton states: %u", cosafety_->numStates());
268  OMPL_INFORM("Number of safety automaton states: %u", safety_->numStates());
269  OMPL_INFORM("Number of high-level states in abstraction graph: %u", boost::num_vertices(graph_));
270 }
271 
273 {
274  return std::find(solutionStates_.begin(), solutionStates_.end(), s) != solutionStates_.end();
275 }
276 
278 {
279  return startState_;
280 }
281 
283 {
284  return decomp_->getRegionVolume(s->decompRegion);
285 }
286 
288 {
289  return cosafety_->distFromAccepting(s->cosafeState);
290 }
291 
293 {
294  return safety_->distFromAccepting(s->safeState);
295 }
296 
298 {
299  return getState(cs, cosafety_->getStartState(), safety_->getStartState());
300 }
301 
303  int safe) const
304 {
305  State s;
306  s.decompRegion = decomp_->locateRegion(cs);
307  s.cosafeState = cosafe;
308  s.safeState = safe;
309  State *&ret = stateToPtr_[s];
310  if (ret == nullptr)
311  ret = new State(s);
312  return ret;
313 }
314 
316 {
317  State s;
318  s.decompRegion = nextRegion;
319  const World nextWorld = decomp_->worldAtRegion(nextRegion);
320  s.cosafeState = cosafety_->step(parent->cosafeState, nextWorld);
321  s.safeState = safety_->step(parent->safeState, nextWorld);
322  State *&ret = stateToPtr_[s];
323  if (ret == nullptr)
324  ret = new State(s);
325  return ret;
326 }
327 
329  const base::State *cs) const
330 {
331  return getState(parent, decomp_->locateRegion(cs));
332 }
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:69
friend std::ostream & operator<<(std::ostream &out, const State &s)
Helper function to print this State to a given output stream.
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