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/est/EST.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::EST::setup(void)
00044 {
00045 Planner::setup();
00046 SelfConfig sc(si_, getName());
00047 sc.configureProjectionEvaluator(projectionEvaluator_);
00048 sc.configurePlannerRange(maxDistance_);
00049
00050 tree_.grid.setDimension(projectionEvaluator_->getDimension());
00051 }
00052
00053 void ompl::geometric::EST::clear(void)
00054 {
00055 Planner::clear();
00056 sampler_.reset();
00057 freeMemory();
00058 tree_.grid.clear();
00059 tree_.size = 0;
00060 }
00061
00062 void ompl::geometric::EST::freeMemory(void)
00063 {
00064 for (Grid<MotionSet>::iterator it = tree_.grid.begin(); it != tree_.grid.end() ; ++it)
00065 {
00066 for (unsigned int i = 0 ; i < it->second->data.size() ; ++i)
00067 {
00068 if (it->second->data[i]->state)
00069 si_->freeState(it->second->data[i]->state);
00070 delete it->second->data[i];
00071 }
00072 }
00073 }
00074
00075 bool ompl::geometric::EST::solve(const base::PlannerTerminationCondition &ptc)
00076 {
00077 checkValidity();
00078 base::Goal *goal = pdef_->getGoal().get();
00079 base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
00080
00081 while (const base::State *st = pis_.nextStart())
00082 {
00083 Motion *motion = new Motion(si_);
00084 si_->copyState(motion->state, st);
00085 addMotion(motion);
00086 }
00087
00088 if (tree_.grid.size() == 0)
00089 {
00090 msg_.error("There are no valid initial states!");
00091 return false;
00092 }
00093
00094 if (!sampler_)
00095 sampler_ = si_->allocValidStateSampler();
00096
00097 msg_.inform("Starting with %u states", tree_.size);
00098
00099 Motion *solution = NULL;
00100 Motion *approxsol = NULL;
00101 double approxdif = std::numeric_limits<double>::infinity();
00102 base::State *xstate = si_->allocState();
00103
00104 while (ptc() == false)
00105 {
00106
00107 Motion *existing = selectMotion();
00108 assert(existing);
00109
00110
00111 if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
00112 goal_s->sampleGoal(xstate);
00113 else
00114 if (!sampler_->sampleNear(xstate, existing->state, maxDistance_))
00115 continue;
00116
00117 if (si_->checkMotion(existing->state, xstate))
00118 {
00119
00120 Motion *motion = new Motion(si_);
00121 si_->copyState(motion->state, xstate);
00122 motion->parent = existing;
00123
00124 addMotion(motion);
00125 double dist = 0.0;
00126 bool solved = goal->isSatisfied(motion->state, &dist);
00127 if (solved)
00128 {
00129 approxdif = dist;
00130 solution = motion;
00131 break;
00132 }
00133 if (dist < approxdif)
00134 {
00135 approxdif = dist;
00136 approxsol = motion;
00137 }
00138 }
00139 }
00140
00141 bool approximate = false;
00142 if (solution == NULL)
00143 {
00144 solution = approxsol;
00145 approximate = true;
00146 }
00147
00148 if (solution != NULL)
00149 {
00150
00151 std::vector<Motion*> mpath;
00152 while (solution != NULL)
00153 {
00154 mpath.push_back(solution);
00155 solution = solution->parent;
00156 }
00157
00158
00159 PathGeometric *path = new PathGeometric(si_);
00160 for (int i = mpath.size() - 1 ; i >= 0 ; --i)
00161 path->states.push_back(si_->cloneState(mpath[i]->state));
00162 goal->setDifference(approxdif);
00163 goal->setSolutionPath(base::PathPtr(path), approximate);
00164
00165 if (approximate)
00166 msg_.warn("Found approximate solution");
00167 }
00168
00169 si_->freeState(xstate);
00170
00171 msg_.inform("Created %u states in %u cells", tree_.size, tree_.grid.size());
00172
00173 return goal->isAchieved();
00174 }
00175
00176 ompl::geometric::EST::Motion* ompl::geometric::EST::selectMotion(void)
00177 {
00178 double sum = 0.0;
00179 Grid<MotionSet>::Cell* cell = NULL;
00180 double prob = rng_.uniform01() * (tree_.grid.size() - 1);
00181 for (Grid<MotionSet>::iterator it = tree_.grid.begin(); it != tree_.grid.end() ; ++it)
00182 {
00183 sum += (double)(tree_.size - it->second->data.size()) / (double)tree_.size;
00184 if (prob < sum)
00185 {
00186 cell = it->second;
00187 break;
00188 }
00189 }
00190 if (!cell && tree_.grid.size() > 0)
00191 cell = tree_.grid.begin()->second;
00192 return cell && !cell->data.empty() ? cell->data[rng_.uniformInt(0, cell->data.size() - 1)] : NULL;
00193 }
00194
00195 void ompl::geometric::EST::addMotion(Motion *motion)
00196 {
00197 Grid<MotionSet>::Coord coord;
00198 projectionEvaluator_->computeCoordinates(motion->state, coord);
00199 Grid<MotionSet>::Cell* cell = tree_.grid.getCell(coord);
00200 if (cell)
00201 cell->data.push_back(motion);
00202 else
00203 {
00204 cell = tree_.grid.createCell(coord);
00205 cell->data.push_back(motion);
00206 tree_.grid.add(cell);
00207 }
00208 tree_.size++;
00209 }
00210
00211 void ompl::geometric::EST::getPlannerData(base::PlannerData &data) const
00212 {
00213 Planner::getPlannerData(data);
00214
00215 std::vector<MotionSet> motions;
00216 tree_.grid.getContent(motions);
00217
00218 for (unsigned int i = 0 ; i < motions.size() ; ++i)
00219 for (unsigned int j = 0 ; j < motions[i].size() ; ++j)
00220 data.recordEdge(motions[i][j]->parent ? motions[i][j]->parent->state : NULL, motions[i][j]->state);
00221 }