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 logError(
"Unknown type of goal (or goal undefined)");
95 si_->copyState(motion->
state, st);
97 projectionEvaluator_->computeCoordinates(motion->
state, xcoord);
98 dStart_.addMotion(motion, xcoord);
101 if (dStart_.getMotionCount() == 0)
103 logError(
"Motion planning start tree could not be initialized!");
109 logError(
"Insufficient states in sampleable goal region");
114 sampler_ = si_->allocValidStateSampler();
116 logInform(
"Starting with %d states", (
int)(dStart_.getMotionCount() + dGoal_.getMotionCount()));
118 std::vector<Motion*> solution;
120 bool startTree =
true;
123 while (ptc() ==
false)
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 logError(
"Unable to sample any valid states for goal tree");
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<base::State*, base::State*>(connectOther->
state, motion->
state);
183 connectionPoint_ = std::make_pair<base::State*, base::State*>(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 logInform(
"Created %u (%u start + %u goal) states in %u cells (%u start (%u on boundary) + %u goal (%u on boundary))",
228 dStart_.getMotionCount() + dGoal_.getMotionCount(), dStart_.getMotionCount(), dGoal_.getMotionCount(),
229 dStart_.getCellCount() + dGoal_.getCellCount(), dStart_.getCellCount(), dStart_.getGrid().countExternal(),
230 dGoal_.getCellCount(), dGoal_.getGrid().countExternal());
238 si_->freeState(motion->
state);
249 connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
254 Planner::getPlannerData(data);
255 dStart_.getPlannerData(data, 1,
true, NULL);
256 dGoal_.getPlannerData(data, 2,
false, NULL);