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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines