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  namespace control
49  {
75  class KPIECE1 : public base::Planner
76  {
77  public:
79  KPIECE1(const SpaceInformationPtr &si);
80 
81  ~KPIECE1() override;
82 
84 
85  void clear() override;
86 
94  void setGoalBias(double goalBias)
95  {
96  goalBias_ = goalBias;
97  }
98 
100  double getGoalBias() const
101  {
102  return goalBias_;
103  }
104 
111  void setBorderFraction(double bp)
112  {
114  }
115 
118  double getBorderFraction() const
119  {
120  return selectBorderFraction_;
121  }
122 
129  void setCellScoreFactor(double good, double bad)
130  {
133  }
134 
136  void setBadCellScoreFactor(double bad)
137  {
138  badScoreFactor_ = bad;
139  }
140 
143  void setGoodCellScoreFactor(double good)
144  {
145  goodScoreFactor_ = good;
146  }
147 
150  double getGoodCellScoreFactor() const
151  {
152  return goodScoreFactor_;
153  }
154 
157  double getBadCellScoreFactor() const
158  {
159  return badScoreFactor_;
160  }
161 
164  void setMaxCloseSamplesCount(unsigned int nCloseSamples)
165  {
166  nCloseSamples_ = nCloseSamples;
167  }
168 
170  unsigned int getMaxCloseSamplesCount() const
171  {
172  return nCloseSamples_;
173  }
174 
177  void setProjectionEvaluator(const base::ProjectionEvaluatorPtr &projectionEvaluator)
178  {
179  projectionEvaluator_ = projectionEvaluator;
180  }
181 
184  void setProjectionEvaluator(const std::string &name)
185  {
186  projectionEvaluator_ = si_->getStateSpace()->getProjection(name);
187  }
188 
191  {
192  return projectionEvaluator_;
193  }
194 
195  void setup() override;
196  void getPlannerData(base::PlannerData &data) const override;
197 
198  protected:
200  struct Motion
201  {
202  Motion() : state(nullptr), control(nullptr), steps(0), parent(nullptr)
203  {
204  }
205 
208  : state(si->allocState()), control(si->allocControl()), steps(0), parent(nullptr)
209  {
210  }
211 
212  ~Motion() = default;
213 
216 
219 
221  unsigned int steps;
222 
225  };
226 
228  struct CellData
229  {
230  CellData() : coverage(0.0), selections(1), score(1.0), iteration(0), importance(0.0)
231  {
232  }
233 
234  ~CellData() = default;
235 
237  std::vector<Motion *> motions;
238 
242  double coverage;
243 
246  unsigned int selections;
247 
251  double score;
252 
254  unsigned int iteration;
255 
257  double importance;
258  };
259 
263  {
264  bool operator()(const CellData *const a, const CellData *const b) const
265  {
266  return a->importance > b->importance;
267  }
268  };
269 
272 
274  struct CloseSample
275  {
277  CloseSample(Grid::Cell *c, Motion *m, double d) : cell(c), motion(m), distance(d)
278  {
279  }
280 
283 
286 
289  double distance;
290 
292  bool operator<(const CloseSample &other) const
293  {
294  return distance < other.distance;
295  }
296  };
297 
300  {
302  CloseSamples(unsigned int size) : maxSize(size)
303  {
304  }
305 
311  bool consider(Grid::Cell *cell, Motion *motion, double distance);
312 
318  bool selectMotion(Motion *&smotion, Grid::Cell *&scell);
319 
321  bool canSample() const
322  {
323  return samples.size() > 0;
324  }
325 
327  unsigned int maxSize;
328 
330  std::set<CloseSample> samples;
331  };
332 
334  struct TreeData
335  {
336  TreeData() : grid(0), size(0), iteration(1)
337  {
338  }
339 
342  Grid grid;
343 
346  unsigned int size;
347 
349  unsigned int iteration;
350  };
351 
355  static void computeImportance(Grid::Cell *cell, void *)
356  {
357  CellData &cd = *(cell->data);
358  cd.importance = cd.score / ((cell->neighbors + 1) * cd.coverage * cd.selections);
359  }
360 
362  void freeMemory();
363 
365  void freeGridMotions(Grid &grid);
366 
368  void freeCellData(CellData *cdata);
369 
371  void freeMotion(Motion *motion);
372 
378  Grid::Cell *addMotion(Motion *motion, double dist);
379 
383  bool selectMotion(Motion *&smotion, Grid::Cell *&scell);
384 
391  unsigned int findNextMotion(const std::vector<Grid::Coord> &coords, unsigned int index, unsigned int count);
392 
395 
398 
401 
406 
411 
416 
420  unsigned int nCloseSamples_;
421 
425 
428  double goalBias_;
429 
432 
435  };
436  }
437 }
438 
439 #endif
CloseSample(Grid::Cell *c, Motion *m, double d)
Constructor fully initializes the content of this structure.
Definition: KPIECE1.h:277
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:136
double distance
The distance to the goal. This value is increased over time, as the number of selections for this sam...
Definition: KPIECE1.h:289
bool operator<(const CloseSample &other) const
Sort samples in accordance to their distance to the goal.
Definition: KPIECE1.h:292
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
Definition: PlannerData.h:174
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:143
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:129
Bounded set of good samples.
Definition: KPIECE1.h:299
double selectBorderFraction_
The fraction of time to focus exploration on the border of the grid.
Definition: KPIECE1.h:424
double getBorderFraction() const
Get the fraction of time to focus exploration on boundary.
Definition: KPIECE1.h:118
void setGoalBias(double goalBias)
Definition: KPIECE1.h:94
bool canSample() const
Return true if samples can be selected from this set.
Definition: KPIECE1.h:321
void setProjectionEvaluator(const std::string &name)
Set the projection evaluator (select one from the ones registered with the state space).
Definition: KPIECE1.h:184
TreeData tree_
The tree datastructure.
Definition: KPIECE1.h:397
Definition of an abstract control.
Definition: Control.h:47
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:157
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:428
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: KPIECE1.cpp:89
Definintion of an operator passed to the Grid structure, to order cells by importance.
Definition: KPIECE1.h:262
A shared pointer wrapper for ompl::control::ControlSampler.
typename GridN< _T >::Cell Cell
Definition of a cell in this grid.
Definition: GridB.h:52
unsigned int iteration
The iteration at which this cell was created.
Definition: KPIECE1.h:254
double importance
The computed importance (based on other class members)
Definition: KPIECE1.h:257
double score
A heuristic score computed based on distance to goal (if available), successes and failures at expand...
Definition: KPIECE1.h:251
double getGoalBias() const
Definition: KPIECE1.h:100
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:60
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:415
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:173
unsigned int size
The total number of motions (there can be multiple per cell) in the grid.
Definition: KPIECE1.h:346
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:355
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:164
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:391
base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override
Function that can solve the motion planning problem. This function can be called multiple times on th...
Definition: KPIECE1.cpp:183
void freeMotion(Motion *motion)
Free the memory for a motion.
Definition: KPIECE1.cpp:118
void freeMemory()
Free all the memory allocated by this planner.
Definition: KPIECE1.cpp:100
void getPlannerData(base::PlannerData &data) const override
Get information about the current run of the motion planner. Repeated calls to this function will upd...
Definition: KPIECE1.cpp:417
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:111
base::ProjectionEvaluatorPtr projectionEvaluator_
This algorithm uses a discretization (a grid) to guide the exploration. The exploration is imposed on...
Definition: KPIECE1.h:405
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:170
Motion * motion
The motion that is close to the goal.
Definition: KPIECE1.h:285
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
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:410
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:58
Representation of a motion for this algorithm.
Definition: KPIECE1.h:200
Base class for a planner.
Definition: Planner.h:232
Motion * parent
The parent motion in the exploration tree.
Definition: KPIECE1.h:224
The data defining a tree of motions for this algorithm.
Definition: KPIECE1.h:334
unsigned int maxSize
Maximum number of samples to maintain.
Definition: KPIECE1.h:327
Information about a known good sample (closer to the goal than others)
Definition: KPIECE1.h:274
double coverage
A measure of coverage for this cell. For this implementation, this is the sum of motion durations...
Definition: KPIECE1.h:242
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: KPIECE1.cpp:73
std::vector< Motion * > motions
The set of motions contained in this grid cell.
Definition: KPIECE1.h:237
A 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:105
KPIECE1(const SpaceInformationPtr &si)
Constructor.
Definition: KPIECE1.cpp:44
Definition of an abstract state.
Definition: State.h:49
ControlSamplerPtr controlSampler_
A control sampler.
Definition: KPIECE1.h:394
unsigned int selections
The number of times this cell has been selected for expansion.
Definition: KPIECE1.h:246
Kinodynamic Planning by Interior-Exterior Cell Exploration.
Definition: KPIECE1.h:75
A 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:177
std::set< CloseSample > samples
The maintained samples.
Definition: KPIECE1.h:330
Definition of a cell in this grid.
Definition: Grid.h:57
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:150
The data held by a cell in the grid of motions.
Definition: KPIECE1.h:228
Motion(const SpaceInformation *si)
Constructor that allocates memory for the state and the control.
Definition: KPIECE1.h:207
Grid grid
A grid containing motions, imposed on a projection of the state space.
Definition: KPIECE1.h:342
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:358
unsigned int iteration
The number of iterations performed on this tree.
Definition: KPIECE1.h:349
RNG rng_
The random number generator.
Definition: KPIECE1.h:431
Grid::Cell * cell
The cell of the motion that is close to the goal.
Definition: KPIECE1.h:282
CloseSamples(unsigned int size)
Construct an object to maintain a set of at most size samples.
Definition: KPIECE1.h:302
Control * control
The control contained by this motion.
Definition: KPIECE1.h:218
const SpaceInformation * siC_
The base::SpaceInformation cast as control::SpaceInformation, for convenience.
Definition: KPIECE1.h:400
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:415
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition: KPIECE1.h:434
Space information containing necessary information for planning with controls. setup() needs to be ca...
GridB< CellData *, OrderCellsByImportance > Grid
The datatype for the maintained grid datastructure.
Definition: KPIECE1.h:271
base::State * state
The state contained by this motion.
Definition: KPIECE1.h:215
void freeCellData(CellData *cdata)
Free the memory for the data contained in a grid cell.
Definition: KPIECE1.cpp:111
unsigned int steps
The number of steps the control is applied for.
Definition: KPIECE1.h:221
const base::ProjectionEvaluatorPtr & getProjectionEvaluator() const
Get the projection evaluator.
Definition: KPIECE1.h:190
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:420