Loading...
Searching...
No Matches
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
46namespace ompl
47{
48 namespace control
49 {
75 class KPIECE1 : public base::Planner
76 {
77 public:
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 {
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
151 {
152 return goodScoreFactor_;
153 }
154
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 {
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
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
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
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
419
422 double goalBias_{0.05};
423
426
429 };
430 }
431}
432
433#endif
typename GridN< _T >::Cell Cell
Definition of a cell in this grid.
Definition GridB.h:55
Representation of a simple grid.
Definition Grid.h:52
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique,...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
Base class for a planner.
Definition Planner.h:216
SpaceInformationPtr si_
The space information for which planning is done.
Definition Planner.h:410
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition State.h:50
A shared pointer wrapper for ompl::control::ControlSampler.
Definition of an abstract control.
Definition Control.h:48
Kinodynamic Planning by Interior-Exterior Cell Exploration.
Definition KPIECE1.h:76
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
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:143
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
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
TreeData tree_
The tree datastructure.
Definition KPIECE1.h:391
double getGoalBias() const
Definition KPIECE1.h:100
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
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:157
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
const SpaceInformation * siC_
The base::SpaceInformation cast as control::SpaceInformation, for convenience.
Definition KPIECE1.h:394
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 setProjectionEvaluator(const std::string &name)
Set the projection evaluator (select one from the ones registered with the state space).
Definition KPIECE1.h:184
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
RNG rng_
The random number generator.
Definition KPIECE1.h:425
double getBorderFraction() const
Get the fraction of time to focus exploration on boundary.
Definition KPIECE1.h:118
void freeMemory()
Free all the memory allocated by this planner.
Definition KPIECE1.cpp:94
ControlSamplerPtr controlSampler_
A control sampler.
Definition KPIECE1.h:388
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:150
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
const base::ProjectionEvaluatorPtr & getProjectionEvaluator() const
Get the projection evaluator.
Definition KPIECE1.h:190
base::ProjectionEvaluatorPtr projectionEvaluator_
This algorithm uses a discretization (a grid) to guide the exploration. The exploration is imposed on...
Definition KPIECE1.h:399
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 setGoalBias(double goalBias)
Definition KPIECE1.h:94
double selectBorderFraction_
The fraction of time to focus exploration on the border of the grid.
Definition KPIECE1.h:418
KPIECE1(const SpaceInformationPtr &si)
Constructor.
Definition KPIECE1.cpp:44
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definition KPIECE1.cpp:83
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
void freeMotion(Motion *motion)
Free the memory for a motion.
Definition KPIECE1.cpp:112
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
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 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 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:136
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:129
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
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
void freeCellData(CellData *cdata)
Free the memory for the data contained in a grid cell.
Definition KPIECE1.cpp:105
void freeGridMotions(Grid &grid)
Free the memory for the motions contained in a grid.
Definition KPIECE1.cpp:99
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...
Main namespace. Contains everything in this library.
Definition of a cell in this grid.
Definition Grid.h:59
A class to store the exit status of Planner::solve()
The data held by a cell in the grid of motions.
Definition KPIECE1.h:227
unsigned int selections
The number of times this cell has been selected for expansion.
Definition KPIECE1.h:242
std::vector< Motion * > motions
The set of motions contained in this grid cell.
Definition KPIECE1.h:233
double importance
The computed importance (based on other class members)
Definition KPIECE1.h:253
double coverage
A measure of coverage for this cell. For this implementation, this is the sum of motion durations.
Definition KPIECE1.h:238
double score
A heuristic score computed based on distance to goal (if available), successes and failures at expand...
Definition KPIECE1.h:247
unsigned int iteration
The iteration at which this cell was created.
Definition KPIECE1.h:250
Information about a known good sample (closer to the goal than others)
Definition KPIECE1.h:271
Motion * motion
The motion that is close to the goal.
Definition KPIECE1.h:281
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
Grid::Cell * cell
The cell of the motion that is close to the goal.
Definition KPIECE1.h:278
bool operator<(const CloseSample &other) const
Sort samples in accordance to their distance to the goal.
Definition KPIECE1.h:288
CloseSample(Grid::Cell *c, Motion *m, double d)
Constructor fully initializes the content of this structure.
Definition KPIECE1.h:273
Bounded set of good samples.
Definition KPIECE1.h:296
CloseSamples(unsigned int size)
Construct an object to maintain a set of at most size samples.
Definition KPIECE1.h:298
unsigned int maxSize
Maximum number of samples to maintain.
Definition KPIECE1.h:323
bool canSample() const
Return true if samples can be selected from this set.
Definition KPIECE1.h:317
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
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
std::set< CloseSample > samples
The maintained samples.
Definition KPIECE1.h:326
Representation of a motion for this algorithm.
Definition KPIECE1.h:201
base::State * state
The state contained by this motion.
Definition KPIECE1.h:213
Motion(const SpaceInformation *si)
Constructor that allocates memory for the state and the control.
Definition KPIECE1.h:205
Motion * parent
The parent motion in the exploration tree.
Definition KPIECE1.h:222
unsigned int steps
The number of steps the control is applied for.
Definition KPIECE1.h:219
Definintion of an operator passed to the Grid structure, to order cells by importance.
Definition KPIECE1.h:259
The data defining a tree of motions for this algorithm.
Definition KPIECE1.h:331
unsigned int iteration
The number of iterations performed on this tree.
Definition KPIECE1.h:343
unsigned int size
The total number of motions (there can be multiple per cell) in the grid.
Definition KPIECE1.h:340
Grid grid
A grid containing motions, imposed on a projection of the state space.
Definition KPIECE1.h:336