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 <unordered_map>
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, ProductGraphPtr a, double exploreTime = 0.5);
65 
67  ~LTLPlanner() override;
68 
71 
73  void setup() override;
74 
76  void clear() override;
77 
81  base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override;
83 
86  void getTree(std::vector<base::State *> &tree) const;
87 
91  std::vector<ProductGraph::State *> getHighLevelPath(const std::vector<base::State *> &path,
92  ProductGraph::State *start = nullptr) const;
93 
94  protected:
99  struct Motion
100  {
101  public:
103  Motion() = default;
104 
107  Motion(const SpaceInformation *si);
108 
111  virtual ~Motion();
112 
114  base::State *state{nullptr};
115 
117  Control *control{nullptr};
118 
120  Motion *parent{nullptr};
121 
123  unsigned int steps{0};
124 
126  ProductGraph::State *abstractState{nullptr};
127  };
128 
132  struct ProductGraphStateInfo
133  {
135  ProductGraphStateInfo() = default;
136 
140  void addMotion(Motion *m);
141 
142  double weight{0.};
143  PDF<Motion *> motions;
144  std::unordered_map<Motion *, PDF<Motion *>::Element *> motionElems;
145  double volume{0.};
146  double autWeight{0.};
147  unsigned int numSel{0};
148  PDF<ProductGraph::State *>::Element *pdfElem{nullptr};
149  };
150 
152  virtual double updateWeight(ProductGraph::State *as);
153 
155  virtual void initAbstractInfo(ProductGraph::State *as);
156 
159  virtual void buildAvail(const std::vector<ProductGraph::State *> &lead);
160 
166  virtual bool explore(const std::vector<ProductGraph::State *> &lead, Motion *&soln, double duration);
167 
171  virtual double abstractEdgeWeight(ProductGraph::State *a, ProductGraph::State *b) const;
172 
174  base::StateSamplerPtr sampler_;
175 
178 
180  const LTLSpaceInformation *ltlsi_;
181 
184 
186  PDF<ProductGraph::State *> availDist_;
187 
189  RNG rng_;
190 
192  std::vector<Motion *> motions_;
193 
196 
198  double exploreTime_;
199 
201  std::unordered_map<ProductGraph::State *, ProductGraphStateInfo> abstractInfo_;
202 
203  private:
205  void clearMotions();
206  };
207  }
208 }
209 
210 #endif
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Continues solving until a solution is found or a given planner termination condition is met....
Definition: LTLPlanner.cpp:79
ProductGraphStateInfo()=default
Creates an info object with no measurements and no tree motions.
~LTLPlanner() override
Clears all memory belonging to this LTLPlanner .
Definition: LTLPlanner.cpp:61
std::unordered_map< ProductGraph::State *, ProductGraphStateInfo > abstractInfo_
Map of abstraction states to their details.
Definition: LTLPlanner.h:297
Motion * parent
The parent motion in the tree.
Definition: LTLPlanner.h:216
ControlSamplerPtr controlSampler_
Control sampler.
Definition: LTLPlanner.h:273
base::State * state
The state contained by the motion.
Definition: LTLPlanner.h:210
std::vector< Motion * > motions_
Set of all motions.
Definition: LTLPlanner.h:288
T * as()
Cast this instance to a desired type.
Definition: Planner.h:294
A shared pointer wrapper for ompl::control::LTLSpaceInformation.
Control * control
The control contained by the motion.
Definition: LTLPlanner.h:213
Motion()=default
Default constructor for Motion.
void clear() override
Clears all datastructures belonging to this LTLPlanner.
Definition: LTLPlanner.cpp:71
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:176
virtual ~Motion()
Motion destructor does not clear memory. Deletions should be performed by the LTLPlanner.
void getTree(std::vector< base::State * > &tree) const
Helper debug method to access this planner's underlying tree of states.
Definition: LTLPlanner.cpp:147
virtual void initAbstractInfo(ProductGraph::State *as)
Initializes the info object for a new high-level state.
Definition: LTLPlanner.cpp:190
A State of a ProductGraph represents a vertex in the graph-based Cartesian product represented by the...
Definition: ProductGraph.h:147
virtual double updateWeight(ProductGraph::State *as)
Updates and returns the weight of an abstraction state.
Definition: LTLPlanner.cpp:181
base::StateSamplerPtr sampler_
State sampler.
Definition: LTLPlanner.h:270
unsigned int steps
The number of steps for which the control is applied.
Definition: LTLPlanner.h:219
LTLPlanner(const LTLSpaceInformationPtr &si, 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:52
A shared pointer wrapper for ompl::control::ControlSampler.
ProductGraph::State * prodStart_
Start state in product graph.
Definition: LTLPlanner.h:291
std::vector< ProductGraph::State * > getHighLevelPath(const std::vector< base::State * > &path, ProductGraph::State *start=nullptr) const
Helper debug method to return the sequence of high-level product graph states corresponding to a sequ...
Definition: LTLPlanner.cpp:155
void setup() override
Initializes LTLPlanner data structures.
Definition: LTLPlanner.cpp:66
A shared pointer wrapper for ompl::control::ProductGraph.
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:205
ProductGraph::State * abstractState
The high-level state to which this motion belongs.
Definition: LTLPlanner.h:222
const LTLSpaceInformation * ltlsi_
Handle to the control::SpaceInformation object.
Definition: LTLPlanner.h:276
PDF< ProductGraph::State * > availDist_
Used to sample nonempty regions in which to promote expansion.
Definition: LTLPlanner.h:282
RNG rng_
A random number generator.
Definition: LTLPlanner.h:285
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:289
double exploreTime_
Time to spend exploring each lead.
Definition: LTLPlanner.h:294
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:223
ProductGraphPtr abstraction_
The high level abstaction used to grow the tree structure.
Definition: LTLPlanner.h:279
Main namespace. Contains everything in this library.