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 }