ompl/control/planners/ltl/src/Automaton.cpp
00001 #include "ompl/control/planners/ltl/Automaton.h"
00002 #include "ompl/control/planners/ltl/World.h"
00003 #include <boost/range/irange.hpp>
00004 #include <boost/lexical_cast.hpp>
00005 #include <boost/unordered_map.hpp>
00006 #include <boost/unordered_set.hpp>
00007 #include <boost/dynamic_bitset.hpp>
00008 #include <ostream>
00009 #include <limits>
00010 #include <queue>
00011 #include <vector>
00012 
00013 int ompl::control::Automaton::TransitionMap::eval(const World& w) const
00014 {
00015     typedef boost::unordered_map<World, unsigned int>::const_iterator DestIter;
00016     DestIter d = entries.find(w);
00017     if (d != entries.end())
00018         return d->second;
00019     for (d = entries.begin(); d != entries.end(); ++d)
00020     {
00021         if (w.satisfies(d->first))
00022         {
00023             //Since w satisfies another world that leads to d->second,
00024             //we can add an edge directly from w to d->second.
00025             entries[w] = d->second;
00026             return d->second;
00027         }
00028     }
00029     return -1;
00030 }
00031 
00032 ompl::control::Automaton::Automaton(unsigned int numProps, unsigned int numStates) :
00033     numProps_(numProps),
00034     numStates_(numStates),
00035     startState_(-1),
00036     accepting_(numStates_, false),
00037     transitions_(numStates_),
00038     distances_(numStates_, std::numeric_limits<unsigned int>::max())
00039 {
00040 }
00041 
00042 unsigned int ompl::control::Automaton::addState(bool accepting)
00043 {
00044     ++numStates_;
00045     accepting_.resize(numStates_);
00046     accepting_[numStates_-1] = accepting;
00047     transitions_.resize(numStates_);
00048     return numStates_-1;
00049 }
00050 
00051 void ompl::control::Automaton::setAccepting(unsigned int s, bool a)
00052 {
00053     accepting_[s] = a;
00054 }
00055 
00056 bool ompl::control::Automaton::isAccepting(unsigned int s) const
00057 {
00058     return accepting_[s];
00059 }
00060 
00061 void ompl::control::Automaton::setStartState(unsigned int s)
00062 {
00063     startState_ = s;
00064 }
00065 
00066 int ompl::control::Automaton::getStartState(void) const
00067 {
00068     return startState_;
00069 }
00070 
00071 void ompl::control::Automaton::addTransition(
00072     unsigned int src, 
00073     const World& w,
00074     unsigned int dest)
00075 {
00076     TransitionMap& map = transitions_[src];
00077     map.entries[w] = dest;
00078 }
00079 
00080 bool ompl::control::Automaton::run(const std::vector<World>& trace) const
00081 {
00082     int current = startState_;
00083     for (std::vector<World>::const_iterator w = trace.begin(); w != trace.end(); ++w)
00084     {
00085         current = step(current, *w);
00086         if (current == -1)
00087             return false;
00088     }
00089     return true;
00090 }
00091 
00092 int ompl::control::Automaton::step(int state, const World& w) const
00093 {
00094     if (state == -1)
00095         return -1;
00096     return transitions_[state].eval(w);
00097 }
00098 
00099 ompl::control::Automaton::TransitionMap& ompl::control::Automaton::getTransitions(unsigned int src)
00100 {
00101     return transitions_[src];
00102 }
00103 
00104 unsigned int ompl::control::Automaton::numStates(void) const
00105 {
00106     return numStates_;
00107 }
00108 
00109 unsigned int ompl::control::Automaton::numTransitions(void) const
00110 {
00111     unsigned int ntrans = 0;
00112     typedef std::vector<TransitionMap>::const_iterator TransIter;
00113     for (TransIter i = transitions_.begin(); i != transitions_.end(); ++i)
00114         ntrans += i->entries.size();
00115     return ntrans;
00116 }
00117 
00118 unsigned int ompl::control::Automaton::numProps(void) const
00119 {
00120     return numProps_;
00121 }
00122 
00123 void ompl::control::Automaton::print(std::ostream& out) const
00124 {
00125     out << "digraph automaton {" << std::endl;
00126     out << "rankdir=LR" << std::endl;
00127     for (unsigned int i = 0; i < numStates_; ++i)
00128     {
00129         out << i << " [label=\"" << i << "\",shape=";
00130         out << (accepting_[i] ? "doublecircle" : "circle") << "]" << std::endl;
00131 
00132         const TransitionMap& map = transitions_[i];
00133         boost::unordered_map<World, unsigned int>::const_iterator e;
00134         for (e = map.entries.begin(); e != map.entries.end(); ++e)
00135         {
00136             const World& w = e->first;
00137             unsigned int dest = e->second;
00138             const std::string formula = w.formula();
00139             out << i << " -> " << dest << " [label=\"" << formula << "\"]" << std::endl;
00140         }
00141     }
00142     out << "}" << std::endl;
00143 }
00144 
00145 unsigned int ompl::control::Automaton::distFromAccepting(unsigned int s, unsigned int maxDist) const
00146 {
00147     if (distances_[s] < std::numeric_limits<unsigned int>::max())
00148         return distances_[s];
00149     if (accepting_[s])
00150         return 0;
00151     std::queue<unsigned int> q;
00152     boost::unordered_set<unsigned int> processed;
00153     boost::unordered_map<unsigned int, unsigned int> distance;
00154 
00155     q.push(s);
00156     distance[s] = 0;
00157     processed.insert(s);
00158 
00159     while (!q.empty())
00160     {
00161         unsigned int current = q.front();
00162         q.pop();
00163         if (accepting_[current])
00164         {
00165             distances_[s] = distance[current];
00166             return distance[current];
00167         }
00168         const TransitionMap& map = transitions_[current];
00169         boost::unordered_map<World, unsigned int>::const_iterator e;
00170         for (e = map.entries.begin(); e != map.entries.end(); ++e)
00171         {
00172             unsigned int neighbor = e->second;
00173             if (processed.count(neighbor) > 0)
00174                 continue;
00175             q.push(neighbor);
00176             processed.insert(neighbor);
00177             distance[neighbor] = distance[current]+1;
00178         }
00179     }
00180     return std::numeric_limits<unsigned int>::max();
00181 }
00182 
00183 ompl::control::AutomatonPtr ompl::control::Automaton::AcceptingAutomaton(unsigned int numProps)
00184 {
00185     AutomatonPtr phi(new Automaton(numProps, 1));
00186     World trivial(numProps);
00187     phi->addTransition(0, trivial, 0);
00188     phi->setStartState(0);
00189     phi->setAccepting(0, true);
00190     return phi;
00191 }
00192 
00193 ompl::control::AutomatonPtr ompl::control::Automaton::CoverageAutomaton(unsigned int numProps, const std::vector<unsigned int>& covProps)
00194 {
00195     AutomatonPtr phi(new Automaton(numProps, 1<<covProps.size()));
00196     for (unsigned int src = 0; src < phi->numStates(); ++src)
00197     {
00198         const boost::dynamic_bitset<> state(covProps.size(), src);
00199         World loop(numProps);
00200         //each value of p is an index of a proposition in covProps
00201         for (unsigned int p = 0; p < covProps.size(); ++p)
00202         {
00203             //if proposition covProps[p] has already been covered at state src, skip it
00204             if (state[p])
00205                 continue;
00206             //for each proposition covProps[p] that has not yet been
00207             //covered at state src, construct a transition from src to (src|p)
00208             //on formula (covProps[p]==true)
00209             boost::dynamic_bitset<> target(state);
00210             target[p] = true;
00211             World nextProp(numProps);
00212             nextProp[covProps[p]] = true;
00213             phi->addTransition(src, nextProp, target.to_ulong());
00214             //also build a loop from src to src on formula with conjunct (covProps[p]==false)
00215             loop[covProps[p]] = false;
00216         }
00217         //now we add a loop from src to src on conjunction of (covProps[p]==false)
00218         //for every p such that the pth bit of src is 1
00219         phi->addTransition(src, loop, src);
00220     }
00221     phi->setAccepting(phi->numStates()-1, true);
00222     phi->setStartState(0);
00223     return phi;
00224 }
00225 
00226 ompl::control::AutomatonPtr ompl::control::Automaton::SequenceAutomaton(unsigned int numProps, const std::vector<unsigned int>& seqProps)
00227 {
00228     AutomatonPtr seq(new Automaton(numProps, seqProps.size()+1));
00229     for (unsigned int state = 0; state < seqProps.size(); ++state)
00230     {
00231         // loop when next proposition in sequence is not satisfied
00232         World loop(numProps);
00233         loop[seqProps[state]] = false;
00234         seq->addTransition(state, loop, state);
00235 
00236         // progress forward when next proposition in sequence is satisfied
00237         World progress(numProps);
00238         progress[seqProps[state]] = true;
00239         seq->addTransition(state, progress, state+1);
00240     }
00241     //loop on all input when in accepting state
00242     seq->addTransition(seqProps.size(), World(numProps), seqProps.size());
00243     seq->setAccepting(seqProps.size(), true);
00244     seq->setStartState(0);
00245     return seq;
00246 }
00247 
00248 ompl::control::AutomatonPtr ompl::control::Automaton::DisjunctionAutomaton(unsigned int numProps, const std::vector<unsigned int>& disjProps)
00249 {
00250     AutomatonPtr disj(new Automaton(numProps, 2));
00251     World loop(numProps);
00252     for (std::vector<unsigned int>::const_iterator p = disjProps.begin(); p != disjProps.end(); ++p)
00253     {
00254         World satisfy(numProps);
00255         satisfy[*p] = true;
00256         loop[*p] = false;
00257         disj->addTransition(0, satisfy, 1);
00258     }
00259     disj->addTransition(0, loop, 0);
00260     disj->addTransition(1, World(numProps), 1);
00261     disj->setAccepting(1, true);
00262     disj->setStartState(0);
00263     return disj;
00264 }
00265 
00266 ompl::control::AutomatonPtr ompl::control::Automaton::AvoidanceAutomaton(unsigned int numProps, const std::vector<unsigned int>& avoidProps)
00267 {
00268     /* An avoidance automaton is simply a disjunction automaton with its acceptance condition flipped. */
00269     AutomatonPtr avoid = DisjunctionAutomaton(numProps, avoidProps);
00270     avoid->setAccepting(0, true);
00271     avoid->setAccepting(1, false);
00272     return avoid;
00273 }
00274 
00275 ompl::control::AutomatonPtr ompl::control::Automaton::CoverageAutomaton(unsigned int numProps)
00276 {
00277     const boost::integer_range<unsigned int> props = boost::irange(0u,numProps);
00278     return CoverageAutomaton(numProps, std::vector<unsigned int>(props.begin(), props.end()));
00279 }
00280 
00281 ompl::control::AutomatonPtr ompl::control::Automaton::SequenceAutomaton(unsigned int numProps)
00282 {
00283     const boost::integer_range<unsigned int> props = boost::irange(0u,numProps);
00284     return SequenceAutomaton(numProps, std::vector<unsigned int>(props.begin(), props.end()));
00285 }
00286 
00287 ompl::control::AutomatonPtr ompl::control::Automaton::DisjunctionAutomaton(unsigned int numProps)
00288 {
00289     const boost::integer_range<unsigned int> props = boost::irange(0u,numProps);
00290     return DisjunctionAutomaton(numProps, std::vector<unsigned int>(props.begin(), props.end()));
00291 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines