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