ompl/control/planners/ltl/src/ProductGraph.cpp
00001 #include "ompl/control/planners/ltl/ProductGraph.h" 00002 #include "ompl/base/State.h" 00003 #include "ompl/control/planners/ltl/Automaton.h" 00004 #include "ompl/control/planners/ltl/PropositionalDecomposition.h" 00005 #include "ompl/control/planners/ltl/World.h" 00006 #include "ompl/util/ClassForward.h" 00007 #include "ompl/util/Console.h" 00008 #include <algorithm> 00009 #include <boost/function.hpp> 00010 #include <boost/functional/hash.hpp> 00011 #include <boost/graph/adjacency_list.hpp> 00012 #include <boost/graph/dijkstra_shortest_paths.hpp> 00013 #include <boost/unordered_map.hpp> 00014 #include <boost/unordered_set.hpp> 00015 #include <map> 00016 #include <ostream> 00017 #include <queue> 00018 #include <stack> 00019 #include <vector> 00020 00021 bool ompl::control::ProductGraph::State::operator==(const State& s) const 00022 { 00023 return decompRegion==s.decompRegion 00024 && cosafeState==s.cosafeState 00025 && safeState==s.safeState; 00026 } 00027 00028 bool ompl::control::ProductGraph::State::isValid(void) const 00029 { 00030 return cosafeState != -1 && safeState != -1; 00031 } 00032 00033 namespace ompl 00034 { 00035 namespace control 00036 { 00037 std::size_t hash_value(const ProductGraph::State& s) 00038 { 00039 std::size_t hash = 0; 00040 boost::hash_combine(hash, s.decompRegion); 00041 boost::hash_combine(hash, s.cosafeState); 00042 boost::hash_combine(hash, s.safeState); 00043 return hash; 00044 } 00045 00046 std::ostream& operator<<(std::ostream& out, const ProductGraph::State& s) 00047 { 00048 out << "(" << s.decompRegion << "," << s.cosafeState << ","; 00049 out << s.safeState << ")"; 00050 return out; 00051 } 00052 } 00053 } 00054 00055 int ompl::control::ProductGraph::State::getDecompRegion(void) const 00056 { 00057 return decompRegion; 00058 } 00059 00060 int ompl::control::ProductGraph::State::getCosafeState(void) const 00061 { 00062 return cosafeState; 00063 } 00064 00065 int ompl::control::ProductGraph::State::getSafeState(void) const 00066 { 00067 return safeState; 00068 } 00069 00070 ompl::control::ProductGraph::ProductGraph(const PropositionalDecompositionPtr& decomp, 00071 const AutomatonPtr& cosafetyAut, const AutomatonPtr& safetyAut) : 00072 decomp_(decomp), 00073 cosafety_(cosafetyAut), 00074 safety_(safetyAut) 00075 { 00076 } 00077 00078 ompl::control::ProductGraph::ProductGraph(const PropositionalDecompositionPtr& decomp, 00079 const AutomatonPtr& cosafetyAut) : 00080 decomp_(decomp), 00081 cosafety_(cosafetyAut), 00082 safety_(Automaton::AcceptingAutomaton(decomp->getNumProps())) 00083 { 00084 } 00085 00086 ompl::control::ProductGraph::~ProductGraph() 00087 { 00088 clear(); 00089 } 00090 00091 const ompl::control::PropositionalDecompositionPtr& ompl::control::ProductGraph::getDecomp() const 00092 { 00093 return decomp_; 00094 } 00095 00096 const ompl::control::AutomatonPtr& ompl::control::ProductGraph::getCosafetyAutom() const 00097 { 00098 return cosafety_; 00099 } 00100 00101 const ompl::control::AutomatonPtr& ompl::control::ProductGraph::getSafetyAutom() const 00102 { 00103 return safety_; 00104 } 00105 00106 std::vector<ompl::control::ProductGraph::State*> 00107 ompl::control::ProductGraph::computeLead( 00108 ProductGraph::State* start, 00109 const boost::function<double(ProductGraph::State*, ProductGraph::State*)>& edgeWeight) 00110 { 00111 std::vector<GraphType::vertex_descriptor> parents(boost::num_vertices(graph_)); 00112 std::vector<double> distances(boost::num_vertices(graph_)); 00113 EdgeIter ei, eend; 00114 //first build up the edge weights 00115 for (boost::tie(ei,eend) = boost::edges(graph_); ei != eend; ++ei) 00116 { 00117 GraphType::vertex_descriptor src = boost::source(*ei, graph_); 00118 GraphType::vertex_descriptor target = boost::target(*ei, graph_); 00119 graph_[*ei].cost = edgeWeight(graph_[src], graph_[target]); 00120 } 00121 int startIndex = stateToIndex_[start]; 00122 boost::dijkstra_shortest_paths(graph_, boost::vertex(startIndex,graph_), 00123 boost::weight_map(get(&Edge::cost, graph_)).distance_map( 00124 boost::make_iterator_property_map(distances.begin(), get(boost::vertex_index, graph_) 00125 )).predecessor_map( 00126 boost::make_iterator_property_map(parents.begin(), get(boost::vertex_index, graph_)) 00127 ) 00128 ); 00129 //pick state from solutionStates_ such that distance[state] is minimized 00130 State* bestSoln = *solutionStates_.begin(); 00131 double cost = distances[boost::vertex(stateToIndex_[bestSoln], graph_)]; 00132 for (std::vector<State*>::const_iterator s = solutionStates_.begin()+1; s != solutionStates_.end(); ++s) 00133 { 00134 if (distances[boost::vertex(stateToIndex_[*s], graph_)] < cost) 00135 { 00136 cost = distances[boost::vertex(stateToIndex_[*s], graph_)]; 00137 bestSoln = *s; 00138 } 00139 } 00140 //build lead from bestSoln parents 00141 std::stack<State*> leadStack; 00142 while (!(bestSoln == start)) 00143 { 00144 leadStack.push(bestSoln); 00145 bestSoln = graph_[parents[boost::vertex(stateToIndex_[bestSoln], graph_)]]; 00146 } 00147 leadStack.push(bestSoln); 00148 00149 std::vector<State*> lead; 00150 while (!leadStack.empty()) 00151 { 00152 lead.push_back(leadStack.top()); 00153 leadStack.pop(); 00154 // Truncate the lead as early when it hits the desired automaton states 00155 // TODO: more elegant way to do this? 00156 if (lead.back()->cosafeState == solutionStates_.front()->cosafeState 00157 && lead.back()->safeState == solutionStates_.front()->safeState) 00158 break; 00159 } 00160 return lead; 00161 } 00162 00163 void ompl::control::ProductGraph::clear() 00164 { 00165 solutionStates_.clear(); 00166 stateToIndex_.clear(); 00167 startState_ = NULL; 00168 graph_.clear(); 00169 boost::unordered_map<State,State*>::iterator i; 00170 for (i = stateToPtr_.begin(); i != stateToPtr_.end(); ++i) 00171 delete i->second; 00172 stateToPtr_.clear(); 00173 } 00174 00175 void ompl::control::ProductGraph::buildGraph(State* start, const boost::function<void(State*)>& initialize) 00176 { 00177 graph_.clear(); 00178 solutionStates_.clear(); 00179 std::queue<State*> q; 00180 boost::unordered_set<State*> processed; 00181 std::vector<int> regNeighbors; 00182 VertexIndexMap index = get(boost::vertex_index, graph_); 00183 00184 GraphType::vertex_descriptor next = boost::add_vertex(graph_); 00185 startState_ = start; 00186 graph_[boost::vertex(next,graph_)] = startState_; 00187 stateToIndex_[startState_] = index[next]; 00188 q.push(startState_); 00189 processed.insert(startState_); 00190 00191 OMPL_INFORM("Building graph from start state (%u,%u,%u) with index %d", 00192 startState_->decompRegion, startState_->cosafeState, 00193 startState_->safeState, stateToIndex_[startState_]); 00194 00195 while (!q.empty()) 00196 { 00197 State* current = q.front(); 00198 //Initialize each state using the supplied state initializer function 00199 initialize(current); 00200 q.pop(); 00201 00202 if (safety_->isAccepting(current->safeState) && cosafety_->isAccepting(current->cosafeState)) 00203 { 00204 solutionStates_.push_back(current); 00205 } 00206 00207 GraphType::vertex_descriptor v = boost::vertex(stateToIndex_[current], graph_); 00208 00209 //enqueue each neighbor of current 00210 decomp_->getNeighbors(current->decompRegion, regNeighbors); 00211 for (std::vector<int>::const_iterator r = regNeighbors.begin(); r != regNeighbors.end(); ++r) 00212 { 00213 State* nextState = getState(current, *r); 00214 if (!nextState->isValid()) 00215 continue; 00216 //if this state is newly discovered, 00217 //then we can dynamically allocate a copy of it 00218 //and add the new pointer to the graph. 00219 //either way, we need the pointer 00220 if (processed.find(nextState) == processed.end()) 00221 { 00222 const GraphType::vertex_descriptor next = boost::add_vertex(graph_); 00223 stateToIndex_[nextState] = index[next]; 00224 graph_[boost::vertex(next,graph_)] = nextState; 00225 q.push(nextState); 00226 processed.insert(nextState); 00227 } 00228 00229 //whether or not the neighbor is newly discovered, 00230 //we still need to add the edge to the graph 00231 GraphType::edge_descriptor edge; 00232 bool ignore; 00233 boost::tie(edge,ignore) = boost::add_edge(v, boost::vertex(stateToIndex_[nextState],graph_), graph_); 00234 //graph_[edge].src = index[v]; 00235 //graph_[edge].dest = stateToIndex_[nextState]; 00236 } 00237 regNeighbors.clear(); 00238 } 00239 if (solutionStates_.empty()) 00240 { 00241 OMPL_ERROR("No solution path found in product graph."); 00242 } 00243 00244 OMPL_INFORM("Number of decomposition regions: %u", decomp_->getNumRegions()); 00245 OMPL_INFORM("Number of cosafety automaton states: %u", cosafety_->numStates()); 00246 OMPL_INFORM("Number of safety automaton states: %u", safety_->numStates()); 00247 OMPL_INFORM("Number of high-level states in abstraction graph: %u", boost::num_vertices(graph_)); 00248 } 00249 00250 bool ompl::control::ProductGraph::isSolution(const State* s) const 00251 { 00252 return std::find(solutionStates_.begin(), solutionStates_.end(), s) 00253 != solutionStates_.end(); 00254 } 00255 00256 ompl::control::ProductGraph::State* ompl::control::ProductGraph::getStartState(void) const 00257 { 00258 return startState_; 00259 } 00260 00261 double ompl::control::ProductGraph::getRegionVolume(const State* s) 00262 { 00263 return decomp_->getRegionVolume(s->decompRegion); 00264 } 00265 00266 int ompl::control::ProductGraph::getCosafeAutDistance(const State* s) const 00267 { 00268 return cosafety_->distFromAccepting(s->cosafeState); 00269 } 00270 00271 int ompl::control::ProductGraph::getSafeAutDistance(const State* s) const 00272 { 00273 return safety_->distFromAccepting(s->safeState); 00274 } 00275 00276 ompl::control::ProductGraph::State* ompl::control::ProductGraph::getState(const base::State* cs) const 00277 { 00278 return getState(cs, cosafety_->getStartState(), safety_->getStartState()); 00279 } 00280 00281 ompl::control::ProductGraph::State* ompl::control::ProductGraph::getState(const base::State* cs, int cosafe, int safe) const 00282 { 00283 State s; 00284 s.decompRegion = decomp_->locateRegion(cs); 00285 s.cosafeState = cosafe; 00286 s.safeState = safe; 00287 State*& ret = stateToPtr_[s]; 00288 if (ret == NULL) 00289 ret = new State(s); 00290 return ret; 00291 } 00292 00293 ompl::control::ProductGraph::State* ompl::control::ProductGraph::getState(const State* parent, int nextRegion) const 00294 { 00295 State s; 00296 s.decompRegion = nextRegion; 00297 const World nextWorld = decomp_->worldAtRegion(nextRegion); 00298 s.cosafeState = cosafety_->step(parent->cosafeState, nextWorld); 00299 s.safeState = safety_->step(parent->safeState, nextWorld); 00300 State*& ret = stateToPtr_[s]; 00301 if (ret == NULL) 00302 ret = new State(s); 00303 return ret; 00304 } 00305 00306 ompl::control::ProductGraph::State* ompl::control::ProductGraph::getState(const State* parent, const base::State* cs) const 00307 { 00308 return getState(parent, decomp_->locateRegion(cs)); 00309 }