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(
"There are no valid initial states!");
119 sampler_ = si_->allocStateSampler();
121 OMPL_INFORM(
"Starting with %u states", 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(
"Created %u states in %u cells (%u internal + %u external)", disc_.getMotionCount(), disc_.getCellCount(),
209 disc_.getGrid().countInternal(), disc_.getGrid().countExternal());
216 Planner::getPlannerData(data);
217 disc_.getPlannerData(data, 0,
true, lastGoalMotion_);