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