ompl/control/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/control/planners/pdst/PDST.h"
00039 
00040 ompl::control::PDST::PDST(const SpaceInformationPtr &si)
00041     : base::Planner(si, "PDST"), siC_(si.get()), 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::control::PDST::~PDST()
00048 {
00049     freeMemory();
00050 }
00051 
00052 ompl::base::PlannerStatus ompl::control::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     // depending on how the planning problem is set up, this may be necessary
00061     bsp_->bounds_ = projectionEvaluator_->getBounds();
00062     base::Goal *goal = pdef_->getGoal().get();
00063     goalSampler_ = dynamic_cast<ompl::base::GoalSampleableRegion*>(goal);
00064 
00065     // Ensure that we have a state sampler AND a control sampler
00066     if (!sampler_)
00067         sampler_ = si_->allocStateSampler();
00068     if (!controlSampler_)
00069         controlSampler_ = siC_->allocDirectedControlSampler();
00070 
00071     // Initialize to correct values depending on whether or not previous calls to solve
00072     // generated an approximate or exact solution. If solve is being called for the first
00073     // time then initializes hasSolution to false and isApproximate to true.
00074     double distanceToGoal, closestDistanceToGoal = std::numeric_limits<double>::infinity();
00075     bool hasSolution = lastGoalMotion_ != NULL;
00076     bool isApproximate = !hasSolution || !goal->isSatisfied(lastGoalMotion_->endState_, &closestDistanceToGoal);
00077     unsigned int ndim = projectionEvaluator_->getDimension();
00078 
00079     // If an exact solution has already been found, do not run for another iteration.
00080     if (hasSolution && !isApproximate)
00081         return ompl::base::PlannerStatus::EXACT_SOLUTION;
00082 
00083     // Initialize tree with start state(s)
00084     while (const base::State *st = pis_.nextStart())
00085     {
00086         Motion *startMotion = new Motion(si_->cloneState(st));
00087         bsp_->addMotion(startMotion);
00088         startMotion->heapElement_ = priorityQueue_.insert(startMotion);
00089     }
00090 
00091     if (priorityQueue_.empty())
00092     {
00093         OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
00094         return base::PlannerStatus::INVALID_START;
00095     }
00096 
00097     OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), priorityQueue_.size());
00098 
00099     base::State *tmpState1 = si_->allocState(), *tmpState2 = si_->allocState();
00100     base::EuclideanProjection tmpProj1(ndim), tmpProj2(ndim);
00101     while (!ptc)
00102     {
00103         // Get the top priority path.
00104         Motion *motionSelected = priorityQueue_.top()->data;
00105         motionSelected->updatePriority();
00106         priorityQueue_.update(motionSelected->heapElement_);
00107 
00108         Motion *newMotion = propagateFrom(motionSelected, tmpState1, tmpState2);
00109         if (newMotion == NULL)
00110             continue;
00111 
00112         addMotion(newMotion, bsp_, tmpState1, tmpState2, tmpProj1, tmpProj2);
00113 
00114         // Check if the newMotion reached the goal.
00115         hasSolution = goal->isSatisfied(newMotion->endState_, &distanceToGoal);
00116         if (hasSolution)
00117         {
00118             closestDistanceToGoal = distanceToGoal;
00119             lastGoalMotion_ = newMotion;
00120             isApproximate = false;
00121             break;
00122         }
00123         else if (distanceToGoal < closestDistanceToGoal)
00124         {
00125             closestDistanceToGoal = distanceToGoal;
00126             lastGoalMotion_ = newMotion;
00127         }
00128 
00129         // subdivide cell that contained selected motion, put motions of that
00130         // cell in subcells and split motions so that they contained within
00131         // one subcell
00132         Cell *cellSelected = motionSelected->cell_;
00133         std::vector<Motion*> motions;
00134         cellSelected->subdivide(ndim);
00135         motions.swap(cellSelected->motions_);
00136         for (std::vector<Motion*>::iterator m = motions.begin() ; m != motions.end() ; ++m)
00137             addMotion(*m, cellSelected, tmpState1, tmpState2, tmpProj1, tmpProj2);
00138     }
00139 
00140     if (lastGoalMotion_ != NULL)
00141         hasSolution = true;
00142 
00143     // If a solution path has been computed, save it in the problem definition object.
00144     if (hasSolution)
00145     {
00146         Motion *m;
00147         std::vector<unsigned int> durations(1,
00148             findDurationAndAncestor(lastGoalMotion_, lastGoalMotion_->endState_, tmpState1, m));
00149         std::vector<Motion*> mpath(1, m);
00150 
00151         while (m->parent_)
00152         {
00153             durations.push_back(findDurationAndAncestor(m->parent_, m->startState_, tmpState1, m));
00154             mpath.push_back(m);
00155         }
00156 
00157         PathControl *path = new PathControl(si_);
00158         double dt = siC_->getPropagationStepSize();
00159         path->append(mpath.back()->endState_);
00160         for (int i = (int) mpath.size() - 2; i > 0; i--)
00161             path->append(mpath[i-1]->startState_, mpath[i]->control_, durations[i] * dt);
00162         path->append(lastGoalMotion_->endState_, mpath[0]->control_, durations[0] * dt);
00163         pdef_->addSolutionPath(base::PathPtr(path), isApproximate, closestDistanceToGoal, getName());
00164     }
00165 
00166     si_->freeState(tmpState1);
00167     si_->freeState(tmpState2);
00168 
00169     OMPL_INFORM("%s: Created %u states and %u cells", getName().c_str(), priorityQueue_.size(), bsp_->size());
00170 
00171     return base::PlannerStatus(hasSolution, isApproximate);
00172 }
00173 
00174 ompl::control::PDST::Motion* ompl::control::PDST::propagateFrom(
00175     Motion *motion, base::State *start, base::State *rnd)
00176 {
00177     // sample a point along the trajectory given by motion
00178     unsigned int prevDuration = motion->controlDuration_;
00179     if (motion->controlDuration_ > 1)
00180         prevDuration = rng_.uniformInt(1, motion->controlDuration_);
00181     if (prevDuration == motion->controlDuration_)
00182         start = motion->endState_;
00183     else
00184         siC_->propagate(motion->startState_, motion->control_, prevDuration, start);
00185     // generate a random state
00186     if (goalSampler_ && rng_.uniform01() < goalBias_ && goalSampler_->canSample())
00187         goalSampler_->sampleGoal(rnd);
00188     else
00189         sampler_->sampleUniform(rnd);
00190     // generate a random control
00191     Control *control = siC_->allocControl();
00192     unsigned int duration = controlSampler_->sampleTo(control, motion->control_, start, rnd);
00193     // return new motion if duration is large enough
00194     if (duration < siC_->getMinControlDuration())
00195     {
00196         siC_->freeControl(control);
00197         return NULL;
00198     }
00199     return new Motion(si_->cloneState(start), si_->cloneState(rnd),
00200         control, duration, ++iteration_, motion);
00201 }
00202 
00203 void ompl::control::PDST::addMotion(Motion *motion, Cell *bsp, base::State *prevState, base::State *state,
00204     base::EuclideanProjection& prevProj, base::EuclideanProjection& proj)
00205 {
00206     // If the motion is at most 1 step, then it cannot be split across cell bounds.
00207     if (motion->controlDuration_ <= 1)
00208     {
00209         projectionEvaluator_->project(motion->endState_, proj);
00210         bsp->stab(proj)->addMotion(motion);
00211         updateHeapElement(motion);
00212         return;
00213     }
00214 
00215     Cell *cell = NULL, *prevCell = NULL;
00216     si_->copyState(prevState, motion->startState_);
00217     // propagate the motion, check for cell boundary crossings, and split as necessary
00218     for (unsigned int i = 0, duration = 0 ; i < motion->controlDuration_ - 1 ; ++i, ++duration)
00219     {
00220         siC_->propagate(prevState, motion->control_, 1, state);
00221         projectionEvaluator_->project(state, proj);
00222         cell = bsp->stab(proj);
00223         if (duration > 0 && cell != prevCell)
00224         {
00225             Motion *newMotion = new Motion(motion->startState_, si_->cloneState(prevState),
00226                 motion->control_, duration, motion->priority_, motion->parent_);
00227             newMotion->isSplit_ = true;
00228             prevCell->addMotion(newMotion);
00229             updateHeapElement(newMotion);
00230             motion->startState_ = newMotion->endState_;
00231             motion->controlDuration_ -= duration;
00232             motion->parent_ = newMotion;
00233             duration = 0;
00234         }
00235         std::swap(state, prevState);
00236         std::swap(cell, prevCell);
00237         proj.swap(prevProj);
00238     }
00239     prevCell->addMotion(motion);
00240     updateHeapElement(motion);
00241 }
00242 
00243 
00244 unsigned int ompl::control::PDST::findDurationAndAncestor(Motion *motion, base::State *state,
00245     base::State *scratch, Motion*& ancestor) const
00246 {
00247     const double eps = std::numeric_limits<float>::epsilon();
00248     unsigned int duration;
00249     ancestor = motion;
00250     if (state == motion->endState_ || motion->controlDuration_ == 0 ||
00251         si_->distance(motion->endState_, state) < eps)
00252         duration = motion->controlDuration_;
00253     else if (motion->controlDuration_ > 0 &&
00254         si_->distance(motion->startState_, state) < eps)
00255         duration = 0;
00256     else
00257     {
00258         duration = 1;
00259         si_->copyState(scratch, motion->startState_);
00260         for (; duration <= motion->controlDuration_; ++duration)
00261         {
00262             siC_->propagate(scratch, motion->control_, 1, scratch);
00263             if (si_->distance(scratch, state) < eps)
00264                 break;
00265         }
00266     }
00267     if (duration <= motion->controlDuration_)
00268     {
00269         // ancestor points to the start of a split motion; duration is computed
00270         // relative to start state of ancestor motion
00271         while (ancestor->parent_ && ancestor->control_ == ancestor->parent_->control_)
00272         {
00273             ancestor = ancestor->parent_;
00274             duration += ancestor->controlDuration_;
00275         }
00276         return duration;
00277     }
00278     // motion is no longer the parent of the motion starting at
00279     // state because it has been split afterwards, so look for
00280     // the starting point in the parent of motion.
00281     return findDurationAndAncestor(motion->parent_, state, scratch, ancestor);
00282 }
00283 
00284 void ompl::control::PDST::clear()
00285 {
00286     Planner::clear();
00287     sampler_.reset();
00288     controlSampler_.reset();
00289     iteration_ = 1;
00290     lastGoalMotion_ = NULL;
00291     freeMemory();
00292     bsp_ = new Cell(1., projectionEvaluator_->getBounds(), 0);
00293 }
00294 
00295 void ompl::control::PDST::freeMemory()
00296 {
00297     // Iterate over the elements in the priority queue and clear it
00298     std::vector<Motion*> motions;
00299     motions.reserve(priorityQueue_.size());
00300     priorityQueue_.getContent(motions);
00301     for (std::vector<Motion*>::iterator it = motions.begin() ; it < motions.end() ; ++it)
00302     {
00303         if ((*it)->startState_ != (*it)->endState_)
00304             si_->freeState((*it)->startState_);
00305         if (!(*it)->isSplit_)
00306         {
00307             si_->freeState((*it)->endState_);
00308             if ((*it)->control_)
00309                 siC_->freeControl((*it)->control_);
00310         }
00311         delete *it;
00312     }
00313     priorityQueue_.clear(); // clears the Element objects in the priority queue
00314     delete bsp_;
00315     bsp_ = NULL;
00316 }
00317 
00318 void ompl::control::PDST::setup()
00319 {
00320     Planner::setup();
00321     tools::SelfConfig sc(si_, getName());
00322     sc.configureProjectionEvaluator(projectionEvaluator_);
00323     if (!projectionEvaluator_->hasBounds())
00324         projectionEvaluator_->inferBounds();
00325     if (!projectionEvaluator_->hasBounds())
00326         throw Exception("PDST requires a projection evaluator that specifies bounds for the projected space");
00327     if (bsp_)
00328         delete bsp_;
00329     bsp_ = new Cell(1., projectionEvaluator_->getBounds(), 0);
00330     lastGoalMotion_ = NULL;
00331 }
00332 
00333 void ompl::control::PDST::getPlannerData(ompl::base::PlannerData &data) const
00334 {
00335     base::Planner::getPlannerData(data);
00336 
00337     double dt = siC_->getPropagationStepSize();
00338     base::State *scratch = si_->allocState();
00339     std::vector<Motion*> motions;
00340     motions.reserve(priorityQueue_.size());
00341     priorityQueue_.getContent(motions);
00342 
00343     // Add goal vertex
00344     if (lastGoalMotion_ != NULL)
00345         data.addGoalVertex(lastGoalMotion_->endState_);
00346 
00347     for (std::vector<Motion*>::iterator it = motions.begin(); it < motions.end(); ++it)
00348         if (!(*it)->isSplit_)
00349         {
00350             // We only add one edge for each motion that has been split into smaller segments
00351             Motion *cur = *it, *ancestor;
00352             unsigned int duration = findDurationAndAncestor(cur, cur->endState_, scratch, ancestor);
00353 
00354             if (cur->parent_ == NULL)
00355                 data.addStartVertex(base::PlannerDataVertex(cur->endState_));
00356             else if (data.hasControls())
00357             {
00358                 data.addEdge(base::PlannerDataVertex(ancestor->startState_),
00359                     base::PlannerDataVertex(cur->endState_),
00360                     PlannerDataEdgeControl(cur->control_, duration * dt));
00361                 if (ancestor->parent_)
00362                 {
00363                     // Include an edge between start state of parent ancestor motion and
00364                     // the start state of the ancestor motion, which lies somewhere on
00365                     // the parent ancestor motion.
00366                     cur = ancestor;
00367                     duration = findDurationAndAncestor(cur->parent_, cur->startState_, scratch, ancestor);
00368                     data.addEdge(base::PlannerDataVertex(ancestor->startState_),
00369                         base::PlannerDataVertex(cur->startState_),
00370                         PlannerDataEdgeControl(ancestor->control_, duration * dt));
00371                 }
00372             }
00373             else
00374             {
00375                 data.addEdge(base::PlannerDataVertex(ancestor->startState_),
00376                     base::PlannerDataVertex(cur->endState_));
00377                 if (ancestor->parent_)
00378                 {
00379                     // Include an edge between start state of parent ancestor motion and
00380                     // the start state of the ancestor motion, which lies somewhere on
00381                     // the parent ancestor motion.
00382                     cur = ancestor;
00383                     findDurationAndAncestor(cur->parent_, cur->startState_, scratch, ancestor);
00384                     data.addEdge(base::PlannerDataVertex(ancestor->startState_),
00385                         base::PlannerDataVertex(cur->startState_));
00386                 }
00387             }
00388         }
00389 
00390     si_->freeState(scratch);
00391 }
00392 
00393 void ompl::control::PDST::Cell::subdivide(unsigned int spaceDimension)
00394 {
00395     double childVolume = .5 * volume_;
00396     unsigned int nextSplitDimension = (splitDimension_ + 1) % spaceDimension;
00397     splitValue_ = .5 * (bounds_.low[splitDimension_] + bounds_.high[splitDimension_]);
00398 
00399     left_ = new Cell(childVolume, bounds_, nextSplitDimension);
00400     left_->bounds_.high[splitDimension_] = splitValue_;
00401     left_->motions_.reserve(motions_.size());
00402     right_ = new Cell(childVolume, bounds_, nextSplitDimension);
00403     right_->bounds_.low[splitDimension_] = splitValue_;
00404     right_->motions_.reserve(motions_.size());
00405 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines