Loading...
Searching...
No Matches
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
68std::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
76namespace 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
115ompl::control::ProductGraph::~ProductGraph()
116{
117 clear();
118}
119
120const ompl::control::PropositionalDecompositionPtr &ompl::control::ProductGraph::getDecomp() const
121{
122 return decomp_;
123}
124
125const ompl::control::AutomatonPtr &ompl::control::ProductGraph::getCosafetyAutom() const
126{
127 return cosafety_;
128}
129
130const ompl::control::AutomatonPtr &ompl::control::ProductGraph::getSafetyAutom() const
131{
132 return safety_;
133}
134
135std::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_));
140OMPL_PUSH_DISABLE_GCC_WARNING(-Wmaybe-uninitialized)
141 EdgeIter ei, eend;
142OMPL_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
201void 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
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}
Definition of an abstract state.
Definition State.h:50
A shared pointer wrapper for ompl::control::Automaton.
A class to represent a deterministic finite automaton, each edge of which corresponds to a World....
Definition Automaton.h:71
A State of a ProductGraph represents a vertex in the graph-based Cartesian product represented by the...
int getDecompRegion() const
Returns this State's PropositionalDecomposition region component.
bool operator==(const State &s) const
Returns whether this State is equivalent to a given State, by comparing their PropositionalDecomposit...
bool isValid() const
Returns whether this State is valid. A State is valid if and only if none of its Automaton states are...
int getCosafeState() const
Returns this State's co-safe Automaton state component.
friend std::ostream & operator<<(std::ostream &out, const State &s)
Helper function to print this State to a given output stream.
State()=default
Creates a State without any assigned PropositionalDecomposition region or Automaton states....
int getSafeState() const
Returns this State's safe Automaton state component.
bool isSolution(const State *s) const
Returns whether the given State is an accepting State in this ProductGraph. We call a State accepting...
State * getStartState() const
Returns the initial State of this ProductGraph.
State * getState(const base::State *cs) const
Returns a ProductGraph State with initial co-safety and safety Automaton states, and the Propositiona...
const PropositionalDecompositionPtr & getDecomp() const
Returns the PropositionalDecomposition 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...
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...
void clear()
Clears all memory belonging to this ProductGraph.
const AutomatonPtr & getSafetyAutom() const
Returns the safe Automaton contained within this ProductGraph.
double getRegionVolume(const State *s)
Helper method to return the volume of the PropositionalDecomposition region corresponding to the give...
const AutomatonPtr & getCosafetyAutom() const
Returns the co-safe Automaton contained within this ProductGraph.
ProductGraph(PropositionalDecompositionPtr decomp, AutomatonPtr cosafetyAut, AutomatonPtr safetyAut)
Initializes a ProductGraph with a given PropositionalDecomposition, co-safe Automaton,...
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....
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...
A shared pointer wrapper for ompl::control::PropositionalDecomposition.
A class to represent an assignment of boolean values to propositions. A World can be partially restri...
Definition World.h:72
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition Console.h:68
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition Console.h:64
This namespace contains sampling based planning routines used by planning under differential constrai...
Definition Control.h:45
Main namespace. Contains everything in this library.
STL namespace.