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
A planner for generating system trajectories to satisfy a logical specification given by an automaton...
Definition LTLPlanner.h:59
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...
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
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.
A structure to hold measurement information for a high-level state, as well as the set of tree motion...
Definition LTLPlanner.h:133
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 ...