00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include "ompl/geometric/planners/kpiece/LBKPIECE1.h"
00038 #include "ompl/base/GoalSampleableRegion.h"
00039 #include "ompl/tools/config/SelfConfig.h"
00040 #include <cassert>
00041
00042 void ompl::geometric::LBKPIECE1::setup(void)
00043 {
00044 Planner::setup();
00045 SelfConfig sc(si_, getName());
00046 sc.configureProjectionEvaluator(projectionEvaluator_);
00047 sc.configurePlannerRange(maxDistance_);
00048
00049 if (minValidPathFraction_ < std::numeric_limits<double>::epsilon() || minValidPathFraction_ > 1.0)
00050 throw Exception("The minimum valid path fraction must be in the range (0,1]");
00051
00052 dStart_.setDimension(projectionEvaluator_->getDimension());
00053 dGoal_.setDimension(projectionEvaluator_->getDimension());
00054 }
00055
00056 bool ompl::geometric::LBKPIECE1::solve(const base::PlannerTerminationCondition &ptc)
00057 {
00058 checkValidity();
00059 base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
00060
00061 if (!goal)
00062 {
00063 msg_.error("Unknown type of goal (or goal undefined)");
00064 return false;
00065 }
00066
00067 Discretization<Motion>::Coord xcoord;
00068
00069 while (const base::State *st = pis_.nextStart())
00070 {
00071 Motion* motion = new Motion(si_);
00072 si_->copyState(motion->state, st);
00073 motion->root = st;
00074 motion->valid = true;
00075 projectionEvaluator_->computeCoordinates(motion->state, xcoord);
00076 dStart_.addMotion(motion, xcoord);
00077 }
00078
00079 if (dStart_.getMotionCount() == 0)
00080 {
00081 msg_.error("Motion planning start tree could not be initialized!");
00082 return false;
00083 }
00084
00085 if (!goal->canSample())
00086 {
00087 msg_.error("Insufficient states in sampleable goal region");
00088 return false;
00089 }
00090
00091 if (!sampler_)
00092 sampler_ = si_->allocStateSampler();
00093
00094 msg_.inform("Starting with %d states", (int)(dStart_.getMotionCount() + dGoal_.getMotionCount()));
00095
00096 base::State *xstate = si_->allocState();
00097 bool startTree = true;
00098
00099 while (ptc() == false)
00100 {
00101 Discretization<Motion> &disc = startTree ? dStart_ : dGoal_;
00102 startTree = !startTree;
00103 Discretization<Motion> &otherDisc = startTree ? dStart_ : dGoal_;
00104 disc.countIteration();
00105
00106
00107 if (dGoal_.getMotionCount() == 0 || pis_.getSampledGoalsCount() < dGoal_.getMotionCount() / 2)
00108 {
00109 const base::State *st = dGoal_.getMotionCount() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
00110 if (st)
00111 {
00112 Motion* motion = new Motion(si_);
00113 si_->copyState(motion->state, st);
00114 motion->root = motion->state;
00115 motion->valid = true;
00116 projectionEvaluator_->computeCoordinates(motion->state, xcoord);
00117 dGoal_.addMotion(motion, xcoord);
00118 }
00119 if (dGoal_.getMotionCount() == 0)
00120 {
00121 msg_.error("Unable to sample any valid states for goal tree");
00122 break;
00123 }
00124 }
00125
00126 Discretization<Motion>::Cell *ecell = NULL;
00127 Motion *existing = NULL;
00128 disc.selectMotion(existing, ecell);
00129 assert(existing);
00130 sampler_->sampleUniformNear(xstate, existing->state, maxDistance_);
00131
00132
00133 Motion* motion = new Motion(si_);
00134 si_->copyState(motion->state, xstate);
00135 motion->parent = existing;
00136 motion->root = existing->root;
00137 existing->children.push_back(motion);
00138 projectionEvaluator_->computeCoordinates(motion->state, xcoord);
00139 disc.addMotion(motion, xcoord);
00140
00141
00142 Discretization<Motion>::Cell *ocell = otherDisc.getGrid().getCell(xcoord);
00143 if (ocell && !ocell->data->motions.empty())
00144 {
00145 Motion* connectOther = ocell->data->motions[rng_.uniformInt(0, ocell->data->motions.size() - 1)];
00146
00147 if (goal->isStartGoalPairValid(startTree ? connectOther->root : motion->root, startTree ? motion->root : connectOther->root))
00148 {
00149 Motion* connect = new Motion(si_);
00150 si_->copyState(connect->state, connectOther->state);
00151 connect->parent = motion;
00152 connect->root = motion->root;
00153 motion->children.push_back(connect);
00154 projectionEvaluator_->computeCoordinates(connect->state, xcoord);
00155 disc.addMotion(connect, xcoord);
00156
00157 if (isPathValid(disc, connect, xstate) && isPathValid(otherDisc, connectOther, xstate))
00158 {
00159
00160
00161 std::vector<Motion*> mpath1;
00162 while (motion != NULL)
00163 {
00164 mpath1.push_back(motion);
00165 motion = motion->parent;
00166 }
00167
00168 std::vector<Motion*> mpath2;
00169 while (connectOther != NULL)
00170 {
00171 mpath2.push_back(connectOther);
00172 connectOther = connectOther->parent;
00173 }
00174
00175 if (startTree)
00176 mpath1.swap(mpath2);
00177
00178 PathGeometric *path = new PathGeometric(si_);
00179 path->states.reserve(mpath1.size() + mpath2.size());
00180 for (int i = mpath1.size() - 1 ; i >= 0 ; --i)
00181 path->states.push_back(si_->cloneState(mpath1[i]->state));
00182 for (unsigned int i = 0 ; i < mpath2.size() ; ++i)
00183 path->states.push_back(si_->cloneState(mpath2[i]->state));
00184
00185 goal->setDifference(0.0);
00186 goal->setSolutionPath(base::PathPtr(path));
00187 break;
00188 }
00189 }
00190 }
00191 }
00192
00193 si_->freeState(xstate);
00194
00195 msg_.inform("Created %u (%u start + %u goal) states in %u cells (%u start (%u on boundary) + %u goal (%u on boundary))",
00196 dStart_.getMotionCount() + dGoal_.getMotionCount(), dStart_.getMotionCount(), dGoal_.getMotionCount(),
00197 dStart_.getCellCount() + dGoal_.getCellCount(), dStart_.getCellCount(), dStart_.getGrid().countExternal(),
00198 dGoal_.getCellCount(), dGoal_.getGrid().countExternal());
00199
00200 return goal->isAchieved();
00201 }
00202
00203 bool ompl::geometric::LBKPIECE1::isPathValid(Discretization<Motion> &disc, Motion *motion, base::State *temp)
00204 {
00205 std::vector<Motion*> mpath;
00206
00207
00208 while (motion != NULL)
00209 {
00210 mpath.push_back(motion);
00211 motion = motion->parent;
00212 }
00213
00214 std::pair<base::State*, double> lastValid;
00215 lastValid.first = temp;
00216
00217
00218 for (int i = mpath.size() - 1 ; i >= 0 ; --i)
00219 if (!mpath[i]->valid)
00220 {
00221 if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state, lastValid))
00222 mpath[i]->valid = true;
00223 else
00224 {
00225 Motion *parent = mpath[i]->parent;
00226 removeMotion(disc, mpath[i]);
00227
00228
00229 if (lastValid.second > minValidPathFraction_)
00230 {
00231 Motion* reAdd = new Motion(si_);
00232 si_->copyState(reAdd->state, lastValid.first);
00233 reAdd->parent = parent;
00234 reAdd->root = parent->root;
00235 parent->children.push_back(reAdd);
00236 reAdd->valid = true;
00237 Discretization<Motion>::Coord coord;
00238 projectionEvaluator_->computeCoordinates(reAdd->state, coord);
00239 disc.addMotion(reAdd, coord);
00240 }
00241
00242 return false;
00243 }
00244 }
00245 return true;
00246 }
00247
00248 void ompl::geometric::LBKPIECE1::removeMotion(Discretization<Motion> &disc, Motion *motion)
00249 {
00250
00251
00252 Discretization<Motion>::Coord coord;
00253 projectionEvaluator_->computeCoordinates(motion->state, coord);
00254 disc.removeMotion(motion, coord);
00255
00256
00257
00258 if (motion->parent)
00259 {
00260 for (unsigned int i = 0 ; i < motion->parent->children.size() ; ++i)
00261 if (motion->parent->children[i] == motion)
00262 {
00263 motion->parent->children.erase(motion->parent->children.begin() + i);
00264 break;
00265 }
00266 }
00267
00268
00269 for (unsigned int i = 0 ; i < motion->children.size() ; ++i)
00270 {
00271 motion->children[i]->parent = NULL;
00272 removeMotion(disc, motion->children[i]);
00273 }
00274
00275 freeMotion(motion);
00276 }
00277
00278
00279 void ompl::geometric::LBKPIECE1::freeMotion(Motion *motion)
00280 {
00281 if (motion->state)
00282 si_->freeState(motion->state);
00283 delete motion;
00284 }
00285
00286 void ompl::geometric::LBKPIECE1::clear(void)
00287 {
00288 Planner::clear();
00289
00290 sampler_.reset();
00291 dStart_.clear();
00292 dGoal_.clear();
00293 }
00294
00295 void ompl::geometric::LBKPIECE1::getPlannerData(base::PlannerData &data) const
00296 {
00297 Planner::getPlannerData(data);
00298 dStart_.getPlannerData(data, 1);
00299 dGoal_.getPlannerData(data, 2);
00300 }