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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines