37 #include "ompl/geometric/planners/kpiece/LBKPIECE1.h"
38 #include "ompl/base/goals/GoalSampleableRegion.h"
39 #include "ompl/tools/config/SelfConfig.h"
43 dStart_(boost::bind(&
LBKPIECE1::freeMotion, this, _1)),
44 dGoal_(boost::bind(&
LBKPIECE1::freeMotion, this, _1))
57 ompl::geometric::LBKPIECE1::~LBKPIECE1(
void)
68 if (minValidPathFraction_ < std::numeric_limits<double>::epsilon() || minValidPathFraction_ > 1.0)
69 throw Exception(
"The minimum valid path fraction must be in the range (0,1]");
71 dStart_.setDimension(projectionEvaluator_->getDimension());
72 dGoal_.setDimension(projectionEvaluator_->getDimension());
82 logError(
"Unknown type of goal (or goal undefined)");
91 si_->copyState(motion->
state, st);
94 projectionEvaluator_->computeCoordinates(motion->
state, xcoord);
95 dStart_.addMotion(motion, xcoord);
98 if (dStart_.getMotionCount() == 0)
100 logError(
"Motion planning start tree could not be initialized!");
106 logError(
"Insufficient states in sampleable goal region");
111 sampler_ = si_->allocStateSampler();
113 logInform(
"Starting with %d states", (
int)(dStart_.getMotionCount() + dGoal_.getMotionCount()));
116 bool startTree =
true;
119 while (ptc() ==
false)
122 startTree = !startTree;
124 disc.countIteration();
127 if (dGoal_.getMotionCount() == 0 || pis_.getSampledGoalsCount() < dGoal_.getMotionCount() / 2)
129 const base::State *st = dGoal_.getMotionCount() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
133 si_->copyState(motion->
state, st);
135 motion->
valid =
true;
136 projectionEvaluator_->computeCoordinates(motion->
state, xcoord);
137 dGoal_.addMotion(motion, xcoord);
139 if (dGoal_.getMotionCount() == 0)
141 logError(
"Unable to sample any valid states for goal tree");
150 sampler_->sampleUniformNear(xstate, existing->
state, maxDistance_);
154 si_->copyState(motion->
state, xstate);
155 motion->
parent = existing;
157 existing->
children.push_back(motion);
158 projectionEvaluator_->computeCoordinates(motion->
state, xcoord);
163 if (ocell && !ocell->
data->motions.empty())
165 Motion* connectOther = ocell->
data->motions[rng_.uniformInt(0, ocell->
data->motions.size() - 1)];
170 si_->copyState(connect->
state, connectOther->
state);
173 motion->
children.push_back(connect);
174 projectionEvaluator_->computeCoordinates(connect->
state, xcoord);
177 if (isPathValid(disc, connect, xstate) && isPathValid(otherDisc, connectOther, xstate))
180 connectionPoint_ = std::make_pair<base::State*, base::State*>(connectOther->
state, motion->
state);
182 connectionPoint_ = std::make_pair<base::State*, base::State*>(motion->
state, connectOther->
state);
186 std::vector<Motion*> mpath1;
187 while (motion != NULL)
189 mpath1.push_back(motion);
193 std::vector<Motion*> mpath2;
194 while (connectOther != NULL)
196 mpath2.push_back(connectOther);
197 connectOther = connectOther->
parent;
204 path->
getStates().reserve(mpath1.size() + mpath2.size());
205 for (
int i = mpath1.size() - 1 ; i >= 0 ; --i)
206 path->
append(mpath1[i]->state);
207 for (
unsigned int i = 0 ; i < mpath2.size() ; ++i)
208 path->
append(mpath2[i]->state);
218 si_->freeState(xstate);
220 logInform(
"Created %u (%u start + %u goal) states in %u cells (%u start (%u on boundary) + %u goal (%u on boundary))",
221 dStart_.getMotionCount() + dGoal_.getMotionCount(), dStart_.getMotionCount(), dGoal_.getMotionCount(),
222 dStart_.getCellCount() + dGoal_.getCellCount(), dStart_.getCellCount(), dStart_.getGrid().countExternal(),
223 dGoal_.getCellCount(), dGoal_.getGrid().countExternal());
230 std::vector<Motion*> mpath;
233 while (motion != NULL)
235 mpath.push_back(motion);
239 std::pair<base::State*, double> lastValid;
240 lastValid.first = temp;
243 for (
int i = mpath.size() - 1 ; i >= 0 ; --i)
244 if (!mpath[i]->valid)
246 if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state, lastValid))
247 mpath[i]->valid =
true;
250 Motion *parent = mpath[i]->parent;
251 removeMotion(disc, mpath[i]);
254 if (lastValid.second > minValidPathFraction_)
257 si_->copyState(reAdd->
state, lastValid.first);
263 projectionEvaluator_->computeCoordinates(reAdd->
state, coord);
278 projectionEvaluator_->computeCoordinates(motion->
state, coord);
279 disc.removeMotion(motion, coord);
285 for (
unsigned int i = 0 ; i < motion->
parent->
children.size() ; ++i)
294 for (
unsigned int i = 0 ; i < motion->
children.size() ; ++i)
297 removeMotion(disc, motion->
children[i]);
307 si_->freeState(motion->
state);
318 connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL);
323 Planner::getPlannerData(data);
324 dStart_.getPlannerData(data, 1,
true, NULL);
325 dGoal_.getPlannerData(data, 2,
false, NULL);