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