37#include "ompl/control/planners/ltl/Automaton.h"
38#include "ompl/control/planners/ltl/World.h"
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>
48#include <boost/range/irange.hpp>
49#include <unordered_map>
50#include <unordered_set>
51#include <boost/dynamic_bitset.hpp>
59 const auto d = entries.find(w);
60 if (d != entries.end())
62 for (
const auto &entry : entries)
68 entries[w] = entry.second;
78 , accepting_(numStates_, false)
79 , transitions_(numStates_)
80 , distances_(numStates_,
std::numeric_limits<unsigned int>::max())
89 formula =
"! (" + formula +
")";
91 spot::formula f = spot::parse_formula(formula);
92 spot::translator trans;
95 trans.set_pref(spot::postprocessor::Deterministic);
97 trans.set_level(spot::postprocessor::High);
98 trans.set_type(spot::postprocessor::BA);
99 spot::twa_graph_ptr au = trans.run(f);
101 const auto &dict = au->get_dict();
102 unsigned int n = au->num_states();
103 for (
unsigned int s = 0; s < n; ++s)
105 for (
unsigned int s = 0; s < n; ++s)
112 for (
auto &t : au->out(s))
118 spot::minato_isop isop(t.cond);
119 bdd clause = isop.next();
120 if (clause == bddfalse)
124 while (clause != bddfalse)
127 while (clause != bddtrue)
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);
135 if (high == bddfalse)
137 edgeLabel[index] =
false;
138 clause = bdd_low(clause);
142 assert(bdd_low(clause) == bddfalse);
143 edgeLabel[index] =
true;
149 clause = isop.next();
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;
179 return accepting_[s];
195 map.entries[w] = dest;
200 int current = startState_;
201 for (
const auto &w : trace)
203 current = step(current, w);
214 return transitions_[state].eval(w);
219 return transitions_[src];
229 unsigned int ntrans = 0;
230 for (
const auto &transition : transitions_)
231 ntrans += transition.entries.size();
242 out <<
"digraph automaton {" << std::endl;
243 out <<
"rankdir=LR" << std::endl;
244 for (
unsigned int i = 0; i < numStates_; ++i)
246 out << i << R
"( [label=")" << i << R"(",shape=)";
247 out << (accepting_[i] ? "doublecircle" :
"circle") <<
"]" << std::endl;
249 for (
const auto &e : transitions_[i].entries)
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;
257 out << "}" << std::endl;
262 if (distances_[s] < std::numeric_limits<unsigned int>::max())
263 return distances_[s];
266 std::queue<unsigned int> q;
267 std::unordered_set<unsigned int> processed;
268 std::unordered_map<unsigned int, unsigned int> distance;
276 unsigned int current = q.front();
278 if (accepting_[current])
280 distances_[s] = distance[current];
281 return distance[current];
283 for (
const auto &e : transitions_[current].entries)
285 unsigned int neighbor = e.second;
286 if (processed.count(neighbor) > 0)
289 processed.insert(neighbor);
290 distance[neighbor] = distance[current] + 1;
293 return std::numeric_limits<unsigned int>::max();
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);
307 const std::vector<unsigned int> &covProps)
309 auto phi(std::make_shared<Automaton>(numProps, 1 << covProps.size()));
310 for (
unsigned int src = 0; src < phi->numStates(); ++src)
312 const boost::dynamic_bitset<> state(covProps.size(), src);
313 World loop(numProps);
315 for (
unsigned int p = 0; p < covProps.size(); ++p)
323 boost::dynamic_bitset<> target(state);
325 World nextProp(numProps);
326 nextProp[covProps[p]] =
true;
327 phi->addTransition(src, nextProp, target.to_ulong());
329 loop[covProps[p]] =
false;
333 phi->addTransition(src, loop, src);
335 phi->setAccepting(phi->numStates() - 1,
true);
336 phi->setStartState(0);
341 const std::vector<unsigned int> &seqProps)
343 auto seq(std::make_shared<Automaton>(numProps, seqProps.size() + 1));
344 for (
unsigned int state = 0; state < seqProps.size(); ++state)
347 World loop(numProps);
348 loop[seqProps[state]] =
false;
349 seq->addTransition(state, loop, state);
352 World progress(numProps);
353 progress[seqProps[state]] =
true;
354 seq->addTransition(state, progress, state + 1);
357 seq->addTransition(seqProps.size(),
World(numProps), seqProps.size());
358 seq->setAccepting(seqProps.size(),
true);
359 seq->setStartState(0);
364 const std::vector<unsigned int> &disjProps)
366 auto disj(std::make_shared<Automaton>(numProps, 2));
367 World loop(numProps);
368 for (
unsigned int disjProp : disjProps)
370 World satisfy(numProps);
371 satisfy[disjProp] =
true;
372 loop[disjProp] =
false;
373 disj->addTransition(0, satisfy, 1);
375 disj->addTransition(0, loop, 0);
376 disj->addTransition(1,
World(numProps), 1);
377 disj->setAccepting(1,
true);
378 disj->setStartState(0);
383 const std::vector<unsigned int> &avoidProps)
386 AutomatonPtr avoid = DisjunctionAutomaton(numProps, avoidProps);
387 avoid->setAccepting(0,
true);
388 avoid->setAccepting(1,
false);
394 const boost::integer_range<unsigned int> props = boost::irange(0u, numProps);
395 return CoverageAutomaton(numProps, std::vector<unsigned int>(props.begin(), props.end()));
401 const boost::integer_range<unsigned int> props = boost::irange(0u, numProps);
402 return SequenceAutomaton(numProps, std::vector<unsigned int>(props.begin(), props.end()));
407 const boost::integer_range<unsigned int> props = boost::irange(0u, numProps);
408 return DisjunctionAutomaton(numProps, std::vector<unsigned int>(props.begin(), props.end()));
A shared pointer wrapper for ompl::control::Automaton.
A class to represent a deterministic finite automaton, each edge of which corresponds to a World....
Automaton(unsigned int numProps, unsigned int numStates=0)
Creates an automaton with a given number of propositions and states.
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...
bool satisfies(const World &w) const
Returns whether this World propositionally satisfies a given World w. Specifically,...
std::string formula() const
Returns a formatted string representation of this World, as a conjunction of literals.
Each automaton state has a transition map, which maps from a World to another automaton state....
int eval(const World &w) const
Returns the automaton state corresponding to a given World in this transition map....