46 namespace ompl
81 ~KPIECE1() override;
85 void clear() override;
96 goalBias_ = goalBias;
102 return goalBias_;
113 selectBorderFraction_ = bp;
120 return selectBorderFraction_;
138 badScoreFactor_ = bad;
145 goodScoreFactor_ = good;
152 return goodScoreFactor_;
159 return badScoreFactor_;
166 nCloseSamples_ = nCloseSamples;
172 return nCloseSamples_;
179 projectionEvaluator_ = projectionEvaluator;
192 return projectionEvaluator_;
195 void setup() override;
202 Motion() = default;
210 ~Motion() = default;
228 CellData() = default;
230 ~CellData() = default;
319 return !samples.empty();
332 TreeData() = default;
356 void freeMemory();
362 void freeCellData(CellData *cdata);
365 void freeMotion(Motion *motion);
385 unsigned int findNextMotion(const std::vector<Grid::Coord> &coords, unsigned int index, unsigned int count);
CloseSample(Grid::Cell *c, Motion *m, double d)
Constructor fully initializes the content of this structure.
void setBadCellScoreFactor(double bad)
Set the factor that is to be applied to a cell's score when an expansion from that cell fails...
The distance to the goal. This value is increased over time, as the number of selections for this sam...
bool operator<(const CloseSample &other) const
Sort samples in accordance to their distance to the goal.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
void setGoodCellScoreFactor(double good)
Set the factor that is to be applied to a cell's score when an expansion from that cell succeedes...
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].
bool selectMotion(Motion *&smotion, Grid::Cell *&scell)
Select the top sample (closest to the goal) and update its position in the set subsequently (pretend ...
The fraction of time to focus exploration on the border of the grid.
double getBorderFraction() const
Get the fraction of time to focus exploration on boundary.
bool canSample() const
Return true if samples can be selected from this set.
void setProjectionEvaluator(const std::string &name)
Set the projection evaluator (select one from the ones registered with the state space).
double getBadCellScoreFactor() const
Get the factor that is multiplied to a cell's score if extending a motion from that cell failed...
The fraction of time the goal is picked as the state to expand towards (if such a state is available)...
void clear() override
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Definintion of an operator passed to the Grid structure, to order cells by importance.
A shared pointer wrapper for ompl::control::ControlSampler.
typename GridN< _T >::Cell Cell
Definition of a cell in this grid.
unsigned int iteration
The iteration at which this cell was created.
The computed importance (based on other class members)
A heuristic score computed based on distance to goal (if available), successes and failures at expand...
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
When extending a motion from a cell, the extension can fail. If it is, the score of the cell is multi...
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...
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...
unsigned int size
The total number of motions (there can be multiple per cell) in the grid.
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...
void setMaxCloseSamplesCount(unsigned int nCloseSamples)
When motions reach close to the goal, they are stored in a separate queue to allow biasing towards th...
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 ...
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...
void freeMotion(Motion *motion)
Free the memory for a motion.
Free all the memory allocated by this planner.
void getPlannerData(base::PlannerData &data) const override
Get information about the current run of the motion planner. Repeated calls to this function will upd...
void setBorderFraction(double bp)
Set the fraction of time for focusing on the border (between 0 and 1). This is the minimum fraction u...
This algorithm uses a discretization (a grid) to guide the exploration. The exploration is imposed on...
unsigned int getMaxCloseSamplesCount() const
Get the maximum number of samples to store in the queue of samples that are close to the goal...
Motion * motion
The motion that is close to the goal.
When extending a motion from a cell, the extension can be successful. If it is, the score of the cell...
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Representation of a motion for this algorithm.
Motion * parent
The parent motion in the exploration tree.
The data defining a tree of motions for this algorithm.
unsigned int maxSize
Maximum number of samples to maintain.
Information about a known good sample (closer to the goal than others)
A measure of coverage for this cell. For this implementation, this is the sum of motion durations...
void setup() override
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
std::vector< Motion * > motions
The set of motions contained in this grid cell.
A shared pointer wrapper for ompl::base::ProjectionEvaluator.
A class to store the exit status of Planner::solve()
void freeGridMotions(Grid &grid)
Free the memory for the motions contained in a grid.
KPIECE1(const SpaceInformationPtr &si)
A control sampler.
unsigned int selections
The number of times this cell has been selected for expansion.
Kinodynamic Planning by Interior-Exterior Cell Exploration.
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...
std::set< CloseSample > samples
The maintained samples.
double getGoodCellScoreFactor() const
Get the factor that is multiplied to a cell's score if extending a motion from that cell succeeded...
The data held by a cell in the grid of motions.
Motion(const SpaceInformation *si)
Constructor that allocates memory for the state and the control.
A grid containing motions, imposed on a projection of the state space.
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...
unsigned int iteration
The number of iterations performed on this tree.
Grid::Cell * cell
The cell of the motion that is close to the goal.
CloseSamples(unsigned int size)
Construct an object to maintain a set of at most size samples.
Control * control
The control contained by this motion.
const SpaceInformation * siC_
The base::SpaceInformation cast as control::SpaceInformation, for convenience.
The space information for which planning is done.
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
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.
base::State * state
The state contained by this motion.
void freeCellData(CellData *cdata)
Free the memory for the data contained in a grid cell.
unsigned int steps
The number of steps the control is applied for.
const base::ProjectionEvaluatorPtr & getProjectionEvaluator() const
Get the projection evaluator.