Automaton.cpp
1 #include "ompl/control/planners/ltl/Automaton.h"
2 #include "ompl/control/planners/ltl/World.h"
3 #include <boost/range/irange.hpp>
4 #include <boost/lexical_cast.hpp>
5 #include <boost/unordered_map.hpp>
6 #include <boost/unordered_set.hpp>
7 #include <boost/dynamic_bitset.hpp>
8 #include <ostream>
9 #include <limits>
10 #include <queue>
11 #include <vector>
12 
14 {
15  typedef boost::unordered_map<World, unsigned int>::const_iterator DestIter;
16  DestIter d = entries.find(w);
17  if (d != entries.end())
18  return d->second;
19  for (d = entries.begin(); d != entries.end(); ++d)
20  {
21  if (w.satisfies(d->first))
22  {
23  //Since w satisfies another world that leads to d->second,
24  //we can add an edge directly from w to d->second.
25  entries[w] = d->second;
26  return d->second;
27  }
28  }
29  return -1;
30 }
31 
33  numProps_(numProps),
34  numStates_(numStates),
35  startState_(-1),
36  accepting_(numStates_, false),
37  transitions_(numStates_),
38  distances_(numStates_, std::numeric_limits<unsigned int>::max())
39 {
40 }
41 
42 unsigned int ompl::control::Automaton::addState(bool accepting)
43 {
44  ++numStates_;
45  accepting_.resize(numStates_);
46  accepting_[numStates_-1] = accepting;
47  transitions_.resize(numStates_);
48  return numStates_-1;
49 }
50 
51 void ompl::control::Automaton::setAccepting(unsigned int s, bool a)
52 {
53  accepting_[s] = a;
54 }
55 
56 bool ompl::control::Automaton::isAccepting(unsigned int s) const
57 {
58  return accepting_[s];
59 }
60 
62 {
63  startState_ = s;
64 }
65 
67 {
68  return startState_;
69 }
70 
72  unsigned int src,
73  const World& w,
74  unsigned int dest)
75 {
76  TransitionMap& map = transitions_[src];
77  map.entries[w] = dest;
78 }
79 
80 bool ompl::control::Automaton::run(const std::vector<World>& trace) const
81 {
82  int current = startState_;
83  for (std::vector<World>::const_iterator w = trace.begin(); w != trace.end(); ++w)
84  {
85  current = step(current, *w);
86  if (current == -1)
87  return false;
88  }
89  return true;
90 }
91 
92 int ompl::control::Automaton::step(int state, const World& w) const
93 {
94  if (state == -1)
95  return -1;
96  return transitions_[state].eval(w);
97 }
98 
100 {
101  return transitions_[src];
102 }
103 
104 unsigned int ompl::control::Automaton::numStates(void) const
105 {
106  return numStates_;
107 }
108 
110 {
111  unsigned int ntrans = 0;
112  typedef std::vector<TransitionMap>::const_iterator TransIter;
113  for (TransIter i = transitions_.begin(); i != transitions_.end(); ++i)
114  ntrans += i->entries.size();
115  return ntrans;
116 }
117 
118 unsigned int ompl::control::Automaton::numProps(void) const
119 {
120  return numProps_;
121 }
122 
123 void ompl::control::Automaton::print(std::ostream& out) const
124 {
125  out << "digraph automaton {" << std::endl;
126  out << "rankdir=LR" << std::endl;
127  for (unsigned int i = 0; i < numStates_; ++i)
128  {
129  out << i << " [label=\"" << i << "\",shape=";
130  out << (accepting_[i] ? "doublecircle" : "circle") << "]" << std::endl;
131 
132  const TransitionMap& map = transitions_[i];
133  boost::unordered_map<World, unsigned int>::const_iterator e;
134  for (e = map.entries.begin(); e != map.entries.end(); ++e)
135  {
136  const World& w = e->first;
137  unsigned int dest = e->second;
138  const std::string formula = w.formula();
139  out << i << " -> " << dest << " [label=\"" << formula << "\"]" << std::endl;
140  }
141  }
142  out << "}" << std::endl;
143 }
144 
145 unsigned int ompl::control::Automaton::distFromAccepting(unsigned int s, unsigned int maxDist) const
146 {
147  if (distances_[s] < std::numeric_limits<unsigned int>::max())
148  return distances_[s];
149  if (accepting_[s])
150  return 0;
151  std::queue<unsigned int> q;
152  boost::unordered_set<unsigned int> processed;
153  boost::unordered_map<unsigned int, unsigned int> distance;
154 
155  q.push(s);
156  distance[s] = 0;
157  processed.insert(s);
158 
159  while (!q.empty())
160  {
161  unsigned int current = q.front();
162  q.pop();
163  if (accepting_[current])
164  {
165  distances_[s] = distance[current];
166  return distance[current];
167  }
168  const TransitionMap& map = transitions_[current];
169  boost::unordered_map<World, unsigned int>::const_iterator e;
170  for (e = map.entries.begin(); e != map.entries.end(); ++e)
171  {
172  unsigned int neighbor = e->second;
173  if (processed.count(neighbor) > 0)
174  continue;
175  q.push(neighbor);
176  processed.insert(neighbor);
177  distance[neighbor] = distance[current]+1;
178  }
179  }
180  return std::numeric_limits<unsigned int>::max();
181 }
182 
184 {
185  AutomatonPtr phi(new Automaton(numProps, 1));
186  World trivial(numProps);
187  phi->addTransition(0, trivial, 0);
188  phi->setStartState(0);
189  phi->setAccepting(0, true);
190  return phi;
191 }
192 
193 ompl::control::AutomatonPtr ompl::control::Automaton::CoverageAutomaton(unsigned int numProps, const std::vector<unsigned int>& covProps)
194 {
195  AutomatonPtr phi(new Automaton(numProps, 1<<covProps.size()));
196  for (unsigned int src = 0; src < phi->numStates(); ++src)
197  {
198  const boost::dynamic_bitset<> state(covProps.size(), src);
199  World loop(numProps);
200  //each value of p is an index of a proposition in covProps
201  for (unsigned int p = 0; p < covProps.size(); ++p)
202  {
203  //if proposition covProps[p] has already been covered at state src, skip it
204  if (state[p])
205  continue;
206  //for each proposition covProps[p] that has not yet been
207  //covered at state src, construct a transition from src to (src|p)
208  //on formula (covProps[p]==true)
209  boost::dynamic_bitset<> target(state);
210  target[p] = true;
211  World nextProp(numProps);
212  nextProp[covProps[p]] = true;
213  phi->addTransition(src, nextProp, target.to_ulong());
214  //also build a loop from src to src on formula with conjunct (covProps[p]==false)
215  loop[covProps[p]] = false;
216  }
217  //now we add a loop from src to src on conjunction of (covProps[p]==false)
218  //for every p such that the pth bit of src is 1
219  phi->addTransition(src, loop, src);
220  }
221  phi->setAccepting(phi->numStates()-1, true);
222  phi->setStartState(0);
223  return phi;
224 }
225 
226 ompl::control::AutomatonPtr ompl::control::Automaton::SequenceAutomaton(unsigned int numProps, const std::vector<unsigned int>& seqProps)
227 {
228  AutomatonPtr seq(new Automaton(numProps, seqProps.size()+1));
229  for (unsigned int state = 0; state < seqProps.size(); ++state)
230  {
231  // loop when next proposition in sequence is not satisfied
232  World loop(numProps);
233  loop[seqProps[state]] = false;
234  seq->addTransition(state, loop, state);
235 
236  // progress forward when next proposition in sequence is satisfied
237  World progress(numProps);
238  progress[seqProps[state]] = true;
239  seq->addTransition(state, progress, state+1);
240  }
241  //loop on all input when in accepting state
242  seq->addTransition(seqProps.size(), World(numProps), seqProps.size());
243  seq->setAccepting(seqProps.size(), true);
244  seq->setStartState(0);
245  return seq;
246 }
247 
248 ompl::control::AutomatonPtr ompl::control::Automaton::DisjunctionAutomaton(unsigned int numProps, const std::vector<unsigned int>& disjProps)
249 {
250  AutomatonPtr disj(new Automaton(numProps, 2));
251  World loop(numProps);
252  for (std::vector<unsigned int>::const_iterator p = disjProps.begin(); p != disjProps.end(); ++p)
253  {
254  World satisfy(numProps);
255  satisfy[*p] = true;
256  loop[*p] = false;
257  disj->addTransition(0, satisfy, 1);
258  }
259  disj->addTransition(0, loop, 0);
260  disj->addTransition(1, World(numProps), 1);
261  disj->setAccepting(1, true);
262  disj->setStartState(0);
263  return disj;
264 }
265 
266 ompl::control::AutomatonPtr ompl::control::Automaton::AvoidanceAutomaton(unsigned int numProps, const std::vector<unsigned int>& avoidProps)
267 {
268  /* An avoidance automaton is simply a disjunction automaton with its acceptance condition flipped. */
269  AutomatonPtr avoid = DisjunctionAutomaton(numProps, avoidProps);
270  avoid->setAccepting(0, true);
271  avoid->setAccepting(1, false);
272  return avoid;
273 }
274 
276 {
277  const boost::integer_range<unsigned int> props = boost::irange(0u,numProps);
278  return CoverageAutomaton(numProps, std::vector<unsigned int>(props.begin(), props.end()));
279 }
280 
282 {
283  const boost::integer_range<unsigned int> props = boost::irange(0u,numProps);
284  return SequenceAutomaton(numProps, std::vector<unsigned int>(props.begin(), props.end()));
285 }
286 
288 {
289  const boost::integer_range<unsigned int> props = boost::irange(0u,numProps);
290  return DisjunctionAutomaton(numProps, std::vector<unsigned int>(props.begin(), props.end()));
291 }
void setAccepting(unsigned int s, bool a)
Sets the accepting status of a given state.
Definition: Automaton.cpp:51
A class to represent an assignment of boolean values to propositions. A World can be partially restri...
Definition: World.h:51
STL namespace.
unsigned int numTransitions(void) const
Returns the number of transitions in this automaton.
Definition: Automaton.cpp:109
unsigned int numProps(void) const
Returns the number of propositions used by this automaton.
Definition: Automaton.cpp:118
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:92
Automaton(unsigned int numProps, unsigned int numStates=0)
Creates an automaton with a given number of propositions and states.
Definition: Automaton.cpp:32
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:226
void setStartState(unsigned int s)
Sets the start state of the automaton.
Definition: Automaton.cpp:61
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:248
bool isAccepting(unsigned int s) const
Returns whether a given state of the automaton is accepting.
Definition: Automaton.cpp:56
Each automaton state has a transition map, which maps from a World to another automaton state...
Definition: Automaton.h:76
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:30
int getStartState(void) const
Returns the start state of the automaton. Returns -1 if no start state has been set.
Definition: Automaton.cpp:66
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:80
void print(std::ostream &out) const
Prints the automaton to a given output stream, in Graphviz dot format.
Definition: Automaton.cpp:123
unsigned int addState(bool accepting=false)
Adds a new state to the automaton and returns an ID for it.
Definition: Automaton.cpp:42
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:13
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:145
void addTransition(unsigned int src, const World &w, unsigned int dest)
Adds a given transition to the automaton.
Definition: Automaton.cpp:71
A boost 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:183
unsigned int numStates(void) const
Returns the number of states in this automaton.
Definition: Automaton.cpp:104
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:193
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:266
A class to represent a deterministic finite automaton, each edge of which corresponds to a World...
Definition: Automaton.h:69
std::string formula(void) const
Returns a formatted string representation of this World, as a conjunction of literals.
Definition: World.cpp:42
TransitionMap & getTransitions(unsigned int src)
Returns the outgoing transition map for a given automaton state.
Definition: Automaton.cpp:99