LTLPlanner.cpp
1 #include "ompl/control/planners/ltl/LTLPlanner.h"
2 #include "ompl/control/planners/PlannerIncludes.h"
3 #include "ompl/control/planners/ltl/ProductGraph.h"
4 #include "ompl/control/planners/ltl/LTLProblemDefinition.h"
5 #include "ompl/datastructures/PDF.h"
6 #include "ompl/util/Console.h"
7 #include <algorithm>
8 #include <boost/unordered_map.hpp>
9 #include <limits>
10 #include <map>
11 #include <vector>
12 
13 #include <stdio.h>
14 
16  ompl::base::Planner(ltlsi, "LTLPlanner"),
17  ltlsi_(ltlsi.get()),
18  abstraction_(a),
19  prodStart_(NULL),
20  exploreTime_(exploreTime)
21 {
23 }
24 
26 {
27  clearMotions();
28 }
29 
31 {
33 }
34 
36 {
38  availDist_.clear();
39  abstractInfo_.clear();
40  clearMotions();
41 }
42 
44 {
45  // TODO make solve work when called more than once!
46  checkValidity();
47  const base::State* start = pis_.nextStart();
48  prodStart_ = ltlsi_->getProdGraphState(start);
49 
51  OMPL_WARN("Multiple start states given. Using only the first start state.");
52 
53  Motion* startMotion = new Motion(ltlsi_);
54  si_->copyState(startMotion->state, start);
55  ltlsi_->nullControl(startMotion->control);
56  startMotion->abstractState = prodStart_;
57 
58  motions_.push_back(startMotion);
59  abstractInfo_[prodStart_].addMotion(startMotion);
62 
63  abstraction_->buildGraph(prodStart_, boost::bind(&LTLPlanner::initAbstractInfo, this, _1));
64 
65  if (!sampler_)
66  sampler_ = si_->allocStateSampler();
67  if (!controlSampler_)
69 
70  bool solved = false;
71  Motion* soln;
72 
73  while (ptc()==false && !solved)
74  {
75  const std::vector<ProductGraph::State*> lead = abstraction_->computeLead(prodStart_, boost::bind(&LTLPlanner::abstractEdgeWeight, this, _1, _2));
76  buildAvail(lead);
77  solved = explore(lead, soln, exploreTime_);
78  }
79 
80  if (solved)
81  {
82  //build solution path
83  std::vector<Motion*> path;
84  while (soln != NULL)
85  {
86  path.push_back(soln);
87  soln = soln->parent;
88  }
89  PathControl* pc = new PathControl(si_);
90  for (int i = path.size()-1; i >= 0; --i)
91  {
92  if (path[i]->parent != NULL) {
93  pc->append(path[i]->state, path[i]->control, path[i]->steps * ltlsi_->getPropagationStepSize());
94  }
95  else {
96  pc->append(path[i]->state);
97  }
98  }
99  pdef_->addSolutionPath(base::PathPtr(pc));
100  }
101 
102  OMPL_INFORM("Created %u states", motions_.size());
103  return base::PlannerStatus(solved, false);
104 }
105 
106 void ompl::control::LTLPlanner::getTree(std::vector<base::State*>& tree) const
107 {
108  tree.resize(motions_.size());
109  for (unsigned int i = 0; i < motions_.size(); ++i)
110  tree[i] = motions_[i]->state;
111 }
112 
113 std::vector<ompl::control::ProductGraph::State*> ompl::control::LTLPlanner::getHighLevelPath(const std::vector<base::State*>& path, ProductGraph::State* start) const
114 {
115  std::vector<ProductGraph::State*> hlPath(path.size());
116  hlPath[0] = (start != NULL ? start : ltlsi_->getProdGraphState(path[0]));
117  for (unsigned int i = 1; i < path.size(); ++i)
118  {
119  hlPath[i] = ltlsi_->getProdGraphState(path[i]);
120  if (!hlPath[i]->isValid())
121  OMPL_WARN("High-level path fails automata");
122  }
123  return hlPath;
124 }
125 
126 ompl::control::LTLPlanner::Motion::Motion(void) : state(NULL), control(NULL), parent(NULL), steps(0)
127 {
128 }
129 
131  state(si->allocState()),
132  control(si->allocControl()),
133  parent(NULL),
134  steps(0)
135 {
136 }
137 
139 {
140 }
141 
143  numSel(0),
144  pdfElem(NULL)
145 {
146 }
147 
149 {
150  motionElems[m] = motions.add(m, 1.);
151 }
152 
154 {
156  /* TODO weight should include freeVolume, for cases in which decomposition
157  does not respect obstacles. */
158  info.weight = ((info.motions.size()+1)*info.volume) / (info.autWeight*(info.numSel+1)*(info.numSel+1));
159  return info.weight;
160 }
161 
163 {
165  info.numSel = 0;
166  info.pdfElem = NULL;
167  info.volume = abstraction_->getRegionVolume(as);
168  unsigned int autDist = std::max(abstraction_->getCosafeAutDistance(as),
169  abstraction_->getSafeAutDistance(as));
170  //TODO try something larger than epsilon
171  if (autDist == 0)
172  info.autWeight = std::numeric_limits<double>::epsilon();
173  else
174  info.autWeight = autDist;
175  info.weight = info.volume/info.autWeight;
176 }
177 
178 void ompl::control::LTLPlanner::buildAvail(const std::vector<ProductGraph::State*>& lead)
179 {
180  for (unsigned int i = 0; i < availDist_.size(); ++i)
181  abstractInfo_[availDist_[i]].pdfElem = NULL;
182  availDist_.clear();
183  unsigned int numTreePts = 1;
184  for (int i = lead.size()-1; i >= 0; --i)
185  {
186  ProductGraph::State* as = lead[i];
188  if (!info.motions.empty())
189  {
190  info.pdfElem = availDist_.add(as, info.weight);
191  numTreePts += info.motions.size();
192  if (rng_.uniform01() < 0.5)
193  break;
194  }
195  }
196 }
197 
198 bool ompl::control::LTLPlanner::explore(const std::vector<ProductGraph::State*>& lead, Motion*& soln, double duration)
199 {
200  bool solved = false;
202  base::GoalPtr goal = pdef_->getGoal();
203  while (!ptc() && !solved)
204  {
206  ++abstractInfo_[as].numSel;
207  updateWeight(as);
208 
209  PDF<Motion*>& motions = abstractInfo_[as].motions;
210  Motion* v = motions.sample(rng_.uniform01());
211  PDF<Motion*>::Element* velem = abstractInfo_[as].motionElems[v];
212  double vweight = motions.getWeight(velem);
213  if (vweight > 1e-20)
214  motions.update(velem, vweight/(vweight+1.));
215 
216  Control* rctrl = ltlsi_->allocControl();
217  controlSampler_->sampleNext(rctrl, v->control, v->state);
218  unsigned int cd = controlSampler_->sampleStepCount(ltlsi_->getMinControlDuration(), ltlsi_->getMaxControlDuration());
219 
220  base::State* newState = si_->allocState();
221  cd = ltlsi_->propagateWhileValid(v->state, rctrl, cd, newState);
222  if (cd < ltlsi_->getMinControlDuration())
223  {
224  si_->freeState(newState);
225  ltlsi_->freeControl(rctrl);
226  continue;
227  }
228  Motion* m = new Motion();
229  m->state = newState;
230  m->control = rctrl;
231  m->steps = cd;
232  m->parent = v;
233  // Since the state was determined to be valid by SpaceInformation, we don't need to check automaton states
234  m->abstractState = ltlsi_->getProdGraphState(m->state);
235  motions_.push_back(m);
236 
237  abstractInfo_[m->abstractState].addMotion(m);
239  // update weight if hl state already exists in avail
240  if (abstractInfo_[m->abstractState].pdfElem != NULL)
241  availDist_.update(abstractInfo_[m->abstractState].pdfElem, abstractInfo_[m->abstractState].weight);
242  else
243  {
244  // otherwise, only add hl state to avail if it already exists in lead
245  if (std::find(lead.begin(), lead.end(), m->abstractState) != lead.end())
246  {
248  abstractInfo_[m->abstractState].pdfElem = elem;
249  }
250  }
251 
252  solved = goal->isSatisfied(m->state);
253  if (solved)
254  {
255  soln = m;
256  break;
257  }
258  }
259  return solved;
260 }
261 
263 {
264  const ProductGraphStateInfo& infoA = abstractInfo_.find(a)->second;
265  const ProductGraphStateInfo& infoB = abstractInfo_.find(b)->second;
266  return 1./(infoA.weight * infoB.weight);
267 }
268 
269 void ompl::control::LTLPlanner::clearMotions(void)
270 {
271  availDist_.clear();
272  for (std::vector<Motion*>::iterator i = motions_.begin(); i != motions_.end(); ++i)
273  {
274  Motion* m = *i;
275  if (m->state != NULL)
276  si_->freeState(m->state);
277  if (m->control != NULL)
279  delete m;
280  }
281  motions_.clear();
282  pis_.clear();
283  pis_.update();
284 }
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
Definition: Planner.h:214
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
double getPropagationStepSize() const
Propagation is performed at integer multiples of a specified step size. This function returns the val...
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
unsigned int getMinControlDuration() const
Get the minimum number of steps a control is propagated for.
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
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
unsigned int propagateWhileValid(const base::State *state, const Control *control, int steps, base::State *result) const
Propagate the model of the system forward, starting at a given state, with a given control...
void append(const base::State *state)
Append state to the end of the path; it is assumed state is the first state, so no control is applied...
void nullControl(Control *control) const
Make the control have no effect if it were to be applied to a state for any amount of time...
void getTree(std::vector< base::State *> &tree) const
Helper debug method to access this planner&#39;s underlying tree of states.
Definition: LTLPlanner.cpp:106
Definition of an abstract control.
Definition: Control.h:48
ProductGraph::State * prodStart_
Start state in product graph.
Definition: LTLPlanner.h:194
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
ControlSamplerPtr allocControlSampler() const
Allocate a control sampler.
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: Planner.cpp:112
Definition of a control path.
Definition: PathControl.h:60
Control * allocControl() const
Allocate memory for a control.
ProblemDefinitionPtr pdef_
The user set problem definition.
Definition: Planner.h:400
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
bool haveMoreStartStates() const
Check if there are more potential start states.
Definition: Planner.cpp:345
A container that supports probabilistic sampling over weighted data.
Definition: PDF.h:48
double uniform01()
Generate a random real between 0 and 1.
Definition: RandomNumbers.h:62
A boost shared pointer wrapper for ompl::control::LTLSpaceInformation.
ControlSamplerPtr controlSampler_
Control sampler.
Definition: LTLPlanner.h:176
PlannerTerminationCondition timedPlannerTerminationCondition(double duration)
Return a termination condition that will become true duration seconds in the future (wall-time) ...
Main namespace. Contains everything in this library.
Definition: Cost.h:42
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
double getWeight(const Element *elem) const
Returns the current weight of the given Element.
Definition: PDF.h:171
unsigned int getMaxControlDuration() const
Get the maximum number of steps a control is propagated for.
RNG rng_
A random number generator.
Definition: LTLPlanner.h:188
void clear()
Clear all stored information.
Definition: Planner.cpp:157
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: Planner.cpp:86
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 checkValidity()
Check to see if the planner is in a working state (setup has been called, a goal was set...
Definition: Planner.cpp:100
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
PlannerInputStates pis_
Utility class to extract valid input states.
Definition: Planner.h:403
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
ProductGraphPtr abstraction_
The high level abstaction used to grow the tree structure.
Definition: LTLPlanner.h:182
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition: Planner.h:409
const State * nextStart()
Return the next valid start state or NULL if no more valid start states are available.
Definition: Planner.cpp:230
void update(Element *elem, const double w)
Updates the data in the given Element with a new weight value.
Definition: PDF.h:155
_T & sample(double r) const
Returns a piece of data from the PDF according to the input sampling value, which must be between 0 a...
Definition: PDF.h:132
bool update()
Set the space information and problem definition this class operates on, based on the available plann...
Definition: Planner.cpp:176
A boost shared pointer wrapper for ompl::control::ProductGraph.
virtual void setup(void)
Initializes LTLPlanner data structures.
Definition: LTLPlanner.cpp:30
A boost shared pointer wrapper for ompl::base::Goal.
void freeControl(Control *control) const
Free the memory of a control.
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
virtual ~LTLPlanner(void)
Clears all memory belonging to this LTLPlanner .
Definition: LTLPlanner.cpp:25
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:397
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
A boost shared pointer wrapper for ompl::base::Path.
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
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
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