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/control/planners/est/EST.h"
00038 #include "ompl/base/goals/GoalSampleableRegion.h"
00039 #include "ompl/tools/config/SelfConfig.h"
00040 #include <limits>
00041 #include <cassert>
00042
00043 ompl::control::EST::EST(const SpaceInformationPtr &si) : base::Planner(si, "EST")
00044 {
00045 specs_.approximateSolutions = true;
00046 goalBias_ = 0.05;
00047 maxDistance_ = 0.0;
00048 siC_ = si.get();
00049 lastGoalMotion_ = NULL;
00050
00051 Planner::declareParam<double>("range", this, &EST::setRange, &EST::getRange);
00052 Planner::declareParam<double>("goal_bias", this, &EST::setGoalBias, &EST::getGoalBias);
00053 }
00054
00055 ompl::control::EST::~EST(void)
00056 {
00057 freeMemory();
00058 }
00059
00060 void ompl::control::EST::setup(void)
00061 {
00062 Planner::setup();
00063 tools::SelfConfig sc(si_, getName());
00064 sc.configureProjectionEvaluator(projectionEvaluator_);
00065 sc.configurePlannerRange(maxDistance_);
00066
00067 tree_.grid.setDimension(projectionEvaluator_->getDimension());
00068 }
00069
00070 void ompl::control::EST::clear(void)
00071 {
00072 Planner::clear();
00073 sampler_.reset();
00074 controlSampler_.reset();
00075 freeMemory();
00076 tree_.grid.clear();
00077 tree_.size = 0;
00078 pdf_.clear ();
00079 lastGoalMotion_ = NULL;
00080 }
00081
00082 void ompl::control::EST::freeMemory(void)
00083 {
00084 for (Grid<MotionInfo>::iterator it = tree_.grid.begin(); it != tree_.grid.end() ; ++it)
00085 {
00086 for (unsigned int i = 0 ; i < it->second->data.size() ; ++i)
00087 {
00088 if (it->second->data[i]->state)
00089 si_->freeState(it->second->data[i]->state);
00090 if (it->second->data[i]->control)
00091 siC_->freeControl(it->second->data[i]->control);
00092 delete it->second->data[i];
00093 }
00094 }
00095 }
00096
00097 ompl::base::PlannerStatus ompl::control::EST::solve(const base::PlannerTerminationCondition &ptc)
00098 {
00099 checkValidity();
00100 base::Goal *goal = pdef_->getGoal().get();
00101 base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
00102
00103
00104 while (const base::State *st = pis_.nextStart())
00105 {
00106 Motion *motion = new Motion(siC_);
00107 si_->copyState(motion->state, st);
00108 siC_->nullControl(motion->control);
00109 addMotion(motion);
00110 }
00111
00112 if (tree_.grid.size() == 0)
00113 {
00114 logError("There are no valid initial states!");
00115 return base::PlannerStatus::INVALID_START;
00116 }
00117
00118
00119 if (!sampler_)
00120 sampler_ = si_->allocValidStateSampler();
00121 if (!controlSampler_)
00122 controlSampler_ = siC_->allocDirectedControlSampler();
00123
00124 logInform("Starting with %u states", tree_.size);
00125
00126 Motion *solution = NULL;
00127 double slndist = std::numeric_limits<double>::infinity();
00128 Motion *rmotion = new Motion(siC_);
00129 bool solved = false;
00130
00131 while (!ptc())
00132 {
00133
00134 Motion *existing = selectMotion();
00135 assert (existing);
00136
00137
00138 if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
00139 goal_s->sampleGoal(rmotion->state);
00140 else
00141 {
00142 if (!sampler_->sampleNear(rmotion->state, existing->state, maxDistance_))
00143 continue;
00144 }
00145
00146
00147 unsigned int duration = controlSampler_->sampleTo(rmotion->control, existing->control,
00148 existing->state, rmotion->state);
00149
00150
00151
00152 duration = siC_->propagateWhileValid(existing->state, rmotion->control, duration, rmotion->state);
00153
00154
00155 if (duration >= siC_->getMinControlDuration())
00156 {
00157
00158 Motion *motion = new Motion(siC_);
00159 si_->copyState(motion->state, rmotion->state);
00160 siC_->copyControl(motion->control, rmotion->control);
00161 motion->steps = duration;
00162 motion->parent = existing;
00163
00164
00165 addMotion(motion);
00166
00167
00168 double dist = 0.0;
00169 solved = goal->isSatisfied(motion->state, &dist);
00170 if (solved || dist < slndist)
00171 {
00172 slndist = dist;
00173 solution = motion;
00174
00175 if (solved)
00176 break;
00177 }
00178 }
00179 }
00180
00181 bool addedSolution = false;
00182
00183
00184 if (solution != NULL)
00185 {
00186 lastGoalMotion_ = solution;
00187
00188 std::vector<Motion*> mpath;
00189 while (solution != NULL)
00190 {
00191 mpath.push_back(solution);
00192 solution = solution->parent;
00193 }
00194
00195 PathControl *path = new PathControl(si_);
00196 for (int i = mpath.size() - 1 ; i >= 0 ; --i)
00197 if (mpath[i]->parent)
00198 path->append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize());
00199 else
00200 path->append(mpath[i]->state);
00201 addedSolution = true;
00202 pdef_->addSolutionPath(base::PathPtr(path), !solved, slndist);
00203 }
00204
00205
00206 if (rmotion->state)
00207 si_->freeState(rmotion->state);
00208 if (rmotion->control)
00209 siC_->freeControl(rmotion->control);
00210 delete rmotion;
00211
00212 logInform("Created %u states in %u cells", tree_.size, tree_.grid.size());
00213
00214 return addedSolution ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT;
00215 }
00216
00217 ompl::control::EST::Motion* ompl::control::EST::selectMotion(void)
00218 {
00219 GridCell* cell = pdf_.sample(rng_.uniform01());
00220 return cell && !cell->data.empty() ? cell->data[rng_.uniformInt(0, cell->data.size() - 1)] : NULL;
00221 }
00222
00223 void ompl::control::EST::addMotion(Motion *motion)
00224 {
00225 Grid<MotionInfo>::Coord coord;
00226 projectionEvaluator_->computeCoordinates(motion->state, coord);
00227 GridCell* cell = tree_.grid.getCell(coord);
00228 if (cell)
00229 {
00230 cell->data.push_back(motion);
00231 pdf_.update(cell->data.elem_, 1.0/cell->data.size());
00232 }
00233 else
00234 {
00235 cell = tree_.grid.createCell(coord);
00236 cell->data.push_back(motion);
00237 tree_.grid.add(cell);
00238 cell->data.elem_ = pdf_.add(cell, 1.0);
00239 }
00240 tree_.size++;
00241 }
00242
00243 void ompl::control::EST::getPlannerData(base::PlannerData &data) const
00244 {
00245 Planner::getPlannerData(data);
00246
00247 std::vector<MotionInfo> motions;
00248 tree_.grid.getContent(motions);
00249
00250 double stepSize = siC_->getPropagationStepSize();
00251
00252 if (lastGoalMotion_)
00253 data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state));
00254
00255 for (unsigned int i = 0 ; i < motions.size() ; ++i)
00256 for (unsigned int j = 0 ; j < motions[i].size() ; ++j)
00257 {
00258 if (motions[i][j]->parent)
00259 {
00260 if (data.hasControls())
00261 data.addEdge (base::PlannerDataVertex (motions[i][j]->parent->state),
00262 base::PlannerDataVertex (motions[i][j]->state),
00263 PlannerDataEdgeControl(motions[i][j]->control, motions[i][j]->steps * stepSize));
00264 else
00265 data.addEdge (base::PlannerDataVertex (motions[i][j]->parent->state),
00266 base::PlannerDataVertex (motions[i][j]->state));
00267 }
00268 else
00269 data.addStartVertex (base::PlannerDataVertex (motions[i][j]->state));
00270 }
00271 }