ompl/control/planners/ltl/LTLPlanner.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_LTLPLANNER_
00038 #define OMPL_CONTROL_PLANNERS_LTL_LTLPLANNER_
00039 
00040 #include "ompl/control/planners/PlannerIncludes.h"
00041 #include "ompl/control/planners/ltl/ProductGraph.h"
00042 #include "ompl/control/planners/ltl/LTLSpaceInformation.h"
00043 #include "ompl/datastructures/PDF.h"
00044 #include <boost/unordered_map.hpp>
00045 #include <map>
00046 #include <vector>
00047 
00048 namespace ompl
00049 {
00050     namespace control
00051     {
00058         class LTLPlanner : public base::Planner
00059         {
00060         public:
00064             LTLPlanner(const LTLSpaceInformationPtr& si, const ProductGraphPtr& a, double exploreTime = 0.5);
00065 
00067             virtual ~LTLPlanner(void);
00068 
00071 
00073             virtual void setup(void);
00074 
00076             virtual void clear(void);
00077 
00081             virtual base::PlannerStatus solve(const base::PlannerTerminationCondition& ptc);
00083 
00086             void getTree(std::vector<base::State*>& tree) const;
00087 
00091             std::vector<ProductGraph::State*> getHighLevelPath(const std::vector<base::State*>& path, ProductGraph::State* start = NULL) const;
00092 
00093         protected:
00098             struct Motion
00099             {
00100             public:
00102                 Motion(void);
00103 
00106                 Motion(const SpaceInformation* si);
00107 
00110                 virtual ~Motion(void);
00111 
00113                 base::State* state;
00114 
00116                 Control* control;
00117 
00119                 Motion* parent;
00120 
00122                 unsigned int steps;
00123 
00125                 ProductGraph::State* abstractState;
00126             };
00127 
00131             struct ProductGraphStateInfo
00132             {
00134                 ProductGraphStateInfo(void);
00135 
00139                 void addMotion(Motion* m);
00140 
00141                 double weight;
00142                 PDF<Motion*> motions;
00143                 boost::unordered_map< Motion*, PDF<Motion*>::Element* > motionElems;
00144                 double volume;
00145                 double autWeight;
00146                 unsigned int numSel;
00147                 PDF<ProductGraph::State*>::Element* pdfElem;
00148             };
00149 
00151             virtual double updateWeight(ProductGraph::State* as);
00152 
00154             virtual void initAbstractInfo(ProductGraph::State* as);
00155 
00158             virtual void buildAvail(const std::vector<ProductGraph::State*>& lead);
00159 
00165             virtual bool explore(const std::vector<ProductGraph::State*>& lead, Motion*& soln, double duration);
00166 
00170             virtual double abstractEdgeWeight(ProductGraph::State* a, ProductGraph::State* b) const;
00171 
00173             base::StateSamplerPtr sampler_;
00174 
00176             ControlSamplerPtr controlSampler_;
00177 
00179             const LTLSpaceInformation* ltlsi_;
00180 
00182             ProductGraphPtr abstraction_;
00183 
00185             PDF<ProductGraph::State*> availDist_;
00186 
00188             RNG rng_;
00189 
00191             std::vector<Motion*> motions_;
00192 
00194             ProductGraph::State* prodStart_;
00195 
00197             double exploreTime_;
00198 
00200             boost::unordered_map< ProductGraph::State*, ProductGraphStateInfo > abstractInfo_;
00201 
00202         private:
00204             void clearMotions(void);
00205         };
00206     }
00207 }
00208 
00209 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines