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 }