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 
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 
127  };
128 
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 
175 
178 
181 
184 
187 
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
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
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:291
base::State * state
The state contained by the motion.
Definition: LTLPlanner.h:114
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:180
double exploreTime_
Time to spend exploring each lead.
Definition: LTLPlanner.h:198
unsigned int steps
The number of steps for which the control is applied.
Definition: LTLPlanner.h:123
void getTree(std::vector< base::State *> &tree) const
Helper debug method to access this planner&#39;s underlying tree of states.
Definition: LTLPlanner.cpp:147
Definition of an abstract control.
Definition: Control.h:47
PDF< ProductGraph::State * > availDist_
Used to sample nonempty regions in which to promote expansion.
Definition: LTLPlanner.h:186
ProductGraph::State * prodStart_
Start state in product graph.
Definition: LTLPlanner.h:195
A shared pointer wrapper for ompl::base::StateSampler.
A 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:190
virtual double updateWeight(ProductGraph::State *as)
Updates and returns the weight of an abstraction state.
Definition: LTLPlanner.cpp:181
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
~LTLPlanner() override
Clears all memory belonging to this LTLPlanner .
Definition: LTLPlanner.cpp:61
Representation of a motion.
Definition: LTLPlanner.h:99
base::StateSamplerPtr sampler_
State sampler.
Definition: LTLPlanner.h:174
std::unordered_map< ProductGraph::State *, ProductGraphStateInfo > abstractInfo_
Map of abstraction states to their details.
Definition: LTLPlanner.h:201
Control * control
The control contained by the motion.
Definition: LTLPlanner.h:117
virtual ~Motion()
Motion destructor does not clear memory. Deletions should be performed by the LTLPlanner.
A container that supports probabilistic sampling over weighted data.
Definition: PDF.h:48
A shared pointer wrapper for ompl::control::LTLSpaceInformation.
ControlSamplerPtr controlSampler_
Control sampler.
Definition: LTLPlanner.h:177
std::vector< Motion * > motions_
Set of all motions.
Definition: LTLPlanner.h:192
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
void setup() override
Initializes LTLPlanner data structures.
Definition: LTLPlanner.cpp:66
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
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:58
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:225
Base class for a planner.
Definition: Planner.h:223
RNG rng_
A random number generator.
Definition: LTLPlanner.h:189
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
A State of a ProductGraph represents a vertex in the graph-based Cartesian product represented by the...
Definition: ProductGraph.h:83
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
void clear() override
Clears all datastructures belonging to this LTLPlanner.
Definition: LTLPlanner.cpp:71
Definition of an abstract state.
Definition: State.h:49
A class that will hold data contained in the PDF.
Definition: PDF.h:52
ProductGraphPtr abstraction_
The high level abstaction used to grow the tree structure.
Definition: LTLPlanner.h:183
A shared pointer wrapper for ompl::control::ProductGraph.
Motion()=default
Default constructor for Motion.
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
Space information containing necessary information for planning with controls. setup() needs to be ca...
Motion * parent
The parent motion in the tree.
Definition: LTLPlanner.h:120
A structure to hold measurement information for a high-level state, as well as the set of tree motion...
Definition: LTLPlanner.h:132
T * as()
Cast this instance to a desired type.
Definition: Planner.h:238
ProductGraph::State * abstractState
The high-level state to which this motion belongs.
Definition: LTLPlanner.h:126