37 #include "ompl/geometric/planners/kpiece/BKPIECE1.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/tools/config/SelfConfig.h"
43 dStart_(boost::bind(&
BKPIECE1::freeMotion, this, _1)),
44 dGoal_(boost::bind(&
BKPIECE1::freeMotion, this, _1))
59 ompl::geometric::BKPIECE1::~BKPIECE1(
void)
70 if (failedExpansionScoreFactor_ < std::numeric_limits<double>::epsilon() || failedExpansionScoreFactor_ > 1.0)
71 throw Exception(
"Failed expansion cell score factor must be in the range (0,1]");
72 if (minValidPathFraction_ < std::numeric_limits<double>::epsilon() || minValidPathFraction_ > 1.0)
73 throw Exception(
"The minimum valid path fraction must be in the range (0,1]");
75 dStart_.setDimension(projectionEvaluator_->getDimension());
76 dGoal_.setDimension(projectionEvaluator_->getDimension());
86 OMPL_ERROR(
"%s: Unknown type of goal", getName().c_str());
95 si_->copyState(motion->
state, st);
97 projectionEvaluator_->computeCoordinates(motion->
state, xcoord);
98 dStart_.addMotion(motion, xcoord);
101 if (dStart_.getMotionCount() == 0)
103 OMPL_ERROR(
"%s: Motion planning start tree could not be initialized!", getName().c_str());
109 OMPL_ERROR(
"%s: Insufficient states in sampleable goal region", getName().c_str());
114 sampler_ = si_->allocValidStateSampler();
116 OMPL_INFORM(
"%s: Starting with %d states", getName().c_str(), (
int)(dStart_.getMotionCount() + dGoal_.getMotionCount()));
118 std::vector<Motion*> solution;
120 bool startTree =
true;
126 startTree = !startTree;
128 disc.countIteration();
131 if (dGoal_.getMotionCount() == 0 || pis_.getSampledGoalsCount() < dGoal_.getMotionCount() / 2)
133 const base::State *st = dGoal_.getMotionCount() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
137 si_->copyState(motion->
state, st);
139 projectionEvaluator_->computeCoordinates(motion->
state, xcoord);
140 dGoal_.addMotion(motion, xcoord);
142 if (dGoal_.getMotionCount() == 0)
144 OMPL_ERROR(
"%s: Unable to sample any valid states for goal tree", getName().c_str());
153 if (sampler_->sampleNear(xstate, existing->
state, maxDistance_))
155 std::pair<base::State*, double> fail(xstate, 0.0);
156 bool keep = si_->checkMotion(existing->
state, xstate, fail);
157 if (!keep && fail.second > minValidPathFraction_)
164 si_->copyState(motion->
state, xstate);
166 motion->
parent = existing;
168 projectionEvaluator_->computeCoordinates(motion->
state, xcoord);
173 if (cellC && !cellC->
data->motions.empty())
175 Motion* connectOther = cellC->
data->motions[rng_.uniformInt(0, cellC->
data->motions.size() - 1)];
178 si_->checkMotion(motion->
state, connectOther->
state))
181 connectionPoint_ = std::make_pair(connectOther->
state, motion->
state);
183 connectionPoint_ = std::make_pair(motion->
state, connectOther->
state);
187 std::vector<Motion*> mpath1;
188 while (motion != NULL)
190 mpath1.push_back(motion);
194 std::vector<Motion*> mpath2;
195 while (connectOther != NULL)
197 mpath2.push_back(connectOther);
198 connectOther = connectOther->
parent;
205 path->
getStates().reserve(mpath1.size() + mpath2.size());
206 for (
int i = mpath1.size() - 1 ; i >= 0 ; --i)
207 path->
append(mpath1[i]->state);
208 for (
unsigned int i = 0 ; i < mpath2.size() ; ++i)
209 path->
append(mpath2[i]->state);
218 ecell->
data->score *= failedExpansionScoreFactor_;
221 ecell->
data->score *= failedExpansionScoreFactor_;
222 disc.updateCell(ecell);
225 si_->freeState(xstate);
227 OMPL_INFORM(
"%s: Created %u (%u start + %u goal) states in %u cells (%u start (%u on boundary) + %u goal (%u on boundary))",
229 dStart_.getMotionCount() + dGoal_.getMotionCount(), dStart_.getMotionCount(), dGoal_.getMotionCount(),
230 dStart_.getCellCount() + dGoal_.getCellCount(), dStart_.getCellCount(), dStart_.getGrid().countExternal(),
231 dGoal_.getCellCount(), dGoal_.getGrid().countExternal());
239 si_->freeState(motion->
state);
250 connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
255 Planner::getPlannerData(data);
256 dStart_.getPlannerData(data, 1,
true, NULL);
257 dGoal_.getPlannerData(data, 2,
false, NULL);
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::Coord Coord
The datatype for the maintained grid coordinates.
void setRange(double distance)
Set the range the planner is supposed to use.
Object containing planner generated vertex and edge data. It is assumed that all vertices are unique...
const base::State * root
The root state (start state) that leads to this motion.
The planner failed to find a solution.
Bi-directional KPIECE with one level of discretization.
GoalType recognizedGoal
The type of goal specification the planner can use.
void freeMotion(Motion *motion)
Free the memory for a motion.
virtual void setup(void)
Perform extra configuration steps, if needed. This call will also issue a call to ompl::base::SpaceIn...
std::pair< base::State *, base::State * > connectionPoint_
The pair of states in each tree connected during planning. Used for PlannerData computation.
Encapsulate a termination condition for a motion planner. Planners will call operator() to decide whe...
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...
_T data
The data we store in the cell.
std::vector< base::State * > & getStates(void)
Get the states that make up the path (as a reference, so it can be modified, hence the function is no...
double failedExpansionScoreFactor_
When extending a motion from a cell, the extension can fail. If it is, the score of the cell is multi...
void append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
double getRange(void) const
Get the range the planner is using.
virtual void clear(void)
Clear all internal datastructures. Planner settings are not affected. Subsequent calls to solve() wil...
Invalid start state or no start state specified.
Abstract definition of a goal region that can be sampled.
double maxDistance_
The maximum length of a motion to be added to a tree.
The goal is of a type that a planner does not recognize.
One-level discretization used for KPIECE.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
The planner found an exact solution.
unsigned int vertexIndex(const PlannerDataVertex &v) const
Return the index for the vertex associated with the given data. INVALID_INDEX is returned if this ver...
Representation of a motion for this algorithm.
unsigned int addMotion(Motion *motion, const Coord &coord, double dist=0.0)
Add a motion to the grid containing motions. As a hint, dist specifies the distance to the goal from ...
double getMinValidPathFraction(void) const
Get the value of the fraction set by setMinValidPathFraction()
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...
virtual bool isStartGoalPairValid(const State *, const State *) const
Since there can be multiple starting states (and multiple goal states) it is possible certain pairs a...
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.
virtual bool couldSample(void) const
Return true if samples could be generated by this sampler at some point in the future. By default this is equivalent to canSample(), but for GoalLazySamples, this call also reflects the fact that a sampling thread is active and although no samples are produced yet, some may become available at some point in the future.
Definition of an abstract state.
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].
base::State * state
The state contained by this motion.
PlannerSpecs specs_
The specifications of the planner (its capabilities)
Definition of a cell in this grid.
Motion * parent
The parent motion in the exploration tree.
The exception type for ompl.
double getFailedExpansionCellScoreFactor(void) const
Get the factor that is multiplied to a cell's score if extending a motion from that cell failed...
double getBorderFraction(void) const
Get the fraction of time to focus exploration on boundary.
void selectMotion(Motion *&smotion, Cell *&scell)
Select a motion and the cell it is part of from the grid of motions. This is where preference is give...
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.
BKPIECE1(const base::SpaceInformationPtr &si)
Constructor.
Definition of a geometric path.
void setBorderFraction(double bp)
Set the fraction of time for focusing on the border (between 0 and 1). This is the minimum fraction u...
A boost shared pointer wrapper for ompl::base::Path.
This bit is set if casting to sampleable goal regions (ompl::base::GoalSampleableRegion) is possible...
#define OMPL_INFORM(fmt,...)
Log a formatted information string.