Loading...
Searching...
No Matches
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
48namespace ompl
49{
50 namespace control
51 {
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
115
117 Control *control{nullptr};
118
120 Motion *parent{nullptr};
121
123 unsigned int steps{0};
124
127 };
128
133 {
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
181
184
187
190
192 std::vector<Motion *> motions_;
193
196
199
201 std::unordered_map<ProductGraph::State *, ProductGraphStateInfo> abstractInfo_;
202
203 private:
205 void clearMotions();
206 };
207 }
208}
209
210#endif
A container that supports probabilistic sampling over weighted data.
Definition PDF.h:49
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Base class for a planner.
Definition Planner.h:216
T * as()
Cast this instance to a desired type.
Definition Planner.h:230
Definition of an abstract state.
Definition State.h:50
A shared pointer wrapper for ompl::control::ControlSampler.
Definition of an abstract control.
Definition Control.h:48
double exploreTime_
Time to spend exploring each lead.
Definition LTLPlanner.h:198
std::unordered_map< ProductGraph::State *, ProductGraphStateInfo > abstractInfo_
Map of abstraction states to their details.
Definition LTLPlanner.h:201
RNG rng_
A random number generator.
Definition LTLPlanner.h:189
ProductGraph::State * prodStart_
Start state in product graph.
Definition LTLPlanner.h:195
virtual void initAbstractInfo(ProductGraph::State *as)
Initializes the info object for a new high-level state.
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...
base::StateSamplerPtr sampler_
State sampler.
Definition LTLPlanner.h:174
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Continues solving until a solution is found or a given planner termination condition is met....
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...
virtual double updateWeight(ProductGraph::State *as)
Updates and returns the weight of an abstraction state.
void clear() override
Clears all datastructures belonging to this LTLPlanner.
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...
virtual void buildAvail(const std::vector< ProductGraph::State * > &lead)
Compute a set of high-level states along a lead to be considered for expansion.
std::vector< Motion * > motions_
Set of all motions.
Definition LTLPlanner.h:192
~LTLPlanner() override
Clears all memory belonging to this LTLPlanner .
const LTLSpaceInformation * ltlsi_
Handle to the control::SpaceInformation object.
Definition LTLPlanner.h:180
ProductGraphPtr abstraction_
The high level abstaction used to grow the tree structure.
Definition LTLPlanner.h:183
void setup() override
Initializes LTLPlanner data structures.
void getTree(std::vector< base::State * > &tree) const
Helper debug method to access this planner's underlying tree of states.
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...
PDF< ProductGraph::State * > availDist_
Used to sample nonempty regions in which to promote expansion.
Definition LTLPlanner.h:186
ControlSamplerPtr controlSampler_
Control sampler.
Definition LTLPlanner.h:177
A shared pointer wrapper for ompl::control::LTLSpaceInformation.
A shared pointer wrapper for ompl::control::ProductGraph.
A State of a ProductGraph represents a vertex in the graph-based Cartesian product represented by the...
Space information containing necessary information for planning with controls. setup() needs to be ca...
This namespace contains sampling based planning routines used by planning under differential constrai...
Definition Control.h:45
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()
Representation of a motion.
Definition LTLPlanner.h:100
base::State * state
The state contained by the motion.
Definition LTLPlanner.h:114
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.
Motion * parent
The parent motion in the tree.
Definition LTLPlanner.h:120
ProductGraph::State * abstractState
The high-level state to which this motion belongs.
Definition LTLPlanner.h:126
unsigned int steps
The number of steps for which the control is applied.
Definition LTLPlanner.h:123
Motion()=default
Default constructor for Motion.
ProductGraphStateInfo()=default
Creates an info object with no measurements and no tree motions.
void addMotion(Motion *m)
Adds a tree motion to an info object. This method is called whenever a new tree motion is created in ...