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