37 #include "ompl/control/planners/kpiece/KPIECE1.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/tools/config/SelfConfig.h"
40 #include "ompl/util/Exception.h"
64 ompl::control::KPIECE1::~KPIECE1(
void)
75 if (badScoreFactor_ < std::numeric_limits<double>::epsilon() || badScoreFactor_ > 1.0)
76 throw Exception(
"Bad cell score factor must be in the range (0,1]");
77 if (goodScoreFactor_ < std::numeric_limits<double>::epsilon() || goodScoreFactor_ > 1.0)
78 throw Exception(
"Good cell score factor must be in the range (0,1]");
79 if (selectBorderFraction_ < std::numeric_limits<double>::epsilon() || selectBorderFraction_ > 1.0)
80 throw Exception(
"The fraction of time spent selecting border cells must be in the range (0,1]");
82 tree_.grid.setDimension(projectionEvaluator_->getDimension());
88 controlSampler_.reset();
93 lastGoalMotion_ = NULL;
98 freeGridMotions(tree_.grid);
104 freeCellData(it->second->data);
109 for (
unsigned int i = 0 ; i < cdata->
motions.size() ; ++i)
117 si_->freeState(motion->
state);
119 siC_->freeControl(motion->
control);
133 if (samples.rbegin()->distance > distance)
137 if (samples.size() >= maxSize)
138 samples.erase(--samples.end());
150 static const double CLOSE_MOTION_DISTANCE_INFLATION_FACTOR = 1.1;
155 if (samples.size() > 0)
157 scell = samples.begin()->cell;
158 smotion = samples.begin()->motion;
161 double d = (samples.begin()->distance + samples.rbegin()->distance) * (CLOSE_MOTION_DISTANCE_INFLATION_FACTOR / 2.0);
162 samples.erase(samples.begin());
163 consider(scell, smotion, d);
171 for (
unsigned int i = index + 1 ; i < count ; ++i)
172 if (coords[i] != coords[index])
186 si_->copyState(motion->
state, st);
187 siC_->nullControl(motion->
control);
188 addMotion(motion, 1.0);
191 if (tree_.grid.size() == 0)
193 OMPL_ERROR(
"%s: There are no valid initial states!", getName().c_str());
197 if (!controlSampler_)
198 controlSampler_ = siC_->allocControlSampler();
200 OMPL_INFORM(
"%s: Starting with %u states", getName().c_str(), tree_.size);
204 double approxdif = std::numeric_limits<double>::infinity();
206 Control *rctrl = siC_->allocControl();
208 std::vector<base::State*> states(siC_->getMaxControlDuration() + 1);
209 std::vector<Grid::Coord> coords(states.size());
210 std::vector<Grid::Cell*> cells(coords.size());
212 for (
unsigned int i = 0 ; i < states.size() ; ++i)
213 states[i] = si_->allocState();
226 if (closeSamples.
canSample() && rng_.uniform01() < goalBias_)
229 selectMotion(existing, ecell);
232 selectMotion(existing, ecell);
236 controlSampler_->sampleNext(rctrl, existing->
control, existing->
state);
239 unsigned int cd = controlSampler_->sampleStepCount(siC_->getMinControlDuration(), siC_->getMaxControlDuration());
240 cd = siC_->propagateWhileValid(existing->
state, rctrl, cd, states,
false);
243 if (cd >= siC_->getMinControlDuration())
245 std::size_t avgCov_two_thirds = (2 * tree_.size) / (3 * tree_.grid.size());
246 bool interestingMotion =
false;
249 for (
unsigned int i = 0 ; i < cd ; ++i)
251 projectionEvaluator_->computeCoordinates(states[i], coords[i]);
252 cells[i] = tree_.grid.getCell(coords[i]);
254 interestingMotion =
true;
257 if (!interestingMotion && cells[i]->data->motions.size() <= avgCov_two_thirds)
258 interestingMotion =
true;
262 if (interestingMotion || rng_.uniform01() < 0.05)
264 unsigned int index = 0;
267 unsigned int nextIndex = findNextMotion(coords, index, cd);
269 si_->copyState(motion->
state, states[nextIndex]);
270 siC_->copyControl(motion->
control, rctrl);
271 motion->
steps = nextIndex - index + 1;
272 motion->
parent = existing;
284 if (dist < approxdif)
290 closeSamples.
consider(toCell, motion, dist);
294 index = nextIndex + 1;
302 ecell->data->score *= goodScoreFactor_;
305 ecell->data->score *= badScoreFactor_;
307 tree_.grid.update(ecell);
311 bool approximate =
false;
312 if (solution == NULL)
314 solution = approxsol;
318 if (solution != NULL)
320 lastGoalMotion_ = solution;
323 std::vector<Motion*> mpath;
324 while (solution != NULL)
326 mpath.push_back(solution);
327 solution = solution->
parent;
332 for (
int i = mpath.size() - 1 ; i >= 0 ; --i)
333 if (mpath[i]->parent)
334 path->
append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize());
336 path->
append(mpath[i]->state);
338 pdef_->addSolutionPath(
base::PathPtr(path), approximate, approxdif);
342 siC_->freeControl(rctrl);
343 for (
unsigned int i = 0 ; i < states.size() ; ++i)
344 si_->freeState(states[i]);
346 OMPL_INFORM(
"%s: Created %u states in %u cells (%u internal + %u external)",
347 getName().c_str(), tree_.size, tree_.grid.size(),
348 tree_.grid.countInternal(), tree_.grid.countExternal());
355 scell = rng_.uniform01() < std::max(selectBorderFraction_, tree_.grid.fracExternal()) ?
356 tree_.grid.topExternal() : tree_.grid.topInternal();
360 if (scell->data->score < std::numeric_limits<double>::epsilon())
362 OMPL_DEBUG(
"%s: Numerical precision limit reached. Resetting costs.", getName().c_str());
363 std::vector<CellData*> content;
364 content.reserve(tree_.grid.size());
365 tree_.grid.getContent(content);
366 for (std::vector<CellData*>::iterator it = content.begin() ; it != content.end() ; ++it)
367 (*it)->score += 1.0 +
log((
double)((*it)->iteration));
368 tree_.grid.updateAll();
371 if (scell && !scell->data->motions.empty())
373 scell->data->selections++;
374 smotion = scell->data->motions[rng_.halfNormalInt(0, scell->data->motions.size() - 1)];
383 static const double DISTANCE_TO_GOAL_OFFSET = 1e-3;
389 projectionEvaluator_->computeCoordinates(motion->
state, coord);
393 cell->data->motions.push_back(motion);
394 cell->data->coverage += motion->
steps;
395 tree_.grid.update(cell);
399 cell = tree_.grid.createCell(coord);
401 cell->data->motions.push_back(motion);
402 cell->data->coverage = motion->
steps;
403 cell->data->iteration = tree_.iteration;
404 cell->data->selections = 1;
405 cell->data->score = (1.0 +
log((
double)(tree_.iteration))) / (DISTANCE_TO_GOAL_OFFSET + dist);
406 tree_.grid.add(cell);
414 Planner::getPlannerData(data);
417 tree_.grid.getCells(cells);
419 double delta = siC_->getPropagationStepSize();
424 for (
unsigned int i = 0 ; i < cells.size() ; ++i)
426 for (
unsigned int j = 0 ; j < cells[i]->data->motions.size() ; ++j)
428 const Motion* m = cells[i]->data->motions[j];
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
void setBadCellScoreFactor(double bad)
Set the factor that is to be applied to a cell's score when an expansion from that cell fails...
double getGoalBias(void) const
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...
virtual base::PlannerStatus solve(const base::PlannerTerminationCondition &ptc)
Function that can solve the motion planning problem. This function can be called multiple times on th...
Bounded set of good samples.
bool selectMotion(Motion *&smotion, Grid::Cell *&scell)
Select the top sample (closest to the goal) and update its position in the set subsequently (pretend ...
void log(const char *file, int line, LogLevel level, const char *m,...)
Root level logging function. This should not be invoked directly, but rather used via a logging macro...
double selectBorderFraction_
The fraction of time to focus exploration on the border of the grid.
GridN< CellData * >::Coord Coord
Datatype for cell coordinates.
void setGoalBias(double goalBias)
void onCellUpdate(EventCellUpdate event, void *arg)
TreeData tree_
The tree datastructure.
void append(const base::State *state)
Append state to the end of the path; it is assumed state is the first state, so no control is applied...
Definition of an abstract control.
double goalBias_
The fraction of time the goal is picked as the state to expand towards (if such a state is available)...
unsigned int addGoalVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Abstract definition of goals.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
double getBorderFraction(void) const
Get the fraction of time to focus exploration on boundary.
double badScoreFactor_
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...
Representation of an edge in PlannerData for planning with controls. This structure encodes a specifi...
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...
GridN< CellData * >::Cell Cell
Definition of a cell in this 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...
std::vector< Motion * > motions
The set of motions contained in this grid cell.
Definition of a control path.
void setMaxCloseSamplesCount(unsigned int nCloseSamples)
When motions reach close to the goal, they are stored in a separate queue to allow biasing towards th...
void freeMemory(void)
Free all the memory allocated by this planner.
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
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 ...
void freeMotion(Motion *motion)
Free the memory for a motion.
void setBorderFraction(double bp)
Set the fraction of time for focusing on the border (between 0 and 1). This is the minimum fraction u...
Base class for a vertex in the PlannerData structure. All derived classes must implement the clone an...
Invalid start state or no start state specified.
double goodScoreFactor_
When extending a motion from a cell, the extension can be successful. If it is, the score of the cell...
iterator begin(void) const
Return the begin() iterator for the grid.
Representation of a motion for this algorithm.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
bool tagState(const State *st, int tag)
Set the integer tag associated with the given state. If the given state does not exist in a vertex...
Motion * parent
The parent motion in the exploration tree.
bool canSample(void) const
Return true if samples can be selected from this set.
Information about a known good sample (closer to the goal than others)
double getBadCellScoreFactor(void) const
Get the factor that is multiplied to a cell's score if extending a motion from that cell failed...
A class to store the exit status of Planner::solve()
virtual bool addEdge(unsigned int v1, unsigned int v2, const PlannerDataEdge &edge=PlannerDataEdge(), Cost weight=Cost(1.0))
Adds a directed edge between the given vertex indexes. An optional edge structure and weight can be s...
void freeGridMotions(Grid &grid)
Free the memory for the motions contained in a grid.
KPIECE1(const SpaceInformationPtr &si)
Constructor.
unsigned int addStartVertex(const PlannerDataVertex &v)
Adds the given vertex to the graph data, and marks it as a start vertex. The vertex index is returned...
Definition of an abstract state.
unsigned int getMaxCloseSamplesCount(void) const
Get the maximum number of samples to store in the queue of samples that are close to the goal...
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
PlannerSpecs specs_
The specifications of the planner (its capabilities)
The exception type for ompl.
The data held by a cell in the grid of motions.
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Grid grid
A grid containing motions, imposed on a projection of the state space.
GridN< CellData * >::CellArray CellArray
The datatype for arrays of cells.
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...
Control * control
The control contained by this motion.
virtual bool hasControls(void) const
Indicate whether any information about controls (ompl::control::Control) is stored in this instance...
const SpaceInformation * siC_
The base::SpaceInformation cast as control::SpaceInformation, for convenience.
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
virtual void clear(void)
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
iterator end(void) const
Return the end() iterator for the grid.
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.
unsigned int nCloseSamples_
When motions reach close to the goal, they are stored in a separate queue to allow biasing towards th...
A boost shared pointer wrapper for ompl::base::Path.
double getGoodCellScoreFactor(void) const
Get the factor that is multiplied to a cell's score if extending a motion from that cell succeeded...
CoordHash::const_iterator iterator
We only allow const iterators.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.