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/BKPIECE1.h"
00038 #include "ompl/base/GoalSampleableRegion.h"
00039 #include "ompl/tools/config/SelfConfig.h"
00040 #include <cassert>
00041
00042 ompl::geometric::BKPIECE1::BKPIECE1(const base::SpaceInformationPtr &si) : base::Planner(si, "BKPIECE1"),
00043 dStart_(boost::bind(&BKPIECE1::freeMotion, this, _1)),
00044 dGoal_(boost::bind(&BKPIECE1::freeMotion, this, _1))
00045 {
00046 specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION;
00047
00048 minValidPathFraction_ = 0.5;
00049 failedExpansionScoreFactor_ = 0.5;
00050 maxDistance_ = 0.0;
00051
00052 Planner::declareParam<double>("range", this, &BKPIECE1::setRange, &BKPIECE1::getRange);
00053 Planner::declareParam<double>("border_fraction", this, &BKPIECE1::setBorderFraction, &BKPIECE1::getBorderFraction);
00054 Planner::declareParam<double>("failed_expansion_score_factor", this, &BKPIECE1::setFailedExpansionCellScoreFactor, &BKPIECE1::getFailedExpansionCellScoreFactor);
00055 Planner::declareParam<double>("min_valid_path_fraction", this, &BKPIECE1::setMinValidPathFraction, &BKPIECE1::getMinValidPathFraction);
00056 }
00057
00058 ompl::geometric::BKPIECE1::~BKPIECE1(void)
00059 {
00060 }
00061
00062 void ompl::geometric::BKPIECE1::setup(void)
00063 {
00064 Planner::setup();
00065 tools::SelfConfig sc(si_, getName());
00066 sc.configureProjectionEvaluator(projectionEvaluator_);
00067 sc.configurePlannerRange(maxDistance_);
00068
00069 if (failedExpansionScoreFactor_ < std::numeric_limits<double>::epsilon() || failedExpansionScoreFactor_ > 1.0)
00070 throw Exception("Failed expansion cell score factor must be in the range (0,1]");
00071 if (minValidPathFraction_ < std::numeric_limits<double>::epsilon() || minValidPathFraction_ > 1.0)
00072 throw Exception("The minimum valid path fraction must be in the range (0,1]");
00073
00074 dStart_.setDimension(projectionEvaluator_->getDimension());
00075 dGoal_.setDimension(projectionEvaluator_->getDimension());
00076 }
00077
00078 bool ompl::geometric::BKPIECE1::solve(const base::PlannerTerminationCondition &ptc)
00079 {
00080 checkValidity();
00081 base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
00082
00083 if (!goal)
00084 {
00085 msg_.error("Unknown type of goal (or goal undefined)");
00086 return false;
00087 }
00088
00089 Discretization<Motion>::Coord xcoord;
00090
00091 while (const base::State *st = pis_.nextStart())
00092 {
00093 Motion* motion = new Motion(si_);
00094 si_->copyState(motion->state, st);
00095 motion->root = motion->state;
00096 projectionEvaluator_->computeCoordinates(motion->state, xcoord);
00097 dStart_.addMotion(motion, xcoord);
00098 }
00099
00100 if (dStart_.getMotionCount() == 0)
00101 {
00102 msg_.error("Motion planning start tree could not be initialized!");
00103 return false;
00104 }
00105
00106 if (!goal->couldSample())
00107 {
00108 msg_.error("Insufficient states in sampleable goal region");
00109 return false;
00110 }
00111
00112 if (!sampler_)
00113 sampler_ = si_->allocValidStateSampler();
00114
00115 msg_.inform("Starting with %d states", (int)(dStart_.getMotionCount() + dGoal_.getMotionCount()));
00116
00117 std::vector<Motion*> solution;
00118 base::State *xstate = si_->allocState();
00119 bool startTree = true;
00120 bool solved = false;
00121
00122 while (ptc() == false)
00123 {
00124 Discretization<Motion> &disc = startTree ? dStart_ : dGoal_;
00125 startTree = !startTree;
00126 Discretization<Motion> &otherDisc = startTree ? dStart_ : dGoal_;
00127 disc.countIteration();
00128
00129
00130 if (dGoal_.getMotionCount() == 0 || pis_.getSampledGoalsCount() < dGoal_.getMotionCount() / 2)
00131 {
00132 const base::State *st = dGoal_.getMotionCount() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal();
00133 if (st)
00134 {
00135 Motion* motion = new Motion(si_);
00136 si_->copyState(motion->state, st);
00137 motion->root = motion->state;
00138 projectionEvaluator_->computeCoordinates(motion->state, xcoord);
00139 dGoal_.addMotion(motion, xcoord);
00140 }
00141 if (dGoal_.getMotionCount() == 0)
00142 {
00143 msg_.error("Unable to sample any valid states for goal tree");
00144 break;
00145 }
00146 }
00147
00148 Discretization<Motion>::Cell *ecell = NULL;
00149 Motion *existing = NULL;
00150 disc.selectMotion(existing, ecell);
00151 assert(existing);
00152 if (sampler_->sampleNear(xstate, existing->state, maxDistance_))
00153 {
00154 std::pair<base::State*, double> fail(xstate, 0.0);
00155 bool keep = si_->checkMotion(existing->state, xstate, fail);
00156 if (!keep && fail.second > minValidPathFraction_)
00157 keep = true;
00158
00159 if (keep)
00160 {
00161
00162 Motion *motion = new Motion(si_);
00163 si_->copyState(motion->state, xstate);
00164 motion->root = existing->root;
00165 motion->parent = existing;
00166
00167 projectionEvaluator_->computeCoordinates(motion->state, xcoord);
00168 disc.addMotion(motion, xcoord);
00169
00170 Discretization<Motion>::Cell* cellC = otherDisc.getGrid().getCell(xcoord);
00171
00172 if (cellC && !cellC->data->motions.empty())
00173 {
00174 Motion* connectOther = cellC->data->motions[rng_.uniformInt(0, cellC->data->motions.size() - 1)];
00175
00176 if (goal->isStartGoalPairValid(startTree ? connectOther->root : motion->root, startTree ? motion->root : connectOther->root) &&
00177 si_->checkMotion(motion->state, connectOther->state))
00178 {
00179
00180
00181 std::vector<Motion*> mpath1;
00182 while (motion != NULL)
00183 {
00184 mpath1.push_back(motion);
00185 motion = motion->parent;
00186 }
00187
00188 std::vector<Motion*> mpath2;
00189 while (connectOther != NULL)
00190 {
00191 mpath2.push_back(connectOther);
00192 connectOther = connectOther->parent;
00193 }
00194
00195 if (startTree)
00196 mpath1.swap(mpath2);
00197
00198 PathGeometric *path = new PathGeometric(si_);
00199 path->getStates().reserve(mpath1.size() + mpath2.size());
00200 for (int i = mpath1.size() - 1 ; i >= 0 ; --i)
00201 path->append(mpath1[i]->state);
00202 for (unsigned int i = 0 ; i < mpath2.size() ; ++i)
00203 path->append(mpath2[i]->state);
00204
00205 goal->addSolutionPath(base::PathPtr(path), false, 0.0);
00206 solved = true;
00207 break;
00208 }
00209 }
00210 }
00211 else
00212 ecell->data->score *= failedExpansionScoreFactor_;
00213 }
00214 else
00215 ecell->data->score *= failedExpansionScoreFactor_;
00216 disc.updateCell(ecell);
00217 }
00218
00219 si_->freeState(xstate);
00220
00221 msg_.inform("Created %u (%u start + %u goal) states in %u cells (%u start (%u on boundary) + %u goal (%u on boundary))",
00222 dStart_.getMotionCount() + dGoal_.getMotionCount(), dStart_.getMotionCount(), dGoal_.getMotionCount(),
00223 dStart_.getCellCount() + dGoal_.getCellCount(), dStart_.getCellCount(), dStart_.getGrid().countExternal(),
00224 dGoal_.getCellCount(), dGoal_.getGrid().countExternal());
00225
00226 return solved;
00227 }
00228
00229 void ompl::geometric::BKPIECE1::freeMotion(Motion *motion)
00230 {
00231 if (motion->state)
00232 si_->freeState(motion->state);
00233 delete motion;
00234 }
00235
00236 void ompl::geometric::BKPIECE1::clear(void)
00237 {
00238 Planner::clear();
00239
00240 sampler_.reset();
00241 dStart_.clear();
00242 dGoal_.clear();
00243 }
00244
00245 void ompl::geometric::BKPIECE1::getPlannerData(base::PlannerData &data) const
00246 {
00247 Planner::getPlannerData(data);
00248 dStart_.getPlannerData(data, 1);
00249 dGoal_.getPlannerData(data, 2);
00250 }