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/KPIECE1.h"
00038 #include "ompl/base/goals/GoalSampleableRegion.h"
00039 #include "ompl/tools/config/SelfConfig.h"
00040 #include <limits>
00041 #include <cassert>
00042
00043 ompl::geometric::KPIECE1::KPIECE1(const base::SpaceInformationPtr &si) : base::Planner(si, "KPIECE1"),
00044 disc_(boost::bind(&KPIECE1::freeMotion, this, _1))
00045 {
00046 specs_.approximateSolutions = true;
00047 specs_.directed = true;
00048
00049 goalBias_ = 0.05;
00050 failedExpansionScoreFactor_ = 0.5;
00051 minValidPathFraction_ = 0.2;
00052 maxDistance_ = 0.0;
00053 lastGoalMotion_ = NULL;
00054
00055 Planner::declareParam<double>("range", this, &KPIECE1::setRange, &KPIECE1::getRange);
00056 Planner::declareParam<double>("goal_bias", this, &KPIECE1::setGoalBias, &KPIECE1::getGoalBias);
00057 Planner::declareParam<double>("border_fraction", this, &KPIECE1::setBorderFraction, &KPIECE1::getBorderFraction);
00058 Planner::declareParam<double>("failed_expansion_score_factor", this, &KPIECE1::setFailedExpansionCellScoreFactor, &KPIECE1::getFailedExpansionCellScoreFactor);
00059 Planner::declareParam<double>("min_valid_path_fraction", this, &KPIECE1::setMinValidPathFraction, &KPIECE1::getMinValidPathFraction);
00060 }
00061
00062 ompl::geometric::KPIECE1::~KPIECE1(void)
00063 {
00064 }
00065
00066 void ompl::geometric::KPIECE1::setup(void)
00067 {
00068 Planner::setup();
00069 tools::SelfConfig sc(si_, getName());
00070 sc.configureProjectionEvaluator(projectionEvaluator_);
00071 sc.configurePlannerRange(maxDistance_);
00072
00073 if (failedExpansionScoreFactor_ < std::numeric_limits<double>::epsilon() || failedExpansionScoreFactor_ > 1.0)
00074 throw Exception("Failed expansion cell score factor must be in the range (0,1]");
00075 if (minValidPathFraction_ < std::numeric_limits<double>::epsilon() || minValidPathFraction_ > 1.0)
00076 throw Exception("The minimum valid path fraction must be in the range (0,1]");
00077
00078 disc_.setDimension(projectionEvaluator_->getDimension());
00079 }
00080
00081 void ompl::geometric::KPIECE1::clear(void)
00082 {
00083 Planner::clear();
00084 sampler_.reset();
00085 disc_.clear();
00086 lastGoalMotion_ = NULL;
00087 }
00088
00089 void ompl::geometric::KPIECE1::freeMotion(Motion *motion)
00090 {
00091 if (motion->state)
00092 si_->freeState(motion->state);
00093 delete motion;
00094 }
00095
00096 ompl::base::PlannerStatus ompl::geometric::KPIECE1::solve(const base::PlannerTerminationCondition &ptc)
00097 {
00098 checkValidity();
00099 base::Goal *goal = pdef_->getGoal().get();
00100 base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
00101
00102 Discretization<Motion>::Coord xcoord;
00103
00104 while (const base::State *st = pis_.nextStart())
00105 {
00106 Motion *motion = new Motion(si_);
00107 si_->copyState(motion->state, st);
00108 projectionEvaluator_->computeCoordinates(motion->state, xcoord);
00109 disc_.addMotion(motion, xcoord, 1.0);
00110 }
00111
00112 if (disc_.getMotionCount() == 0)
00113 {
00114 logError("There are no valid initial states!");
00115 return base::PlannerStatus::INVALID_START;
00116 }
00117
00118 if (!sampler_)
00119 sampler_ = si_->allocStateSampler();
00120
00121 logInform("Starting with %u states", disc_.getMotionCount());
00122
00123 Motion *solution = NULL;
00124 Motion *approxsol = NULL;
00125 double approxdif = std::numeric_limits<double>::infinity();
00126 base::State *xstate = si_->allocState();
00127
00128 while (ptc() == false)
00129 {
00130 disc_.countIteration();
00131
00132
00133 Motion *existing = NULL;
00134 Discretization<Motion>::Cell *ecell = NULL;
00135 disc_.selectMotion(existing, ecell);
00136 assert(existing);
00137
00138
00139 if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
00140 goal_s->sampleGoal(xstate);
00141 else
00142 sampler_->sampleUniformNear(xstate, existing->state, maxDistance_);
00143
00144 std::pair<base::State*, double> fail(xstate, 0.0);
00145 bool keep = si_->checkMotion(existing->state, xstate, fail);
00146 if (!keep && fail.second > minValidPathFraction_)
00147 keep = true;
00148
00149 if (keep)
00150 {
00151
00152 Motion *motion = new Motion(si_);
00153 si_->copyState(motion->state, xstate);
00154 motion->parent = existing;
00155
00156 double dist = 0.0;
00157 bool solv = goal->isSatisfied(motion->state, &dist);
00158 projectionEvaluator_->computeCoordinates(motion->state, xcoord);
00159 disc_.addMotion(motion, xcoord, dist);
00160
00161 if (solv)
00162 {
00163 approxdif = dist;
00164 solution = motion;
00165 break;
00166 }
00167 if (dist < approxdif)
00168 {
00169 approxdif = dist;
00170 approxsol = motion;
00171 }
00172 }
00173 else
00174 ecell->data->score *= failedExpansionScoreFactor_;
00175 disc_.updateCell(ecell);
00176 }
00177
00178 bool solved = false;
00179 bool approximate = false;
00180 if (solution == NULL)
00181 {
00182 solution = approxsol;
00183 approximate = true;
00184 }
00185
00186 if (solution != NULL)
00187 {
00188 lastGoalMotion_ = solution;
00189
00190
00191 std::vector<Motion*> mpath;
00192 while (solution != NULL)
00193 {
00194 mpath.push_back(solution);
00195 solution = solution->parent;
00196 }
00197
00198
00199 PathGeometric *path = new PathGeometric(si_);
00200 for (int i = mpath.size() - 1 ; i >= 0 ; --i)
00201 path->append(mpath[i]->state);
00202 pdef_->addSolutionPath(base::PathPtr(path), approximate, approxdif);
00203 solved = true;
00204 }
00205
00206 si_->freeState(xstate);
00207
00208 logInform("Created %u states in %u cells (%u internal + %u external)", disc_.getMotionCount(), disc_.getCellCount(),
00209 disc_.getGrid().countInternal(), disc_.getGrid().countExternal());
00210
00211 return base::PlannerStatus(solved, approximate);
00212 }
00213
00214 void ompl::geometric::KPIECE1::getPlannerData(base::PlannerData &data) const
00215 {
00216 Planner::getPlannerData(data);
00217 disc_.getPlannerData(data, 0, true, lastGoalMotion_);
00218 }