ompl/geometric/planners/kpiece/src/KPIECE1.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: Ioan Sucan */
00036 
00037 #include "ompl/geometric/planners/kpiece/KPIECE1.h"
00038 #include "ompl/base/goals/GoalSampleableRegion.h"
00039 #include "ompl/tools/config/SelfConfig.h"
00040 #include <limits>
00041 #include <cassert>
00042 
00043 ompl::geometric::KPIECE1::KPIECE1(const base::SpaceInformationPtr &si) : base::Planner(si, "KPIECE1"),
00044                                                                          disc_(boost::bind(&KPIECE1::freeMotion, this, _1))
00045 {
00046     specs_.approximateSolutions = true;
00047     specs_.directed = true;
00048 
00049     goalBias_ = 0.05;
00050     failedExpansionScoreFactor_ = 0.5;
00051     minValidPathFraction_ = 0.2;
00052     maxDistance_ = 0.0;
00053     lastGoalMotion_ = NULL;
00054 
00055     Planner::declareParam<double>("range", this, &KPIECE1::setRange, &KPIECE1::getRange, "0.:1.:10000.");
00056     Planner::declareParam<double>("goal_bias", this, &KPIECE1::setGoalBias, &KPIECE1::getGoalBias, "0.:.05:1.");
00057     Planner::declareParam<double>("border_fraction", this, &KPIECE1::setBorderFraction, &KPIECE1::getBorderFraction, "0.:0.05:1.");
00058     Planner::declareParam<double>("failed_expansion_score_factor", this, &KPIECE1::setFailedExpansionCellScoreFactor, &KPIECE1::getFailedExpansionCellScoreFactor);
00059     Planner::declareParam<double>("min_valid_path_fraction", this, &KPIECE1::setMinValidPathFraction, &KPIECE1::getMinValidPathFraction);
00060 }
00061 
00062 ompl::geometric::KPIECE1::~KPIECE1()
00063 {
00064 }
00065 
00066 void ompl::geometric::KPIECE1::setup()
00067 {
00068     Planner::setup();
00069     tools::SelfConfig sc(si_, getName());
00070     sc.configureProjectionEvaluator(projectionEvaluator_);
00071     sc.configurePlannerRange(maxDistance_);
00072 
00073     if (failedExpansionScoreFactor_ < std::numeric_limits<double>::epsilon() || failedExpansionScoreFactor_ > 1.0)
00074         throw Exception("Failed expansion cell score factor must be in the range (0,1]");
00075     if (minValidPathFraction_ < std::numeric_limits<double>::epsilon() || minValidPathFraction_ > 1.0)
00076         throw Exception("The minimum valid path fraction must be in the range (0,1]");
00077 
00078     disc_.setDimension(projectionEvaluator_->getDimension());
00079 }
00080 
00081 void ompl::geometric::KPIECE1::clear()
00082 {
00083     Planner::clear();
00084     sampler_.reset();
00085     disc_.clear();
00086     lastGoalMotion_ = NULL;
00087 }
00088 
00089 void ompl::geometric::KPIECE1::freeMotion(Motion *motion)
00090 {
00091     if (motion->state)
00092         si_->freeState(motion->state);
00093     delete motion;
00094 }
00095 
00096 ompl::base::PlannerStatus ompl::geometric::KPIECE1::solve(const base::PlannerTerminationCondition &ptc)
00097 {
00098     checkValidity();
00099     base::Goal                   *goal = pdef_->getGoal().get();
00100     base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
00101 
00102     Discretization<Motion>::Coord xcoord;
00103 
00104     while (const base::State *st = pis_.nextStart())
00105     {
00106         Motion *motion = new Motion(si_);
00107         si_->copyState(motion->state, st);
00108         projectionEvaluator_->computeCoordinates(motion->state, xcoord);
00109         disc_.addMotion(motion, xcoord, 1.0);
00110     }
00111 
00112     if (disc_.getMotionCount() == 0)
00113     {
00114         OMPL_ERROR("%s: There are no valid initial states!", getName().c_str());
00115         return base::PlannerStatus::INVALID_START;
00116     }
00117 
00118     if (!sampler_)
00119         sampler_ = si_->allocStateSampler();
00120 
00121     OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), disc_.getMotionCount());
00122 
00123     Motion *solution    = NULL;
00124     Motion *approxsol   = NULL;
00125     double  approxdif   = std::numeric_limits<double>::infinity();
00126     base::State *xstate = si_->allocState();
00127 
00128     while (ptc == false)
00129     {
00130         disc_.countIteration();
00131 
00132         /* Decide on a state to expand from */
00133         Motion                       *existing = NULL;
00134         Discretization<Motion>::Cell *ecell    = NULL;
00135         disc_.selectMotion(existing, ecell);
00136         assert(existing);
00137 
00138         /* sample random state (with goal biasing) */
00139         if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample())
00140             goal_s->sampleGoal(xstate);
00141         else
00142             sampler_->sampleUniformNear(xstate, existing->state, maxDistance_);
00143 
00144         std::pair<base::State*, double> fail(xstate, 0.0);
00145         bool keep = si_->checkMotion(existing->state, xstate, fail);
00146         if (!keep && fail.second > minValidPathFraction_)
00147             keep = true;
00148 
00149         if (keep)
00150         {
00151             /* create a motion */
00152             Motion *motion = new Motion(si_);
00153             si_->copyState(motion->state, xstate);
00154             motion->parent = existing;
00155 
00156             double dist = 0.0;
00157             bool solv = goal->isSatisfied(motion->state, &dist);
00158             projectionEvaluator_->computeCoordinates(motion->state, xcoord);
00159             disc_.addMotion(motion, xcoord, dist); // this will also update the discretization heaps as needed, so no call to updateCell() is needed
00160 
00161             if (solv)
00162             {
00163                 approxdif = dist;
00164                 solution = motion;
00165                 break;
00166             }
00167             if (dist < approxdif)
00168             {
00169                 approxdif = dist;
00170                 approxsol = motion;
00171             }
00172         }
00173         else
00174             ecell->data->score *= failedExpansionScoreFactor_;
00175         disc_.updateCell(ecell);
00176     }
00177 
00178     bool solved = false;
00179     bool approximate = false;
00180     if (solution == NULL)
00181     {
00182         solution = approxsol;
00183         approximate = true;
00184     }
00185 
00186     if (solution != NULL)
00187     {
00188         lastGoalMotion_ = solution;
00189 
00190         /* construct the solution path */
00191         std::vector<Motion*> mpath;
00192         while (solution != NULL)
00193         {
00194             mpath.push_back(solution);
00195             solution = solution->parent;
00196         }
00197 
00198         /* set the solution path */
00199         PathGeometric *path = new PathGeometric(si_);
00200         for (int i = mpath.size() - 1 ; i >= 0 ; --i)
00201             path->append(mpath[i]->state);
00202         pdef_->addSolutionPath(base::PathPtr(path), approximate, approxdif, getName());
00203         solved = true;
00204     }
00205 
00206     si_->freeState(xstate);
00207 
00208     OMPL_INFORM("%s: Created %u states in %u cells (%u internal + %u external)",
00209                 getName().c_str(),
00210                 disc_.getMotionCount(), disc_.getCellCount(),
00211                 disc_.getGrid().countInternal(), disc_.getGrid().countExternal());
00212 
00213     return base::PlannerStatus(solved, approximate);
00214 }
00215 
00216 void ompl::geometric::KPIECE1::getPlannerData(base::PlannerData &data) const
00217 {
00218     Planner::getPlannerData(data);
00219     disc_.getPlannerData(data, 0, true, lastGoalMotion_);
00220 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines