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