Loading...
Searching...
No Matches
Automaton.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, Keliang He */
36
37#include "ompl/control/planners/ltl/Automaton.h"
38#include "ompl/control/planners/ltl/World.h"
39#if OMPL_HAVE_SPOT
40#include <spot/tl/parse.hh>
41#include <spot/tl/print.hh>
42#include <spot/twaalgos/translate.hh>
43#include <spot/twa/bddprint.hh>
44#include <spot/misc/minato.hh>
45#include <spot/twa/formula2bdd.hh>
46#include <typeinfo>
47#endif
48#include <boost/range/irange.hpp>
49#include <unordered_map>
50#include <unordered_set>
51#include <boost/dynamic_bitset.hpp>
52#include <ostream>
53#include <limits>
54#include <queue>
55#include <vector>
56
58{
59 const auto d = entries.find(w);
60 if (d != entries.end())
61 return d->second;
62 for (const auto &entry : entries)
63 {
64 if (w.satisfies(entry.first))
65 {
66 // Since w satisfies another world that leads to d->second,
67 // we can add an edge directly from w to d->second.
68 entries[w] = entry.second;
69 return entry.second;
70 }
71 }
72 return -1;
73}
74
76 : numProps_(numProps)
77 , numStates_(numStates)
78 , accepting_(numStates_, false)
79 , transitions_(numStates_)
80 , distances_(numStates_, std::numeric_limits<unsigned int>::max())
81{
82}
83
84#if OMPL_HAVE_SPOT
85ompl::control::Automaton::Automaton(unsigned numProps, std::string formula, bool isCosafe)
86 : Automaton::Automaton(numProps)
87{
88 if (!isCosafe)
89 formula = "! (" + formula + ")";
90
91 spot::formula f = spot::parse_formula(formula);
92 spot::translator trans;
93
94 // We want deterministic output (dfa)
95 trans.set_pref(spot::postprocessor::Deterministic);
96 // Apply all optimizations - will be slowest
97 trans.set_level(spot::postprocessor::High);
98 trans.set_type(spot::postprocessor::BA);
99 spot::twa_graph_ptr au = trans.run(f);
100
101 const auto &dict = au->get_dict();
102 unsigned int n = au->num_states();
103 for (unsigned int s = 0; s < n; ++s)
104 addState(false);
105 for (unsigned int s = 0; s < n; ++s)
106 {
107 // The out(s) method returns a fake container that can be
108 // iterated over as if the contents was the edges going
109 // out of s. Each of these edge is a quadruplet
110 // (src,dst,cond,acc). Note that because this returns
111 // a reference, the edge can also be modified.
112 for (auto &t : au->out(s))
113 {
114 if (t.acc)
115 setAccepting(s, true);
116
117 // Parse the formula
118 spot::minato_isop isop(t.cond);
119 bdd clause = isop.next();
120 if (clause == bddfalse)
121 addTransition(s, numProps, t.dst);
122 else
123 {
124 while (clause != bddfalse)
125 {
127 while (clause != bddtrue)
128 {
129 int var = bdd_var(clause);
130 const spot::bdd_dict::bdd_info &i = dict->bdd_map[var];
131 assert(i.type == spot::bdd_dict::var);
132 auto index = std::stoul(i.f.ap_name().substr(1));
133 bdd high = bdd_high(clause);
134 assert(index < numProps);
135 if (high == bddfalse)
136 {
137 edgeLabel[index] = false;
138 clause = bdd_low(clause);
139 }
140 else
141 {
142 assert(bdd_low(clause) == bddfalse);
143 edgeLabel[index] = true;
144 clause = high;
145 }
146 }
147 addTransition(s, edgeLabel, t.dst);
148
149 clause = isop.next();
150 }
151 }
152 }
153 }
154
155 setStartState(au->get_init_state_number());
156
157 if (!isCosafe)
158 accepting_.flip();
159}
160#endif
161
162unsigned int ompl::control::Automaton::addState(bool accepting)
163{
164 ++numStates_;
165 accepting_.resize(numStates_);
166 accepting_[numStates_ - 1] = accepting;
167 transitions_.resize(numStates_);
168 distances_.resize(numStates_, std::numeric_limits<unsigned int>::max());
169 return numStates_ - 1;
170}
171
172void ompl::control::Automaton::setAccepting(unsigned int s, bool a)
173{
174 accepting_[s] = a;
175}
176
178{
179 return accepting_[s];
180}
181
183{
184 startState_ = s;
185}
186
188{
189 return startState_;
190}
191
192void ompl::control::Automaton::addTransition(unsigned int src, const World &w, unsigned int dest)
193{
194 TransitionMap &map = transitions_[src];
195 map.entries[w] = dest;
196}
197
198bool ompl::control::Automaton::run(const std::vector<World> &trace) const
199{
200 int current = startState_;
201 for (const auto &w : trace)
202 {
203 current = step(current, w);
204 if (current == -1)
205 return false;
206 }
207 return true;
208}
209
210int ompl::control::Automaton::step(int state, const World &w) const
211{
212 if (state == -1)
213 return -1;
214 return transitions_[state].eval(w);
215}
216
218{
219 return transitions_[src];
220}
221
223{
224 return numStates_;
225}
226
228{
229 unsigned int ntrans = 0;
230 for (const auto &transition : transitions_)
231 ntrans += transition.entries.size();
232 return ntrans;
233}
234
236{
237 return numProps_;
238}
239
240void ompl::control::Automaton::print(std::ostream &out) const
241{
242 out << "digraph automaton {" << std::endl;
243 out << "rankdir=LR" << std::endl;
244 for (unsigned int i = 0; i < numStates_; ++i)
245 {
246 out << i << R"( [label=")" << i << R"(",shape=)";
247 out << (accepting_[i] ? "doublecircle" : "circle") << "]" << std::endl;
248
249 for (const auto &e : transitions_[i].entries)
250 {
251 const World &w = e.first;
252 unsigned int dest = e.second;
253 const std::string formula = w.formula();
254 out << i << " -> " << dest << R"( [label=")" << formula << R"("])" << std::endl;
255 }
256 }
257 out << "}" << std::endl;
258}
259
260unsigned int ompl::control::Automaton::distFromAccepting(unsigned int s) const
261{
262 if (distances_[s] < std::numeric_limits<unsigned int>::max())
263 return distances_[s];
264 if (accepting_[s])
265 return 0;
266 std::queue<unsigned int> q;
267 std::unordered_set<unsigned int> processed;
268 std::unordered_map<unsigned int, unsigned int> distance;
269
270 q.push(s);
271 distance[s] = 0;
272 processed.insert(s);
273
274 while (!q.empty())
275 {
276 unsigned int current = q.front();
277 q.pop();
278 if (accepting_[current])
279 {
280 distances_[s] = distance[current];
281 return distance[current];
282 }
283 for (const auto &e : transitions_[current].entries)
284 {
285 unsigned int neighbor = e.second;
286 if (processed.count(neighbor) > 0)
287 continue;
288 q.push(neighbor);
289 processed.insert(neighbor);
290 distance[neighbor] = distance[current] + 1;
291 }
292 }
293 return std::numeric_limits<unsigned int>::max();
294}
295
296ompl::control::AutomatonPtr ompl::control::Automaton::AcceptingAutomaton(unsigned int numProps)
297{
298 auto phi(std::make_shared<Automaton>(numProps, 1));
299 World trivial(numProps);
300 phi->addTransition(0, trivial, 0);
301 phi->setStartState(0);
302 phi->setAccepting(0, true);
303 return phi;
304}
305
306ompl::control::AutomatonPtr ompl::control::Automaton::CoverageAutomaton(unsigned int numProps,
307 const std::vector<unsigned int> &covProps)
308{
309 auto phi(std::make_shared<Automaton>(numProps, 1 << covProps.size()));
310 for (unsigned int src = 0; src < phi->numStates(); ++src)
311 {
312 const boost::dynamic_bitset<> state(covProps.size(), src);
313 World loop(numProps);
314 // each value of p is an index of a proposition in covProps
315 for (unsigned int p = 0; p < covProps.size(); ++p)
316 {
317 // if proposition covProps[p] has already been covered at state src, skip it
318 if (state[p])
319 continue;
320 // for each proposition covProps[p] that has not yet been
321 // covered at state src, construct a transition from src to (src|p)
322 // on formula (covProps[p]==true)
323 boost::dynamic_bitset<> target(state);
324 target[p] = true;
325 World nextProp(numProps);
326 nextProp[covProps[p]] = true;
327 phi->addTransition(src, nextProp, target.to_ulong());
328 // also build a loop from src to src on formula with conjunct (covProps[p]==false)
329 loop[covProps[p]] = false;
330 }
331 // now we add a loop from src to src on conjunction of (covProps[p]==false)
332 // for every p such that the pth bit of src is 1
333 phi->addTransition(src, loop, src);
334 }
335 phi->setAccepting(phi->numStates() - 1, true);
336 phi->setStartState(0);
337 return phi;
338}
339
340ompl::control::AutomatonPtr ompl::control::Automaton::SequenceAutomaton(unsigned int numProps,
341 const std::vector<unsigned int> &seqProps)
342{
343 auto seq(std::make_shared<Automaton>(numProps, seqProps.size() + 1));
344 for (unsigned int state = 0; state < seqProps.size(); ++state)
345 {
346 // loop when next proposition in sequence is not satisfied
347 World loop(numProps);
348 loop[seqProps[state]] = false;
349 seq->addTransition(state, loop, state);
350
351 // progress forward when next proposition in sequence is satisfied
352 World progress(numProps);
353 progress[seqProps[state]] = true;
354 seq->addTransition(state, progress, state + 1);
355 }
356 // loop on all input when in accepting state
357 seq->addTransition(seqProps.size(), World(numProps), seqProps.size());
358 seq->setAccepting(seqProps.size(), true);
359 seq->setStartState(0);
360 return seq;
361}
362
363ompl::control::AutomatonPtr ompl::control::Automaton::DisjunctionAutomaton(unsigned int numProps,
364 const std::vector<unsigned int> &disjProps)
365{
366 auto disj(std::make_shared<Automaton>(numProps, 2));
367 World loop(numProps);
368 for (unsigned int disjProp : disjProps)
369 {
370 World satisfy(numProps);
371 satisfy[disjProp] = true;
372 loop[disjProp] = false;
373 disj->addTransition(0, satisfy, 1);
374 }
375 disj->addTransition(0, loop, 0);
376 disj->addTransition(1, World(numProps), 1);
377 disj->setAccepting(1, true);
378 disj->setStartState(0);
379 return disj;
380}
381
382ompl::control::AutomatonPtr ompl::control::Automaton::AvoidanceAutomaton(unsigned int numProps,
383 const std::vector<unsigned int> &avoidProps)
384{
385 /* An avoidance automaton is simply a disjunction automaton with its acceptance condition flipped. */
386 AutomatonPtr avoid = DisjunctionAutomaton(numProps, avoidProps);
387 avoid->setAccepting(0, true);
388 avoid->setAccepting(1, false);
389 return avoid;
390}
391
392ompl::control::AutomatonPtr ompl::control::Automaton::CoverageAutomaton(unsigned int numProps)
393{
394 const boost::integer_range<unsigned int> props = boost::irange(0u, numProps);
395 return CoverageAutomaton(numProps, std::vector<unsigned int>(props.begin(), props.end()));
396}
397
398
399ompl::control::AutomatonPtr ompl::control::Automaton::SequenceAutomaton(unsigned int numProps)
400{
401 const boost::integer_range<unsigned int> props = boost::irange(0u, numProps);
402 return SequenceAutomaton(numProps, std::vector<unsigned int>(props.begin(), props.end()));
403}
404
405ompl::control::AutomatonPtr ompl::control::Automaton::DisjunctionAutomaton(unsigned int numProps)
406{
407 const boost::integer_range<unsigned int> props = boost::irange(0u, numProps);
408 return DisjunctionAutomaton(numProps, std::vector<unsigned int>(props.begin(), props.end()));
409}
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
Automaton(unsigned int numProps, unsigned int numStates=0)
Creates an automaton with a given number of propositions and states.
Definition Automaton.cpp:75
static AutomatonPtr AvoidanceAutomaton(unsigned int numProps, const std::vector< unsigned int > &avoidProps)
Returns an avoidance automaton, which rejects when any one of the given list of propositions becomes ...
unsigned int numTransitions() const
Returns the number of transitions in this automaton.
unsigned int addState(bool accepting=false)
Adds a new state to the automaton and returns an ID for it.
bool isAccepting(unsigned int s) const
Returns whether a given state of the automaton is accepting.
void print(std::ostream &out) const
Prints the automaton to a given output stream, in Graphviz dot format.
void setStartState(unsigned int s)
Sets the start state of the automaton.
unsigned int numProps() const
Returns the number of propositions used by this automaton.
static AutomatonPtr CoverageAutomaton(unsigned int numProps, const std::vector< unsigned int > &covProps)
Helper function to return a coverage automaton. Assumes all propositions are mutually exclusive.
static AutomatonPtr AcceptingAutomaton(unsigned int numProps)
Returns a single-state automaton that accepts on all inputs.
TransitionMap & getTransitions(unsigned int src)
Returns the outgoing transition map for a given automaton state.
int step(int state, const World &w) const
Runs the automaton for one step from the given state, using the values of propositions from a given W...
int getStartState() const
Returns the start state of the automaton. Returns -1 if no start state has been set.
void addTransition(unsigned int src, const World &w, unsigned int dest)
Adds a given transition to the automaton.
unsigned int distFromAccepting(unsigned int s) const
Returns the shortest number of transitions from a given state to an accepting state.
unsigned int numStates() const
Returns the number of states in this automaton.
void setAccepting(unsigned int s, bool a)
Sets the accepting status of a given state.
static AutomatonPtr SequenceAutomaton(unsigned int numProps, const std::vector< unsigned int > &seqProps)
Helper function to return a sequence automaton. Assumes all propositions are mutually exclusive.
bool run(const std::vector< World > &trace) const
Runs the automaton from its start state, using the values of propositions from a given sequence of Wo...
static AutomatonPtr DisjunctionAutomaton(unsigned int numProps, const std::vector< unsigned int > &disjProps)
Helper function to return a disjunction automaton, which accepts when one of the given propositions b...
A class to represent an assignment of boolean values to propositions. A World can be partially restri...
Definition World.h:72
bool satisfies(const World &w) const
Returns whether this World propositionally satisfies a given World w. Specifically,...
Definition World.cpp:65
std::string formula() const
Returns a formatted string representation of this World, as a conjunction of literals.
Definition World.cpp:77
STL namespace.
Each automaton state has a transition map, which maps from a World to another automaton state....
Definition Automaton.h:78
int eval(const World &w) const
Returns the automaton state corresponding to a given World in this transition map....
Definition Automaton.cpp:57