ompl/control/planners/ltl/ProductGraph.h
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2012, Rice University
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Rice University nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Matt Maly */
00036 
00037 #ifndef OMPL_CONTROL_PLANNERS_LTL_PRODUCTGRAPH_
00038 #define OMPL_CONTROL_PLANNERS_LTL_PRODUCTGRAPH_
00039 
00040 #include "ompl/base/State.h"
00041 #include "ompl/control/planners/ltl/Automaton.h"
00042 #include "ompl/control/planners/ltl/PropositionalDecomposition.h"
00043 #include "ompl/util/ClassForward.h"
00044 #include <boost/function.hpp>
00045 #include <boost/graph/adjacency_list.hpp>
00046 #include <boost/unordered_map.hpp>
00047 #include <map>
00048 #include <ostream>
00049 #include <vector>
00050 
00051 namespace ompl
00052 {
00053     namespace control
00054     {
00056 
00057         OMPL_CLASS_FORWARD(ProductGraph);
00059 
00067         class ProductGraph
00068         {
00069         public:
00074             class State
00075             {
00076             friend class ProductGraph;
00077             public:
00080                 State(void)
00081                     : decompRegion(-1),
00082                       cosafeState(-1),
00083                       safeState(-1) { }
00084 
00086                 State(const State& s)
00087                     : decompRegion(s.decompRegion),
00088                       cosafeState(s.cosafeState),
00089                       safeState(s.safeState) { }
00090 
00094                                 bool operator==(const State& s) const;
00095 
00099                 bool isValid(void) const;
00100 
00102 
00103                 friend std::size_t hash_value(const ProductGraph::State& s);
00105 
00107                 friend std::ostream& operator<<(std::ostream& out, const State& s);
00108 
00110                 int getDecompRegion(void) const;
00111 
00113                 int getCosafeState(void) const;
00114 
00116                 int getSafeState(void) const;
00117 
00118             private:
00119                 int decompRegion;
00120                 int cosafeState;
00121                 int safeState;
00122             };
00123 
00126             ProductGraph(
00127                 const PropositionalDecompositionPtr& decomp,
00128                 const AutomatonPtr& cosafetyAut,
00129                 const AutomatonPtr& safetyAut
00130             );
00131 
00135             ProductGraph(
00136                 const PropositionalDecompositionPtr& decomp,
00137                 const AutomatonPtr& cosafetyAut
00138             );
00139 
00140             ~ProductGraph();
00141 
00144             const PropositionalDecompositionPtr& getDecomp() const;
00145 
00148             const AutomatonPtr& getCosafetyAutom() const;
00149 
00152             const AutomatonPtr& getSafetyAutom() const;
00153 
00162             std::vector<State*> computeLead(State* start, const boost::function<double(State*, State*)>& edgeWeight);
00163 
00165             void clear();
00166 
00172             void buildGraph(State* start, const boost::function<void(State*)>& initialize = ProductGraph::noInit);
00173 
00180             bool isSolution(const State* s) const;
00181 
00183             State* getStartState(void) const;
00184 
00187             double getRegionVolume(const State* s);
00188 
00191             int getCosafeAutDistance(const State* s) const;
00192 
00195             int getSafeAutDistance(const State* s) const;
00196 
00200                         State* getState(const base::State* cs) const;
00201 
00205             State* getState(const base::State* cs, int cosafe, int safe) const;
00206 
00210             State* getState(const State* parent, int nextRegion) const;
00211 
00216             State* getState(const State* parent, const base::State* cs) const;
00217 
00220             State* getState(int region, int cosafe, int safe) const
00221             {
00222                 State s;
00223                 s.decompRegion = region;
00224                 s.cosafeState = cosafe;
00225                 s.safeState = safe;
00226                 State*& ret = stateToPtr_[s];
00227                 if (ret == NULL) ret = new State(s);
00228                 return ret;
00229             }
00230 
00231         protected:
00232             static void noInit(State* s)
00233             {
00234             }
00235             struct Edge
00236             {
00237                 double cost;
00238             };
00239 
00240             typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, State*, Edge> GraphType;
00241             typedef boost::graph_traits<GraphType>::vertex_descriptor Vertex;
00242             typedef boost::graph_traits<GraphType>::vertex_iterator VertexIter;
00243             typedef boost::property_map<GraphType, boost::vertex_index_t>::type VertexIndexMap;
00244             typedef boost::graph_traits<GraphType>::edge_iterator EdgeIter;
00245 
00246             PropositionalDecompositionPtr decomp_;
00247             AutomatonPtr cosafety_;
00248             AutomatonPtr safety_;
00249             GraphType graph_;
00250             State* startState_;
00251                         std::vector<State*> solutionStates_;
00252 
00253             /* Only one State pointer will be allocated for each possible State
00254                in the ProductGraph. There will exist situations in which
00255                all we have are the component values (region, automaton states)
00256                of a State and we want the actual State pointer.
00257                We use this map to access it. */
00258                         mutable boost::unordered_map<State, State*> stateToPtr_;
00259 
00260             /* Map from State pointer to the index of the corresponding vertex
00261                in the graph. */
00262             boost::unordered_map<State*, int> stateToIndex_;
00263         };
00264     }
00265 }
00266 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines