ompl/geometric/planners/stride/src/STRIDE.cpp
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
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 Willow Garage 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: Bryant Gipson, Mark Moll, Ioan Sucan */
00036 
00037 #include "ompl/geometric/planners/stride/STRIDE.h"
00038 // enable sampling from the GNAT data structure
00039 #define GNAT_SAMPLER
00040 #include "ompl/datastructures/NearestNeighborsGNAT.h"
00041 #include "ompl/base/goals/GoalSampleableRegion.h"
00042 #include "ompl/tools/config/SelfConfig.h"
00043 #include <limits>
00044 #include <cassert>
00045 
00046 ompl::geometric::STRIDE::STRIDE(const base::SpaceInformationPtr &si,
00047     bool useProjectedDistance,
00048     unsigned int degree, unsigned int minDegree,
00049     unsigned int maxDegree, unsigned int maxNumPtsPerLeaf, double estimatedDimension)
00050     : base::Planner(si, "STRIDE"), goalBias_(0.05), maxDistance_(0.),
00051     useProjectedDistance_(useProjectedDistance),
00052     degree_(degree), minDegree_(minDegree), maxDegree_(maxDegree),
00053     maxNumPtsPerLeaf_(maxNumPtsPerLeaf), estimatedDimension_(estimatedDimension),
00054     minValidPathFraction_(0.2)
00055 {
00056     specs_.approximateSolutions = true;
00057 
00058     if (estimatedDimension_ < 1.)
00059         estimatedDimension_ = si->getStateDimension();
00060 
00061     Planner::declareParam<double>("range", this, &STRIDE::setRange, &STRIDE::getRange, "0.:1.:10000.");
00062     Planner::declareParam<double>("goal_bias", this, &STRIDE::setGoalBias, &STRIDE::getGoalBias, "0.:.05:1.");
00063     Planner::declareParam<bool>("use_projected_distance", this, &STRIDE::setUseProjectedDistance, &STRIDE::getUseProjectedDistance, "0,1");
00064     Planner::declareParam<unsigned int>("degree", this, &STRIDE::setDegree, &STRIDE::getDegree, "2:20");
00065     Planner::declareParam<unsigned int>("max_degree", this, &STRIDE::setMaxDegree, &STRIDE::getMaxDegree, "2:20");
00066     Planner::declareParam<unsigned int>("min_degree", this, &STRIDE::setMinDegree, &STRIDE::getMinDegree, "2:20");
00067     Planner::declareParam<unsigned int>("max_pts_per_leaf", this, &STRIDE::setMaxNumPtsPerLeaf, &STRIDE::getMaxNumPtsPerLeaf, "1:200");
00068     Planner::declareParam<double>("estimated_dimension", this, &STRIDE::setEstimatedDimension, &STRIDE::getEstimatedDimension, "1.:30.");
00069     Planner::declareParam<double>("min_valid_path_fraction", this, &STRIDE::setMinValidPathFraction, &STRIDE::getMinValidPathFraction, "0.:.05:1.");
00070 }
00071 
00072 ompl::geometric::STRIDE::~STRIDE()
00073 {
00074     freeMemory();
00075 }
00076 
00077 void ompl::geometric::STRIDE::setup()
00078 {
00079     Planner::setup();
00080     tools::SelfConfig sc(si_, getName());
00081     sc.configureProjectionEvaluator(projectionEvaluator_);
00082     sc.configurePlannerRange(maxDistance_);
00083     setupTree();
00084 }
00085 
00086 void ompl::geometric::STRIDE::setupTree()
00087 {
00088     tree_.reset(new NearestNeighborsGNAT<Motion*>(degree_, minDegree_, maxDegree_, maxNumPtsPerLeaf_, estimatedDimension_));
00089     if (useProjectedDistance_)
00090         tree_->setDistanceFunction(boost::bind(&STRIDE::projectedDistanceFunction, this, _1, _2));
00091     else
00092         tree_->setDistanceFunction(boost::bind(&STRIDE::distanceFunction, this, _1, _2));
00093 }
00094 
00095 void ompl::geometric::STRIDE::clear()
00096 {
00097     Planner::clear();
00098     sampler_.reset();
00099     freeMemory();
00100     setupTree();
00101 }
00102 
00103 void ompl::geometric::STRIDE::freeMemory()
00104 {
00105     if (tree_)
00106     {
00107         std::vector<Motion*> motions;
00108         tree_->list(motions);
00109         for (std::size_t i = 0 ; i < motions.size() ; ++i)
00110         {
00111             if (motions[i]->state)
00112                 si_->freeState(motions[i]->state);
00113             delete motions[i];
00114         }
00115         tree_.reset();
00116     }
00117 }
00118 
00119 ompl::base::PlannerStatus ompl::geometric::STRIDE::solve(const base::PlannerTerminationCondition &ptc)
00120 {
00121     checkValidity();
00122     base::Goal                   *goal = pdef_->getGoal().get();
00123     base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
00124 
00125     while (const base::State *st = pis_.nextStart())
00126     {
00127         Motion *motion = new Motion(si_);
00128         si_->copyState(motion->state, st);
00129         addMotion(motion);
00130     }
00131 
00132     if (tree_->size() == 0)
00133     {
00134         OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
00135         return base::PlannerStatus::INVALID_START;
00136     }
00137 
00138     if (!sampler_)
00139         sampler_ = si_->allocValidStateSampler();
00140 
00141     OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), tree_->size());
00142 
00143     Motion *solution  = NULL;
00144     Motion *approxsol = NULL;
00145     double  approxdif = std::numeric_limits<double>::infinity();
00146     base::State *xstate = si_->allocState();
00147 
00148     while (ptc == false)
00149     {
00150         /* Decide on a state to expand from */
00151         Motion *existing = selectMotion();
00152         assert(existing);
00153 
00154         /* sample random state (with goal biasing) */
00155         if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
00156             goal_s->sampleGoal(xstate);
00157         else
00158             if (!sampler_->sampleNear(xstate, existing->state, maxDistance_))
00159                 continue;
00160 
00161         std::pair<base::State*, double> fail(xstate, 0.0);
00162         bool keep = si_->checkMotion(existing->state, xstate, fail) || fail.second > minValidPathFraction_;
00163 
00164         if (keep)
00165         {
00166             /* create a motion */
00167             Motion *motion = new Motion(si_);
00168             si_->copyState(motion->state, xstate);
00169             motion->parent = existing;
00170 
00171             addMotion(motion);
00172             double dist = 0.0;
00173             bool solved = goal->isSatisfied(motion->state, &dist);
00174             if (solved)
00175             {
00176                 approxdif = dist;
00177                 solution = motion;
00178                 break;
00179             }
00180             if (dist < approxdif)
00181             {
00182                 approxdif = dist;
00183                 approxsol = motion;
00184             }
00185         }
00186     }
00187 
00188     bool solved = false;
00189     bool approximate = false;
00190     if (solution == NULL)
00191     {
00192         solution = approxsol;
00193         approximate = true;
00194     }
00195 
00196     if (solution != NULL)
00197     {
00198         /* construct the solution path */
00199         std::vector<Motion*> mpath;
00200         while (solution != NULL)
00201         {
00202             mpath.push_back(solution);
00203             solution = solution->parent;
00204         }
00205 
00206         /* set the solution path */
00207         PathGeometric *path = new PathGeometric(si_);
00208         for (int i = mpath.size() - 1 ; i >= 0 ; --i)
00209             path->append(mpath[i]->state);
00210         pdef_->addSolutionPath(base::PathPtr(path), approximate, approxdif, getName());
00211         solved = true;
00212     }
00213 
00214     si_->freeState(xstate);
00215 
00216     OMPL_INFORM("%s: Created %u states", getName().c_str(), tree_->size());
00217 
00218     return base::PlannerStatus(solved, approximate);
00219 }
00220 
00221 void ompl::geometric::STRIDE::addMotion(Motion *motion)
00222 {
00223     tree_->add(motion);
00224 }
00225 
00226 ompl::geometric::STRIDE::Motion* ompl::geometric::STRIDE::selectMotion()
00227 {
00228     return tree_->sample(rng_);
00229 }
00230 
00231 void ompl::geometric::STRIDE::getPlannerData(base::PlannerData &data) const
00232 {
00233     Planner::getPlannerData(data);
00234 
00235     std::vector<Motion*> motions;
00236     tree_->list(motions);
00237     for (std::vector<Motion*>::iterator it=motions.begin(); it!=motions.end(); it++)
00238     {
00239         if((*it)->parent == NULL)
00240             data.addStartVertex(base::PlannerDataVertex((*it)->state,1));
00241         else
00242             data.addEdge(base::PlannerDataVertex((*it)->parent->state,1),base::PlannerDataVertex((*it)->state,1));
00243     }
00244 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines