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