ompl/geometric/planners/pdst/src/PDST.cpp
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2013, Rice University 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions 00009 * are met: 00010 * 00011 * * Redistributions of source code must retain the above copyright 00012 * notice, this list of conditions and the following disclaimer. 00013 * * Redistributions in binary form must reproduce the above 00014 * copyright notice, this list of conditions and the following 00015 * disclaimer in the documentation and/or other materials provided 00016 * with the distribution. 00017 * * Neither the name of the Rice University nor the names of its 00018 * contributors may be used to endorse or promote products derived 00019 * from this software without specific prior written permission. 00020 * 00021 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 * POSSIBILITY OF SUCH DAMAGE. 00033 *********************************************************************/ 00034 00035 /* Author: Jonathan Sobieski, Mark Moll */ 00036 00037 #include "ompl/tools/config/SelfConfig.h" 00038 #include "ompl/geometric/planners/pdst/PDST.h" 00039 00040 ompl::geometric::PDST::PDST(const base::SpaceInformationPtr &si) 00041 : base::Planner(si, "PDST"), bsp_(NULL), goalBias_(0.05), 00042 goalSampler_(NULL), iteration_(1), lastGoalMotion_(NULL) 00043 { 00044 Planner::declareParam<double>("goal_bias", this, &PDST::setGoalBias, &PDST::getGoalBias, "0.:.05:1."); 00045 } 00046 00047 ompl::geometric::PDST::~PDST() 00048 { 00049 freeMemory(); 00050 } 00051 00052 ompl::base::PlannerStatus ompl::geometric::PDST::solve(const base::PlannerTerminationCondition &ptc) 00053 { 00054 // Make sure the planner is configured correctly. 00055 // ompl::base::Planner::checkValidity checks that there is at least one 00056 // start state and an ompl::base::Goal object specified and throws an 00057 // exception if this is not the case. 00058 checkValidity(); 00059 00060 if (!bsp_) 00061 throw Exception("PDST was not set up."); 00062 00063 // depending on how the planning problem is set up, this may be necessary 00064 bsp_->bounds_ = projectionEvaluator_->getBounds(); 00065 00066 base::Goal *goal = pdef_->getGoal().get(); 00067 goalSampler_ = dynamic_cast<ompl::base::GoalSampleableRegion*>(goal); 00068 00069 // Ensure that we have a state sampler 00070 if (!sampler_) 00071 sampler_ = si_->allocStateSampler(); 00072 00073 // Initialize to correct values depending on whether or not previous calls to solve 00074 // generated an approximate or exact solution. If solve is being called for the first 00075 // time then initializes hasSolution to false and isApproximate to true. 00076 double distanceToGoal, closestDistanceToGoal = std::numeric_limits<double>::infinity(); 00077 bool hasSolution = lastGoalMotion_ != NULL; 00078 bool isApproximate = !hasSolution || !goal->isSatisfied(lastGoalMotion_->endState_, &closestDistanceToGoal); 00079 unsigned ndim = projectionEvaluator_->getDimension(); 00080 00081 // If an exact solution has already been found, do not run for another iteration. 00082 if (hasSolution && !isApproximate) 00083 return ompl::base::PlannerStatus::EXACT_SOLUTION; 00084 00085 // Initialize tree with start state(s) 00086 while (const base::State *st = pis_.nextStart()) 00087 { 00088 Motion *startMotion = new Motion(si_->cloneState(st)); 00089 bsp_->addMotion(startMotion); 00090 startMotion->heapElement_ = priorityQueue_.insert(startMotion); 00091 } 00092 00093 if (priorityQueue_.empty()) 00094 { 00095 OMPL_ERROR("%s: There are no valid initial states!", getName().c_str()); 00096 return base::PlannerStatus::INVALID_START; 00097 } 00098 00099 OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), priorityQueue_.size()); 00100 00101 base::State *tmpState1 = si_->allocState(), *tmpState2 = si_->allocState(); 00102 base::EuclideanProjection tmpProj(ndim); 00103 while (!ptc) 00104 { 00105 // Get the top priority path. 00106 Motion *motionSelected = priorityQueue_.top()->data; 00107 motionSelected->updatePriority(); 00108 priorityQueue_.update(motionSelected->heapElement_); 00109 00110 Motion *newMotion = propagateFrom(motionSelected, tmpState1, tmpState2); 00111 addMotion(newMotion, bsp_, tmpState1, tmpProj); 00112 00113 // Check if the newMotion reached the goal. 00114 hasSolution = goal->isSatisfied(newMotion->endState_, &distanceToGoal); 00115 if (hasSolution) 00116 { 00117 closestDistanceToGoal = distanceToGoal; 00118 lastGoalMotion_ = newMotion; 00119 isApproximate = false; 00120 break; 00121 } 00122 else if (distanceToGoal < closestDistanceToGoal) 00123 { 00124 closestDistanceToGoal = distanceToGoal; 00125 lastGoalMotion_ = newMotion; 00126 } 00127 00128 // subdivide cell that contained selected motion, put motions of that 00129 // cell in subcells and split motions so that they contained within 00130 // one subcell 00131 Cell *cellSelected = motionSelected->cell_; 00132 std::vector<Motion*> motions; 00133 cellSelected->subdivide(ndim); 00134 motions.swap(cellSelected->motions_); 00135 for (std::vector<Motion*>::iterator m = motions.begin() ; m != motions.end() ; ++m) 00136 addMotion(*m, cellSelected, tmpState1, tmpProj); 00137 } 00138 00139 if (lastGoalMotion_ != NULL) 00140 hasSolution = true; 00141 00142 // If a solution path has been computed, save it in the problem definition object. 00143 if (hasSolution) 00144 { 00145 PathGeometric *path = new PathGeometric(si_); 00146 00147 // Compute the path by going up the tree of motions. 00148 std::vector<base::State*> spath(1, lastGoalMotion_->endState_); 00149 Motion *m = lastGoalMotion_; 00150 while (m) 00151 { 00152 m = m->ancestor(); 00153 spath.push_back(m->startState_); 00154 m = m->parent_; 00155 } 00156 00157 // Add the solution path in order from the start state to the goal. 00158 for (std::vector<base::State*>::reverse_iterator rIt = spath.rbegin(); rIt != spath.rend(); ++rIt) 00159 path->append((*rIt)); 00160 pdef_->addSolutionPath(base::PathPtr(path), isApproximate, closestDistanceToGoal, getName()); 00161 } 00162 00163 si_->freeState(tmpState1); 00164 si_->freeState(tmpState2); 00165 00166 OMPL_INFORM("%s: Created %u states and %u cells", getName().c_str(), priorityQueue_.size(), bsp_->size()); 00167 00168 return base::PlannerStatus(hasSolution, isApproximate); 00169 } 00170 00171 ompl::geometric::PDST::Motion *ompl::geometric::PDST::propagateFrom( 00172 Motion *motion, base::State *start, base::State *rnd) 00173 { 00174 // sample a point along the trajectory given by motion 00175 si_->getStateSpace()->interpolate(motion->startState_, motion->endState_, rng_.uniform01(), start); 00176 // generate a random state 00177 if (goalSampler_ && rng_.uniform01() < goalBias_ && goalSampler_->canSample()) 00178 goalSampler_->sampleGoal(rnd); 00179 else 00180 sampler_->sampleUniform(rnd); 00181 // compute longest valid segment from start towards rnd 00182 std::pair<base::State*, double> lastValid = std::make_pair(rnd, 0.); 00183 si_->checkMotion(start, rnd, lastValid); 00184 return new Motion(si_->cloneState(start), si_->cloneState(rnd), ++iteration_, motion); 00185 } 00186 00187 void ompl::geometric::PDST::addMotion(Motion *motion, Cell *bsp, base::State *state, base::EuclideanProjection& proj) 00188 { 00189 projectionEvaluator_->project(motion->endState_, proj); 00190 bsp->stab(proj)->addMotion(motion); 00191 updateHeapElement(motion); 00192 00193 // If the motion corresponds to a start state, then it cannot be split across cell 00194 // bounds. So we're done. 00195 if (!motion->parent_) 00196 return; 00197 00198 // Otherwise, split into smaller segments s.t. endpoints project into same cell 00199 int numSegments = si_->getStateSpace()->validSegmentCount(motion->startState_, motion->endState_); 00200 Cell *startCell; 00201 projectionEvaluator_->project(motion->startState_, proj); 00202 startCell = bsp->stab(proj); 00203 00204 while (startCell != motion->cell_ && numSegments > 1) 00205 { 00206 Cell *nextStartCell = motion->cell_, *cell; 00207 int i = 0, j = numSegments, k = 1; 00208 // Find the largest k such that the interpolated state at k/numSegments is 00209 // still in startCell. The variables i and j bracket the range that k 00210 // can be in. 00211 while (j - i > 1) 00212 { 00213 k = (i + j) / 2; 00214 si_->getStateSpace()->interpolate(motion->startState_, motion->endState_, 00215 (double)k / (double)numSegments, state); 00216 projectionEvaluator_->project(state, proj); 00217 cell = bsp->stab(proj); 00218 if (cell == startCell) 00219 i = k; 00220 else 00221 { 00222 j = k; 00223 nextStartCell = cell; 00224 } 00225 } 00226 00227 Motion *m = new Motion(motion->startState_, si_->cloneState(state), motion->priority_, motion->parent_); 00228 startCell->addMotion(m); 00229 m->heapElement_ = priorityQueue_.insert(m); 00230 m->isSplit_ = true; 00231 motion->startState_ = m->endState_; 00232 motion->parent_ = m; 00233 numSegments -= k; 00234 startCell = nextStartCell; 00235 } 00236 } 00237 00238 void ompl::geometric::PDST::clear() 00239 { 00240 Planner::clear(); 00241 sampler_.reset(); 00242 iteration_ = 1; 00243 lastGoalMotion_ = NULL; 00244 freeMemory(); 00245 bsp_ = new Cell(1., projectionEvaluator_->getBounds(), 0); 00246 } 00247 00248 void ompl::geometric::PDST::freeMemory() 00249 { 00250 // Iterate over the elements in the priority queue and clear it 00251 std::vector<Motion*> motions; 00252 motions.reserve(priorityQueue_.size()); 00253 priorityQueue_.getContent(motions); 00254 for (std::vector<Motion*>::iterator it = motions.begin(); it < motions.end(); ++it) 00255 { 00256 if ((*it)->startState_ != (*it)->endState_) 00257 si_->freeState((*it)->startState_); 00258 if (!(*it)->isSplit_) 00259 si_->freeState((*it)->endState_); 00260 delete *it; 00261 } 00262 priorityQueue_.clear(); // clears the Element objects in the priority queue 00263 delete bsp_; 00264 bsp_ = NULL; 00265 } 00266 00267 void ompl::geometric::PDST::setup() 00268 { 00269 Planner::setup(); 00270 tools::SelfConfig sc(si_, getName()); 00271 sc.configureProjectionEvaluator(projectionEvaluator_); 00272 if (!projectionEvaluator_->hasBounds()) 00273 projectionEvaluator_->inferBounds(); 00274 if (!projectionEvaluator_->hasBounds()) 00275 throw Exception("PDST requires a projection evaluator that specifies bounds for the projected space"); 00276 if (bsp_) 00277 delete bsp_; 00278 bsp_ = new Cell(1., projectionEvaluator_->getBounds(), 0); 00279 lastGoalMotion_ = NULL; 00280 } 00281 00282 void ompl::geometric::PDST::getPlannerData(ompl::base::PlannerData &data) const 00283 { 00284 base::Planner::getPlannerData(data); 00285 00286 std::vector<Motion*> motions; 00287 priorityQueue_.getContent(motions); 00288 00289 // Add goal vertex 00290 if (lastGoalMotion_ != NULL) 00291 data.addGoalVertex(lastGoalMotion_->endState_); 00292 00293 for (std::vector<Motion*>::iterator it = motions.begin(); it < motions.end(); ++it) 00294 if (!(*it)->isSplit_) 00295 { 00296 Motion *cur = *it, *ancestor = cur->ancestor(); 00297 if (!cur->parent_) 00298 data.addStartVertex(base::PlannerDataVertex(cur->endState_)); 00299 else 00300 { 00301 data.addEdge(base::PlannerDataVertex(ancestor->startState_), 00302 base::PlannerDataVertex(cur->endState_)); 00303 // add edge connecting start state of ancestor's parent to start state of ancestor 00304 if (ancestor->parent_) 00305 data.addEdge(base::PlannerDataVertex(ancestor->parent_->ancestor()->startState_), 00306 base::PlannerDataVertex(ancestor->startState_)); 00307 } 00308 } 00309 } 00310 00311 void ompl::geometric::PDST::Cell::subdivide(unsigned int spaceDimension) 00312 { 00313 double childVolume = .5 * volume_; 00314 unsigned int nextSplitDimension = (splitDimension_ + 1) % spaceDimension; 00315 splitValue_ = .5 * (bounds_.low[splitDimension_] + bounds_.high[splitDimension_]); 00316 00317 left_ = new Cell(childVolume, bounds_, nextSplitDimension); 00318 left_->bounds_.high[splitDimension_] = splitValue_; 00319 left_->motions_.reserve(motions_.size()); 00320 right_ = new Cell(childVolume, bounds_, nextSplitDimension); 00321 right_->bounds_.low[splitDimension_] = splitValue_; 00322 right_->motions_.reserve(motions_.size()); 00323 }