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 
83  base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc) override;
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 
190  const base::ProjectionEvaluatorPtr &getProjectionEvaluator() const
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() = default;
203 
205  Motion(const SpaceInformation *si)
206  : state(si->allocState()), control(si->allocControl())
207  {
208  }
209 
210  ~Motion() = default;
211 
213  base::State *state{nullptr};
214 
216  Control *control{nullptr};
217 
219  unsigned int steps{0};
220 
222  Motion *parent{nullptr};
223  };
224 
226  struct CellData
227  {
228  CellData() = default;
229 
230  ~CellData() = default;
231 
233  std::vector<Motion *> motions;
234 
238  double coverage{0.0};
239 
242  unsigned int selections{1};
243 
247  double score{1.0};
248 
250  unsigned int iteration{0};
251 
253  double importance{0.0};
254  };
255 
258  struct OrderCellsByImportance
259  {
260  bool operator()(const CellData *const a, const CellData *const b) const
261  {
262  return a->importance > b->importance;
263  }
264  };
265 
268 
270  struct CloseSample
271  {
273  CloseSample(Grid::Cell *c, Motion *m, double d) : cell(c), motion(m), distance(d)
274  {
275  }
276 
278  Grid::Cell *cell;
279 
281  Motion *motion;
282 
285  double distance;
286 
288  bool operator<(const CloseSample &other) const
289  {
290  return distance < other.distance;
291  }
292  };
293 
295  struct CloseSamples
296  {
298  CloseSamples(unsigned int size) : maxSize(size)
299  {
300  }
301 
307  bool consider(Grid::Cell *cell, Motion *motion, double distance);
308 
314  bool selectMotion(Motion *&smotion, Grid::Cell *&scell);
315 
317  bool canSample() const
318  {
319  return !samples.empty();
320  }
321 
323  unsigned int maxSize;
324 
326  std::set<CloseSample> samples;
327  };
328 
330  struct TreeData
331  {
332  TreeData() = default;
333 
336  Grid grid{0};
337 
340  unsigned int size{0};
341 
343  unsigned int iteration{1};
344  };
345 
349  static void computeImportance(Grid::Cell *cell, void * /*unused*/)
350  {
351  CellData &cd = *(cell->data);
352  cd.importance = cd.score / ((cell->neighbors + 1) * cd.coverage * cd.selections);
353  }
354 
356  void freeMemory();
357 
359  void freeGridMotions(Grid &grid);
360 
362  void freeCellData(CellData *cdata);
363 
365  void freeMotion(Motion *motion);
366 
372  Grid::Cell *addMotion(Motion *motion, double dist);
373 
377  bool selectMotion(Motion *&smotion, Grid::Cell *&scell);
378 
385  unsigned int findNextMotion(const std::vector<Grid::Coord> &coords, unsigned int index, unsigned int count);
386 
389 
392 
395 
399  base::ProjectionEvaluatorPtr projectionEvaluator_;
400 
404  double goodScoreFactor_{0.9};
405 
409  double badScoreFactor_{0.45};
410 
414  unsigned int nCloseSamples_{30};
415 
418  double selectBorderFraction_{0.8};
419 
422  double goalBias_{0.05};
423 
425  RNG rng_;
426 
428  Motion *lastGoalMotion_{nullptr};
429  };
430  }
431 }
432 
433 #endif
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:89
Control * control
The control contained by this motion.
Definition: KPIECE1.h:312
Information about a known good sample (closer to the goal than others)
Definition: KPIECE1.h:366
void freeCellData(CellData *cdata)
Free the memory for the data contained in a grid cell.
Definition: KPIECE1.cpp:105
bool canSample() const
Return true if samples can be selected from this set.
Definition: KPIECE1.h:413
Definition of an abstract control.
Definition: Control.h:111
A shared pointer wrapper for ompl::base::SpaceInformation.
double coverage
A measure of coverage for this cell. For this implementation, this is the sum of motion durations.
Definition: KPIECE1.h:334
base::State * state
The state contained by this motion.
Definition: KPIECE1.h:309
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:518
std::vector< Motion * > motions
The set of motions contained in this grid cell.
Definition: KPIECE1.h:329
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:260
void setGoodCellScoreFactor(double good)
Set the factor that is to be applied to a cell's score when an expansion from that cell succeedes.
Definition: KPIECE1.h:239
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition: KPIECE1.h:524
Definition of an abstract state.
Definition: State.h:113
RNG rng_
The random number generator.
Definition: KPIECE1.h:521
double importance
The computed importance (based on other class members)
Definition: KPIECE1.h:349
CloseSample(Grid::Cell *c, Motion *m, double d)
Constructor fully initializes the content of this structure.
Definition: KPIECE1.h:369
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:385
double getGoalBias() const
Definition: KPIECE1.h:196
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:207
double getGoodCellScoreFactor() const
Get the factor that is multiplied to a cell's score if extending a motion from that cell succeeded.
Definition: KPIECE1.h:246
std::set< CloseSample > samples
The maintained samples.
Definition: KPIECE1.h:422
Motion * parent
The parent motion in the exploration tree.
Definition: KPIECE1.h:318
double score
A heuristic score computed based on distance to goal (if available), successes and failures at expand...
Definition: KPIECE1.h:343
Grid grid
A grid containing motions, imposed on a projection of the state space.
Definition: KPIECE1.h:432
unsigned int size
The total number of motions (there can be multiple per cell) in the grid.
Definition: KPIECE1.h:436
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:445
bool operator<(const CloseSample &other) const
Sort samples in accordance to their distance to the goal.
Definition: KPIECE1.h:384
unsigned int iteration
The iteration at which this cell was created.
Definition: KPIECE1.h:346
CloseSamples(unsigned int size)
Construct an object to maintain a set of at most size samples.
Definition: KPIECE1.h:394
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:411
double getBadCellScoreFactor() const
Get the factor that is multiplied to a cell's score if extending a motion from that cell failed.
Definition: KPIECE1.h:253
bool consider(Grid::Cell *cell, Motion *motion, double distance)
Evaluate whether motion motion, part of cell cell is good enough to be part of the set of samples clo...
Definition: KPIECE1.cpp:121
unsigned int selections
The number of times this cell has been selected for expansion.
Definition: KPIECE1.h:338
void setCellScoreFactor(double good, double bad)
When extending a motion from a cell, the extension can be successful or it can fail....
Definition: KPIECE1.h:225
TreeData tree_
The tree datastructure.
Definition: KPIECE1.h:487
double selectBorderFraction_
The fraction of time to focus exploration on the border of the grid.
Definition: KPIECE1.h:514
base::ProjectionEvaluatorPtr projectionEvaluator_
This algorithm uses a discretization (a grid) to guide the exploration. The exploration is imposed on...
Definition: KPIECE1.h:495
void freeMotion(Motion *motion)
Free the memory for a motion.
Definition: KPIECE1.cpp:112
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Definition: PlannerData.h:238
Motion * motion
The motion that is close to the goal.
Definition: KPIECE1.h:377
double distance
The distance to the goal. This value is increased over time, as the number of selections for this sam...
Definition: KPIECE1.h:381
Representation of a motion for this algorithm.
Definition: KPIECE1.h:296
The data defining a tree of motions for this algorithm.
Definition: KPIECE1.h:426
void freeGridMotions(Grid &grid)
Free the memory for the motions contained in a grid.
Definition: KPIECE1.cpp:99
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:177
bool selectMotion(Motion *&smotion, Grid::Cell *&scell)
Select the top sample (closest to the goal) and update its position in the set subsequently (pretend ...
Definition: KPIECE1.cpp:150
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:266
void setBadCellScoreFactor(double bad)
Set the factor that is to be applied to a cell's score when an expansion from that cell fails.
Definition: KPIECE1.h:232
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:352
typename GridN< CellData * >::Cell Cell
Definition of a cell in this grid.
Definition: GridB.h:119
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: KPIECE1.cpp:67
void setGoalBias(double goalBias)
Definition: KPIECE1.h:190
Definition of a cell in this grid.
Definition: Grid.h:122
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:167
The data held by a cell in the grid of motions.
Definition: KPIECE1.h:322
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:510
unsigned int iteration
The number of iterations performed on this tree.
Definition: KPIECE1.h:439
A shared pointer wrapper for ompl::control::ControlSampler.
Space information containing necessary information for planning with controls. setup() needs to be ca...
ControlSamplerPtr controlSampler_
A control sampler.
Definition: KPIECE1.h:484
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:505
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:273
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:474
KPIECE1(const SpaceInformationPtr &si)
Constructor.
Definition: KPIECE1.cpp:44
void freeMemory()
Free all the memory allocated by this planner.
Definition: KPIECE1.cpp:94
Grid::Cell * cell
The cell of the motion that is close to the goal.
Definition: KPIECE1.h:374
Representation of a simple grid.
Definition: Grid.h:83
unsigned int maxSize
Maximum number of samples to maintain.
Definition: KPIECE1.h:419
unsigned int steps
The number of steps the control is applied for.
Definition: KPIECE1.h:315
const SpaceInformation * siC_
The base::SpaceInformation cast as control::SpaceInformation, for convenience.
Definition: KPIECE1.h:490
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:500
Main namespace. Contains everything in this library.
const base::ProjectionEvaluatorPtr & getProjectionEvaluator() const
Get the projection evaluator.
Definition: KPIECE1.h:286
double getBorderFraction() const
Get the fraction of time to focus exploration on boundary.
Definition: KPIECE1.h:214
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: KPIECE1.cpp:83