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
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
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
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
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
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
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
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
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 ...
Definition: Automaton.cpp:382
unsigned int numTransitions() const
Returns the number of transitions in this automaton.
Definition: Automaton.cpp:227
unsigned int addState(bool accepting=false)
Adds a new state to the automaton and returns an ID for it.
Definition: Automaton.cpp:162
bool isAccepting(unsigned int s) const
Returns whether a given state of the automaton is accepting.
Definition: Automaton.cpp:177
void print(std::ostream &out) const
Prints the automaton to a given output stream, in Graphviz dot format.
Definition: Automaton.cpp:240
void setStartState(unsigned int s)
Sets the start state of the automaton.
Definition: Automaton.cpp:182
unsigned int numProps() const
Returns the number of propositions used by this automaton.
Definition: Automaton.cpp:235
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.
Definition: Automaton.cpp:306
static AutomatonPtr AcceptingAutomaton(unsigned int numProps)
Returns a single-state automaton that accepts on all inputs.
Definition: Automaton.cpp:296
TransitionMap & getTransitions(unsigned int src)
Returns the outgoing transition map for a given automaton state.
Definition: Automaton.cpp:217
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...
Definition: Automaton.cpp:210
int getStartState() const
Returns the start state of the automaton. Returns -1 if no start state has been set.
Definition: Automaton.cpp:187
void addTransition(unsigned int src, const World &w, unsigned int dest)
Adds a given transition to the automaton.
Definition: Automaton.cpp:192
unsigned int distFromAccepting(unsigned int s) const
Returns the shortest number of transitions from a given state to an accepting state.
Definition: Automaton.cpp:260
unsigned int numStates() const
Returns the number of states in this automaton.
Definition: Automaton.cpp:222
void setAccepting(unsigned int s, bool a)
Sets the accepting status of a given state.
Definition: Automaton.cpp:172
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.
Definition: Automaton.cpp:340
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...
Definition: Automaton.cpp:198
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...
Definition: Automaton.cpp:363
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