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() = default;
203 
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 
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 
279 
282 
285  double distance;
286 
288  bool operator<(const CloseSample &other) const
289  {
290  return distance < other.distance;
291  }
292  };
293 
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 
400 
404  double goodScoreFactor_{0.9};
405 
409  double badScoreFactor_{0.45};
410 
414  unsigned int nCloseSamples_{30};
415 
419 
422  double goalBias_{0.05};
423 
426 
429  };
430  }
431 }
432 
433 #endif
CloseSample(Grid::Cell *c, Motion *m, double d)
Constructor fully initializes the content of this structure.
Definition: KPIECE1.h:273
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:285
bool operator<(const CloseSample &other) const
Sort samples in accordance to their distance to the goal.
Definition: KPIECE1.h:288
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:295
double selectBorderFraction_
The fraction of time to focus exploration on the border of the grid.
Definition: KPIECE1.h:418
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:317
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:391
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:422
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition: KPIECE1.cpp:83
Definintion of an operator passed to the Grid structure, to order cells by importance.
Definition: KPIECE1.h:258
A shared pointer wrapper for ompl::control::ControlSampler.
typename GridN< _T >::Cell Cell
Definition of a cell in this grid.
Definition: GridB.h:52
double importance
The computed importance (based on other class members)
Definition: KPIECE1.h:253
double score
A heuristic score computed based on distance to goal (if available), successes and failures at expand...
Definition: KPIECE1.h:247
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:409
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
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:349
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:385
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
void freeMotion(Motion *motion)
Free the memory for a motion.
Definition: KPIECE1.cpp:112
void freeMemory()
Free all the memory allocated by this planner.
Definition: KPIECE1.cpp:94
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
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:399
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:281
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:404
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:223
Motion * parent
The parent motion in the exploration tree.
Definition: KPIECE1.h:222
The data defining a tree of motions for this algorithm.
Definition: KPIECE1.h:330
unsigned int maxSize
Maximum number of samples to maintain.
Definition: KPIECE1.h:323
Information about a known good sample (closer to the goal than others)
Definition: KPIECE1.h:270
double coverage
A measure of coverage for this cell. For this implementation, this is the sum of motion durations...
Definition: KPIECE1.h:238
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Definition: KPIECE1.cpp:67
std::vector< Motion * > motions
The set of motions contained in this grid cell.
Definition: KPIECE1.h:233
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:99
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:388
unsigned int selections
The number of times this cell has been selected for expansion.
Definition: KPIECE1.h:242
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:326
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:226
Motion(const SpaceInformation *si)
Constructor that allocates memory for the state and the control.
Definition: KPIECE1.h:205
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
RNG rng_
The random number generator.
Definition: KPIECE1.h:425
Grid::Cell * cell
The cell of the motion that is close to the goal.
Definition: KPIECE1.h:278
CloseSamples(unsigned int size)
Construct an object to maintain a set of at most size samples.
Definition: KPIECE1.h:298
Control * control
The control contained by this motion.
Definition: KPIECE1.h:216
const SpaceInformation * siC_
The base::SpaceInformation cast as control::SpaceInformation, for convenience.
Definition: KPIECE1.h:394
SpaceInformationPtr si_
The space information for which planning is done.
Definition: Planner.h:406
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
Definition: KPIECE1.h:428
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:267
base::State * state
The state contained by this motion.
Definition: KPIECE1.h:213
void freeCellData(CellData *cdata)
Free the memory for the data contained in a grid cell.
Definition: KPIECE1.cpp:105
unsigned int steps
The number of steps the control is applied for.
Definition: KPIECE1.h:219
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:414