37 #include "ompl/geometric/planners/kpiece/KPIECE1.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/tools/config/SelfConfig.h"
44 disc_(boost::bind(&
KPIECE1::freeMotion, this, _1))
62 ompl::geometric::KPIECE1::~KPIECE1(
void)
73 if (failedExpansionScoreFactor_ < std::numeric_limits<double>::epsilon() || failedExpansionScoreFactor_ > 1.0)
74 throw Exception(
"Failed expansion cell score factor must be in the range (0,1]");
75 if (minValidPathFraction_ < std::numeric_limits<double>::epsilon() || minValidPathFraction_ > 1.0)
76 throw Exception(
"The minimum valid path fraction must be in the range (0,1]");
78 disc_.setDimension(projectionEvaluator_->getDimension());
86 lastGoalMotion_ = NULL;
92 si_->freeState(motion->
state);
107 si_->copyState(motion->
state, st);
108 projectionEvaluator_->computeCoordinates(motion->
state, xcoord);
109 disc_.addMotion(motion, xcoord, 1.0);
112 if (disc_.getMotionCount() == 0)
114 OMPL_ERROR(
"%s: There are no valid initial states!", getName().c_str());
119 sampler_ = si_->allocStateSampler();
121 OMPL_INFORM(
"%s: Starting with %u states", getName().c_str(), disc_.getMotionCount());
125 double approxdif = std::numeric_limits<double>::infinity();
130 disc_.countIteration();
135 disc_.selectMotion(existing, ecell);
139 if (goal_s && rng_.uniform01() < goalBias_ && goal_s->
canSample())
142 sampler_->sampleUniformNear(xstate, existing->
state, maxDistance_);
144 std::pair<base::State*, double> fail(xstate, 0.0);
145 bool keep = si_->checkMotion(existing->
state, xstate, fail);
146 if (!keep && fail.second > minValidPathFraction_)
153 si_->copyState(motion->
state, xstate);
154 motion->
parent = existing;
158 projectionEvaluator_->computeCoordinates(motion->
state, xcoord);
159 disc_.addMotion(motion, xcoord, dist);
167 if (dist < approxdif)
174 ecell->
data->score *= failedExpansionScoreFactor_;
175 disc_.updateCell(ecell);
179 bool approximate =
false;
180 if (solution == NULL)
182 solution = approxsol;
186 if (solution != NULL)
188 lastGoalMotion_ = solution;
191 std::vector<Motion*> mpath;
192 while (solution != NULL)
194 mpath.push_back(solution);
195 solution = solution->parent;
200 for (
int i = mpath.size() - 1 ; i >= 0 ; --i)
201 path->
append(mpath[i]->state);
202 pdef_->addSolutionPath(
base::PathPtr(path), approximate, approxdif);
206 si_->freeState(xstate);
208 OMPL_INFORM(
"%s: Created %u states in %u cells (%u internal + %u external)",
210 disc_.getMotionCount(), disc_.getCellCount(),
211 disc_.getGrid().countInternal(), disc_.getGrid().countExternal());
218 Planner::getPlannerData(data);
219 disc_.getPlannerData(data, 0,
true, lastGoalMotion_);
bool approximateSolutions
Flag indicating whether the planner is able to compute approximate solutions.
double failedExpansionScoreFactor_
When extending a motion from a cell, the extension can fail. If it is, the score of the cell is multi...
double getBorderFraction(void) const
Get the fraction of time to focus exploration on boundary.
bool canSample(void) const
Return true if maxSampleCount() > 0, since in this case samples can certainly be produced.
Grid::Coord Coord
The datatype for the maintained grid coordinates.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
double getFailedExpansionCellScoreFactor(void) const
Get the factor that is multiplied to a cell's score if extending a motion from that cell failed...
Motion * lastGoalMotion_
The most recent goal motion. Used for PlannerData computation.
base::State * state
The state contained by this motion.
Representation of a motion for this algorithm.
void freeMotion(Motion *motion)
Free the memory for a motion.
double minValidPathFraction_
When extending a motion, the planner can decide to keep the first valid part of it, even if invalid states are found, as long as the valid part represents a sufficiently large fraction from the original motion.
Abstract definition of goals.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
_T data
The data we store in the cell.
void append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
bool directed
Flag indicating whether the planner is able to account for the fact that the validity of a motion fro...
double goalBias_
The fraction of time the goal is picked as the state to expand towards (if such a state is available)...
virtual void sampleGoal(State *st) const =0
Sample a state in the goal region.
void setGoalBias(double goalBias)
Set the goal bias.
double getGoalBias(void) const
Get the goal bias the planner is using.
Invalid start state or no start state specified.
double maxDistance_
The maximum length of a motion to be added to a tree.
Abstract definition of a goal region that can be sampled.
virtual void getPlannerData(base::PlannerData &data) const
Get information about the current run of the motion planner. Repeated calls to this function will upd...
double getRange(void) const
Get the range the planner is using.
Kinematic Planning by Interior-Exterior Cell Exploration.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
void setRange(double distance)
Set the range the planner is supposed to use.
A class to store the exit status of Planner::solve()
void setBorderFraction(double bp)
Set the fraction of time for focusing on the border (between 0 and 1). This is the minimum fraction u...
KPIECE1(const base::SpaceInformationPtr &si)
Constructor.
Definition of an abstract state.
virtual bool isSatisfied(const State *st) const =0
Return true if the state satisfies the goal constraints.
virtual void clear(void)
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
PlannerSpecs specs_
The specifications of the planner (its capabilities)
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
Motion * parent
The parent motion in the exploration tree.
Definition of a cell in this grid.
void setMinValidPathFraction(double fraction)
When extending a motion, the planner can decide to keep the first valid part of it, even if invalid states are found, as long as the valid part represents a sufficiently large fraction from the original motion. This function sets the minimum acceptable fraction (between 0 and 1).
The exception type for ompl.
Definition of a geometric path.
double getMinValidPathFraction(void) const
Get the value of the fraction set by setMinValidPathFraction()
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...
void setFailedExpansionCellScoreFactor(double factor)
When extending a motion from a cell, the extension can be successful or it can fail. If the extension fails, the score of the cell is multiplied by factor. These number should be in the range (0, 1].
A boost shared pointer wrapper for ompl::base::Path.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.