LTLPlanner.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2012, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Matt Maly */
36 
37 #ifndef OMPL_CONTROL_PLANNERS_LTL_LTLPLANNER_
38 #define OMPL_CONTROL_PLANNERS_LTL_LTLPLANNER_
39 
40 #include "ompl/control/planners/PlannerIncludes.h"
41 #include "ompl/control/planners/ltl/ProductGraph.h"
42 #include "ompl/control/planners/ltl/LTLSpaceInformation.h"
43 #include "ompl/datastructures/PDF.h"
44 #include <boost/unordered_map.hpp>
45 #include <map>
46 #include <vector>
47 
48 namespace ompl
49 {
50  namespace control
51  {
58  class LTLPlanner : public base::Planner
59  {
60  public:
64  LTLPlanner(const LTLSpaceInformationPtr& si, const ProductGraphPtr& a, double exploreTime = 0.5);
65 
67  virtual ~LTLPlanner(void);
68 
71 
73  virtual void setup(void);
74 
76  virtual void clear(void);
77 
83 
86  void getTree(std::vector<base::State*>& tree) const;
87 
91  std::vector<ProductGraph::State*> getHighLevelPath(const std::vector<base::State*>& path, ProductGraph::State* start = NULL) const;
92 
93  protected:
98  struct Motion
99  {
100  public:
102  Motion(void);
103 
106  Motion(const SpaceInformation* si);
107 
110  virtual ~Motion(void);
111 
114 
117 
120 
122  unsigned int steps;
123 
126  };
127 
132  {
134  ProductGraphStateInfo(void);
135 
139  void addMotion(Motion* m);
140 
141  double weight;
142  PDF<Motion*> motions;
143  boost::unordered_map< Motion*, PDF<Motion*>::Element* > motionElems;
144  double volume;
145  double autWeight;
146  unsigned int numSel;
148  };
149 
151  virtual double updateWeight(ProductGraph::State* as);
152 
154  virtual void initAbstractInfo(ProductGraph::State* as);
155 
158  virtual void buildAvail(const std::vector<ProductGraph::State*>& lead);
159 
165  virtual bool explore(const std::vector<ProductGraph::State*>& lead, Motion*& soln, double duration);
166 
170  virtual double abstractEdgeWeight(ProductGraph::State* a, ProductGraph::State* b) const;
171 
174 
177 
180 
183 
186 
189 
191  std::vector<Motion*> motions_;
192 
195 
197  double exploreTime_;
198 
200  boost::unordered_map< ProductGraph::State*, ProductGraphStateInfo > abstractInfo_;
201 
202  private:
204  void clearMotions(void);
205  };
206  }
207 }
208 
209 #endif
void addMotion(Motion *m)
Adds a tree motion to an info object. This method is called whenever a new tree motion is created in ...
Definition: LTLPlanner.cpp:148
base::State * state
The state contained by the motion.
Definition: LTLPlanner.h:113
A planner for generating system trajectories to satisfy a logical specification given by an automaton...
Definition: LTLPlanner.h:58
const LTLSpaceInformation * ltlsi_
Handle to the control::SpaceInformation object.
Definition: LTLPlanner.h:179
boost::unordered_map< ProductGraph::State *, ProductGraphStateInfo > abstractInfo_
Map of abstraction states to their details.
Definition: LTLPlanner.h:200
std::vector< ProductGraph::State * > getHighLevelPath(const std::vector< base::State * > &path, ProductGraph::State *start=NULL) const
Helper debug method to return the sequence of high-level product graph states corresponding to a sequ...
Definition: LTLPlanner.cpp:113
double exploreTime_
Time to spend exploring each lead.
Definition: LTLPlanner.h:197
unsigned int steps
The number of steps for which the control is applied.
Definition: LTLPlanner.h:122
Definition of an abstract control.
Definition: Control.h:48
ProductGraph::State * prodStart_
Start state in product graph.
Definition: LTLPlanner.h:194
A boost shared pointer wrapper for ompl::base::StateSampler.
A boost shared pointer wrapper for ompl::control::ControlSampler.
virtual void initAbstractInfo(ProductGraph::State *as)
Initializes the info object for a new high-level state.
Definition: LTLPlanner.cpp:162
virtual double updateWeight(ProductGraph::State *as)
Updates and returns the weight of an abstraction state.
Definition: LTLPlanner.cpp:153
std::vector< Motion * > motions_
Set of all motions.
Definition: LTLPlanner.h:191
virtual void clear(void)
Clears all datastructures belonging to this LTLPlanner.
Definition: LTLPlanner.cpp:35
PDF< ProductGraph::State * > availDist_
Used to sample nonempty regions in which to promote expansion.
Definition: LTLPlanner.h:185
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Motion(void)
Default constructor for Motion.
Definition: LTLPlanner.cpp:126
virtual double abstractEdgeWeight(ProductGraph::State *a, ProductGraph::State *b) const
Returns the weight of an edge between two given high-level states, which we compute as the product of...
Definition: LTLPlanner.cpp:262
Representation of a motion.
Definition: LTLPlanner.h:98
base::StateSamplerPtr sampler_
State sampler.
Definition: LTLPlanner.h:173
Control * control
The control contained by the motion.
Definition: LTLPlanner.h:116
A container that supports probabilistic sampling over weighted data.
Definition: PDF.h:48
A boost shared pointer wrapper for ompl::control::LTLSpaceInformation.
ControlSamplerPtr controlSampler_
Control sampler.
Definition: LTLPlanner.h:176
Main namespace. Contains everything in this library.
Definition: Cost.h:42
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:54
Base class for a planner.
Definition: Planner.h:232
RNG rng_
A random number generator.
Definition: LTLPlanner.h:188
A State of a ProductGraph represents a vertex in the graph-based Cartesian product represented by the...
Definition: ProductGraph.h:74
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
LTLPlanner(const LTLSpaceInformationPtr &si, const ProductGraphPtr &a, double exploreTime=0.5)
Create an LTLPlanner with a given space and product graph. Accepts an optional third parameter to con...
Definition: LTLPlanner.cpp:15
Definition of an abstract state.
Definition: State.h:50
virtual void buildAvail(const std::vector< ProductGraph::State * > &lead)
Compute a set of high-level states along a lead to be considered for expansion.
Definition: LTLPlanner.cpp:178
ProductGraphStateInfo(void)
Creates an info object with no measurements and no tree motions.
Definition: LTLPlanner.cpp:142
A class that will hold data contained in the PDF.
Definition: PDF.h:53
void getTree(std::vector< base::State * > &tree) const
Helper debug method to access this planner's underlying tree of states.
Definition: LTLPlanner.cpp:106
ProductGraphPtr abstraction_
The high level abstaction used to grow the tree structure.
Definition: LTLPlanner.h:182
virtual bool explore(const std::vector< ProductGraph::State * > &lead, Motion *&soln, double duration)
Expand the tree of motions along a given lead for a given duration of time. Returns true if a solutio...
Definition: LTLPlanner.cpp:198
A boost shared pointer wrapper for ompl::control::ProductGraph.
virtual void setup(void)
Initializes LTLPlanner data structures.
Definition: LTLPlanner.cpp:30
virtual ~LTLPlanner(void)
Clears all memory belonging to this LTLPlanner .
Definition: LTLPlanner.cpp:25
Space information containing necessary information for planning with controls. setup() needs to be ca...
virtual ~Motion(void)
Motion destructor does not clear memory. Deletions should be performed by the LTLPlanner.
Definition: LTLPlanner.cpp:138
Motion * parent
The parent motion in the tree.
Definition: LTLPlanner.h:119
A structure to hold measurement information for a high-level state, as well as the set of tree motion...
Definition: LTLPlanner.h:131
T * as()
Cast this instance to a desired type.
Definition: Planner.h:247
ProductGraph::State * abstractState
The high-level state to which this motion belongs.
Definition: LTLPlanner.h:125
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Continues solving until a solution is found or a given planner termination condition is met...
Definition: LTLPlanner.cpp:43