KPIECE1.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, 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: Ioan Sucan */
36 
37 #ifndef OMPL_CONTROL_PLANNERS_KPIECE_KPIECE1_
38 #define OMPL_CONTROL_PLANNERS_KPIECE_KPIECE1_
39 
40 #include "ompl/control/planners/PlannerIncludes.h"
41 #include "ompl/base/ProjectionEvaluator.h"
42 #include "ompl/datastructures/GridB.h"
43 #include <vector>
44 #include <set>
45 
46 namespace ompl
47 {
48 
49  namespace control
50  {
51 
77  class KPIECE1 : public base::Planner
78  {
79  public:
80 
82  KPIECE1(const SpaceInformationPtr &si);
83 
84  virtual ~KPIECE1();
85 
87 
88  virtual void clear();
89 
97  void setGoalBias(double goalBias)
98  {
99  goalBias_ = goalBias;
100  }
101 
103  double getGoalBias() const
104  {
105  return goalBias_;
106  }
107 
114  void setBorderFraction(double bp)
115  {
117  }
118 
121  double getBorderFraction() const
122  {
123  return selectBorderFraction_;
124  }
125 
132  void setCellScoreFactor(double good, double bad)
133  {
136  }
137 
139  void setBadCellScoreFactor(double bad)
140  {
141  badScoreFactor_ = bad;
142  }
143 
145  void setGoodCellScoreFactor(double good)
146  {
147  goodScoreFactor_ = good;
148  }
149 
152  double getGoodCellScoreFactor() const
153  {
154  return goodScoreFactor_;
155  }
156 
159  double getBadCellScoreFactor() const
160  {
161  return badScoreFactor_;
162  }
163 
166  void setMaxCloseSamplesCount(unsigned int nCloseSamples)
167  {
168  nCloseSamples_ = nCloseSamples;
169  }
170 
172  unsigned int getMaxCloseSamplesCount() const
173  {
174  return nCloseSamples_;
175  }
176 
179  void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
180  {
181  projectionEvaluator_ = projectionEvaluator;
182  }
183 
186  void setProjectionEvaluator(const std::string &name)
187  {
188  projectionEvaluator_ = si_->getStateSpace()->getProjection(name);
189  }
190 
193  {
194  return projectionEvaluator_;
195  }
196 
197  virtual void setup();
198  virtual void getPlannerData(base::PlannerData &data) const;
199 
200  protected:
201 
203  struct Motion
204  {
205  Motion() : state(NULL), control(NULL), steps(0), parent(NULL)
206  {
207  }
208 
210  Motion(const SpaceInformation *si) : state(si->allocState()), control(si->allocControl()), steps(0), parent(NULL)
211  {
212  }
213 
214  ~Motion()
215  {
216  }
217 
220 
223 
225  unsigned int steps;
226 
229  };
230 
232  struct CellData
233  {
234  CellData() : coverage(0.0), selections(1), score(1.0), iteration(0), importance(0.0)
235  {
236  }
237 
238  ~CellData()
239  {
240  }
241 
243  std::vector<Motion*> motions;
244 
248  double coverage;
249 
252  unsigned int selections;
253 
257  double score;
258 
260  unsigned int iteration;
261 
263  double importance;
264  };
265 
269  {
270  bool operator()(const CellData * const a, const CellData * const b) const
271  {
272  return a->importance > b->importance;
273  }
274  };
275 
278 
280  struct CloseSample
281  {
283  CloseSample(Grid::Cell *c, Motion *m, double d) : cell(c), motion(m), distance(d)
284  {
285  }
286 
289 
292 
294  double distance;
295 
297  bool operator<(const CloseSample &other) const
298  {
299  return distance < other.distance;
300  }
301  };
302 
305  {
307  CloseSamples(unsigned int size) : maxSize(size)
308  {
309  }
310 
316  bool consider(Grid::Cell *cell, Motion *motion, double distance);
317 
323  bool selectMotion(Motion* &smotion, Grid::Cell* &scell);
324 
326  bool canSample() const
327  {
328  return samples.size() > 0;
329  }
330 
332  unsigned int maxSize;
333 
335  std::set<CloseSample> samples;
336  };
337 
338 
340  struct TreeData
341  {
342  TreeData() : grid(0), size(0), iteration(1)
343  {
344  }
345 
348  Grid grid;
349 
352  unsigned int size;
353 
355  unsigned int iteration;
356  };
357 
361  static void computeImportance(Grid::Cell *cell, void*)
362  {
363  CellData &cd = *(cell->data);
364  cd.importance = cd.score / ((cell->neighbors + 1) * cd.coverage * cd.selections);
365  }
366 
368  void freeMemory();
369 
371  void freeGridMotions(Grid &grid);
372 
374  void freeCellData(CellData *cdata);
375 
377  void freeMotion(Motion *motion);
378 
384  Grid::Cell* addMotion(Motion *motion, double dist);
385 
389  bool selectMotion(Motion* &smotion, Grid::Cell* &scell);
390 
394  unsigned int findNextMotion(const std::vector<Grid::Coord> &coords, unsigned int index, unsigned int count);
395 
398 
401 
404 
409 
414 
419 
423  unsigned int nCloseSamples_;
424 
428 
430  double goalBias_;
431 
434 
437  };
438 
439  }
440 }
441 
442 #endif
CloseSample(Grid::Cell *c, Motion *m, double d)
Constructor fully initializes the content of this structure.
Definition: KPIECE1.h:283
void setBadCellScoreFactor(double bad)
Set the factor that is to be applied to a cell&#39;s score when an expansion from that cell fails...
Definition: KPIECE1.h:139
double distance
The distance to the goal. This value is increased over time, as the number of selections for this sam...
Definition: KPIECE1.h:294
bool operator<(const CloseSample &other) const
Sort samples in accordance to their distance to the goal.
Definition: KPIECE1.h:297
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:164
void setGoodCellScoreFactor(double good)
Set the factor that is to be applied to a cell&#39;s score when an expansion from that cell succeedes...
Definition: KPIECE1.h:145
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition: KPIECE1.cpp:178
void setCellScoreFactor(double good, double bad)
When extending a motion from a cell, the extension can be successful or it can fail. If the extension is successful, the score of the cell is multiplied by good. If the extension fails, the score of the cell is multiplied by bad. These numbers should be in the range (0, 1].
Definition: KPIECE1.h:132
Bounded set of good samples.
Definition: KPIECE1.h:304
double selectBorderFraction_
The fraction of time to focus exploration on the border of the grid.
Definition: KPIECE1.h:427
double getBorderFraction() const
Get the fraction of time to focus exploration on boundary.
Definition: KPIECE1.h:121
void setGoalBias(double goalBias)
Definition: KPIECE1.h:97
bool canSample() const
Return true if samples can be selected from this set.
Definition: KPIECE1.h:326
void setProjectionEvaluator(const std::string &name)
Set the projection evaluator (select one from the ones registered with the state space).
Definition: KPIECE1.h:186
TreeData tree_
The tree datastructure.
Definition: KPIECE1.h:400
Definition of an abstract control.
Definition: Control.h:48
double getBadCellScoreFactor() const
Get the factor that is multiplied to a cell&#39;s score if extending a motion from that cell failed...
Definition: KPIECE1.h:159
GridB< CellData *, OrderCellsByImportance > Grid
The datatype for the maintained grid datastructure.
Definition: KPIECE1.h:277
double goalBias_
The fraction of time the goal is picked as the state to expand towards (if such a state is available)...
Definition: KPIECE1.h:430
Definintion of an operator passed to the Grid structure, to order cells by importance.
Definition: KPIECE1.h:268
A boost shared pointer wrapper for ompl::control::ControlSampler.
unsigned int iteration
The iteration at which this cell was created.
Definition: KPIECE1.h:260
double importance
The computed importance (based on other class members)
Definition: KPIECE1.h:263
double score
A heuristic score computed based on distance to goal (if available), successes and failures at expand...
Definition: KPIECE1.h:257
double getGoalBias() const
Definition: KPIECE1.h:103
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
_T data
The data we store in the cell.
Definition: Grid.h:62
double badScoreFactor_
When extending a motion from a cell, the extension can fail. If it is, the score of the cell is multi...
Definition: KPIECE1.h:418
unsigned int findNextMotion(const std::vector< Grid::Coord > &coords, unsigned int index, unsigned int count)
When generated motions are to be added to the tree of motions, they often need to be split...
Definition: KPIECE1.cpp:169
virtual void clear()
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: KPIECE1.cpp:85
unsigned int size
The total number of motions (there can be multiple per cell) in the grid.
Definition: KPIECE1.h:352
static void computeImportance(Grid::Cell *cell, void *)
This function is provided as a calback to the grid datastructure to update the importance of a cell...
Definition: KPIECE1.h:361
std::vector< Motion * > motions
The set of motions contained in this grid cell.
Definition: KPIECE1.h:243
void setMaxCloseSamplesCount(unsigned int nCloseSamples)
When motions reach close to the goal, they are stored in a separate queue to allow biasing towards th...
Definition: KPIECE1.h:166
Grid::Cell * addMotion(Motion *motion, double dist)
Add a motion to the grid containing motions. As a hint, dist specifies the distance to the goal from ...
Definition: KPIECE1.cpp:386
void freeMotion(Motion *motion)
Free the memory for a motion.
Definition: KPIECE1.cpp:114
void freeMemory()
Free all the memory allocated by this planner.
Definition: KPIECE1.cpp:96
void setBorderFraction(double bp)
Set the fraction of time for focusing on the border (between 0 and 1). This is the minimum fraction u...
Definition: KPIECE1.h:114
base::ProjectionEvaluatorPtr projectionEvaluator_
This algorithm uses a discretization (a grid) to guide the exploration. The exploration is imposed on...
Definition: KPIECE1.h:408
unsigned int getMaxCloseSamplesCount() const
Get the maximum number of samples to store in the queue of samples that are close to the goal...
Definition: KPIECE1.h:172
Motion * motion
The motion that is close to the goal.
Definition: KPIECE1.h:291
Main namespace. Contains everything in this library.
Definition: Cost.h:42
double goodScoreFactor_
When extending a motion from a cell, the extension can be successful. If it is, the score of the cell...
Definition: KPIECE1.h:413
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: KPIECE1.cpp:412
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:54
virtual void setup()
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: KPIECE1.cpp:69
Representation of a motion for this algorithm.
Definition: KPIECE1.h:203
Base class for a planner.
Definition: Planner.h:232
Motion * parent
The parent motion in the exploration tree.
Definition: KPIECE1.h:228
The data defining a tree of motions for this algorithm.
Definition: KPIECE1.h:340
unsigned int maxSize
Maximum number of samples to maintain.
Definition: KPIECE1.h:332
Information about a known good sample (closer to the goal than others)
Definition: KPIECE1.h:280
double coverage
A measure of coverage for this cell. For this implementation, this is the sum of motion durations...
Definition: KPIECE1.h:248
A boost shared pointer wrapper for ompl::base::ProjectionEvaluator.
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:48
void freeGridMotions(Grid &grid)
Free the memory for the motions contained in a grid.
Definition: KPIECE1.cpp:101
KPIECE1(const SpaceInformationPtr &si)
Constructor.
Definition: KPIECE1.cpp:44
Definition of an abstract state.
Definition: State.h:50
ControlSamplerPtr controlSampler_
A control sampler.
Definition: KPIECE1.h:397
unsigned int selections
The number of times this cell has been selected for expansion.
Definition: KPIECE1.h:252
Kinodynamic Planning by Interior-Exterior Cell Exploration.
Definition: KPIECE1.h:77
A boost shared pointer wrapper for ompl::control::SpaceInformation.
void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
Set the projection evaluator. This class is able to compute the projection of a given state...
Definition: KPIECE1.h:179
std::set< CloseSample > samples
The maintained samples.
Definition: KPIECE1.h:335
Definition of a cell in this grid.
Definition: Grid.h:59
double getGoodCellScoreFactor() const
Get the factor that is multiplied to a cell&#39;s score if extending a motion from that cell succeeded...
Definition: KPIECE1.h:152
The data held by a cell in the grid of motions.
Definition: KPIECE1.h:232
Motion(const SpaceInformation *si)
Constructor that allocates memory for the state and the control.
Definition: KPIECE1.h:210
Grid grid
A grid containing motions, imposed on a projection of the state space.
Definition: KPIECE1.h:348
bool selectMotion(Motion *&smotion, Grid::Cell *&scell)
Select a motion and the cell it is part of from the grid of motions. This is where preference is give...
Definition: KPIECE1.cpp:353
unsigned int iteration
The number of iterations performed on this tree.
Definition: KPIECE1.h:355
RNG rng_
The random number generator.
Definition: KPIECE1.h:433
Grid::Cell * cell
The cell of the motion that is close to the goal.
Definition: KPIECE1.h:288
CloseSamples(unsigned int size)
Construct an object to maintain a set of at most size samples.
Definition: KPIECE1.h:307
Control * control
The control contained by this motion.
Definition: KPIECE1.h:222
const SpaceInformation * siC_
The base::SpaceInformation cast as control::SpaceInformation, for convenience.
Definition: KPIECE1.h:403
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:397
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition: KPIECE1.h:436
Space information containing necessary information for planning with controls. setup() needs to be ca...
base::State * state
The state contained by this motion.
Definition: KPIECE1.h:219
Definition of a cell in this grid.
Definition: GridN.h:61
void freeCellData(CellData *cdata)
Free the memory for the data contained in a grid cell.
Definition: KPIECE1.cpp:107
unsigned int steps
The number of steps the control is applied for.
Definition: KPIECE1.h:225
const base::ProjectionEvaluatorPtr & getProjectionEvaluator() const
Get the projection evaluator.
Definition: KPIECE1.h:192
unsigned int nCloseSamples_
When motions reach close to the goal, they are stored in a separate queue to allow biasing towards th...
Definition: KPIECE1.h:423