ompl/geometric/planners/kpiece/src/BKPIECE1.cpp
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2011, 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: Ioan Sucan */ 00036 00037 #include "ompl/geometric/planners/kpiece/BKPIECE1.h" 00038 #include "ompl/base/goals/GoalSampleableRegion.h" 00039 #include "ompl/tools/config/SelfConfig.h" 00040 #include <cassert> 00041 00042 ompl::geometric::BKPIECE1::BKPIECE1(const base::SpaceInformationPtr &si) : base::Planner(si, "BKPIECE1"), 00043 dStart_(boost::bind(&BKPIECE1::freeMotion, this, _1)), 00044 dGoal_(boost::bind(&BKPIECE1::freeMotion, this, _1)) 00045 { 00046 specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION; 00047 00048 minValidPathFraction_ = 0.5; 00049 failedExpansionScoreFactor_ = 0.5; 00050 maxDistance_ = 0.0; 00051 connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL); 00052 00053 Planner::declareParam<double>("range", this, &BKPIECE1::setRange, &BKPIECE1::getRange, "0.:1.:10000."); 00054 Planner::declareParam<double>("border_fraction", this, &BKPIECE1::setBorderFraction, &BKPIECE1::getBorderFraction, "0.:.05:1."); 00055 Planner::declareParam<double>("failed_expansion_score_factor", this, &BKPIECE1::setFailedExpansionCellScoreFactor, &BKPIECE1::getFailedExpansionCellScoreFactor); 00056 Planner::declareParam<double>("min_valid_path_fraction", this, &BKPIECE1::setMinValidPathFraction, &BKPIECE1::getMinValidPathFraction); 00057 } 00058 00059 ompl::geometric::BKPIECE1::~BKPIECE1() 00060 { 00061 } 00062 00063 void ompl::geometric::BKPIECE1::setup() 00064 { 00065 Planner::setup(); 00066 tools::SelfConfig sc(si_, getName()); 00067 sc.configureProjectionEvaluator(projectionEvaluator_); 00068 sc.configurePlannerRange(maxDistance_); 00069 00070 if (failedExpansionScoreFactor_ < std::numeric_limits<double>::epsilon() || failedExpansionScoreFactor_ > 1.0) 00071 throw Exception("Failed expansion cell score factor must be in the range (0,1]"); 00072 if (minValidPathFraction_ < std::numeric_limits<double>::epsilon() || minValidPathFraction_ > 1.0) 00073 throw Exception("The minimum valid path fraction must be in the range (0,1]"); 00074 00075 dStart_.setDimension(projectionEvaluator_->getDimension()); 00076 dGoal_.setDimension(projectionEvaluator_->getDimension()); 00077 } 00078 00079 ompl::base::PlannerStatus ompl::geometric::BKPIECE1::solve(const base::PlannerTerminationCondition &ptc) 00080 { 00081 checkValidity(); 00082 base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get()); 00083 00084 if (!goal) 00085 { 00086 OMPL_ERROR("%s: Unknown type of goal", getName().c_str()); 00087 return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE; 00088 } 00089 00090 Discretization<Motion>::Coord xcoord; 00091 00092 while (const base::State *st = pis_.nextStart()) 00093 { 00094 Motion *motion = new Motion(si_); 00095 si_->copyState(motion->state, st); 00096 motion->root = motion->state; 00097 projectionEvaluator_->computeCoordinates(motion->state, xcoord); 00098 dStart_.addMotion(motion, xcoord); 00099 } 00100 00101 if (dStart_.getMotionCount() == 0) 00102 { 00103 OMPL_ERROR("%s: Motion planning start tree could not be initialized!", getName().c_str()); 00104 return base::PlannerStatus::INVALID_START; 00105 } 00106 00107 if (!goal->couldSample()) 00108 { 00109 OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str()); 00110 return base::PlannerStatus::INVALID_GOAL; 00111 } 00112 00113 if (!sampler_) 00114 sampler_ = si_->allocValidStateSampler(); 00115 00116 OMPL_INFORM("%s: Starting planning with %d states already in datastructure", getName().c_str(), (int)(dStart_.getMotionCount() + dGoal_.getMotionCount())); 00117 00118 std::vector<Motion*> solution; 00119 base::State *xstate = si_->allocState(); 00120 bool startTree = true; 00121 bool solved = false; 00122 00123 while (ptc == false) 00124 { 00125 Discretization<Motion> &disc = startTree ? dStart_ : dGoal_; 00126 startTree = !startTree; 00127 Discretization<Motion> &otherDisc = startTree ? dStart_ : dGoal_; 00128 disc.countIteration(); 00129 00130 // if we have not sampled too many goals already 00131 if (dGoal_.getMotionCount() == 0 || pis_.getSampledGoalsCount() < dGoal_.getMotionCount() / 2) 00132 { 00133 const base::State *st = dGoal_.getMotionCount() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal(); 00134 if (st) 00135 { 00136 Motion *motion = new Motion(si_); 00137 si_->copyState(motion->state, st); 00138 motion->root = motion->state; 00139 projectionEvaluator_->computeCoordinates(motion->state, xcoord); 00140 dGoal_.addMotion(motion, xcoord); 00141 } 00142 if (dGoal_.getMotionCount() == 0) 00143 { 00144 OMPL_ERROR("%s: Unable to sample any valid states for goal tree", getName().c_str()); 00145 break; 00146 } 00147 } 00148 00149 Discretization<Motion>::Cell *ecell = NULL; 00150 Motion *existing = NULL; 00151 disc.selectMotion(existing, ecell); 00152 assert(existing); 00153 if (sampler_->sampleNear(xstate, existing->state, maxDistance_)) 00154 { 00155 std::pair<base::State*, double> fail(xstate, 0.0); 00156 bool keep = si_->checkMotion(existing->state, xstate, fail); 00157 if (!keep && fail.second > minValidPathFraction_) 00158 keep = true; 00159 00160 if (keep) 00161 { 00162 /* create a motion */ 00163 Motion *motion = new Motion(si_); 00164 si_->copyState(motion->state, xstate); 00165 motion->root = existing->root; 00166 motion->parent = existing; 00167 00168 projectionEvaluator_->computeCoordinates(motion->state, xcoord); 00169 disc.addMotion(motion, xcoord); 00170 00171 Discretization<Motion>::Cell* cellC = otherDisc.getGrid().getCell(xcoord); 00172 00173 if (cellC && !cellC->data->motions.empty()) 00174 { 00175 Motion *connectOther = cellC->data->motions[rng_.uniformInt(0, cellC->data->motions.size() - 1)]; 00176 00177 if (goal->isStartGoalPairValid(startTree ? connectOther->root : motion->root, startTree ? motion->root : connectOther->root) && 00178 si_->checkMotion(motion->state, connectOther->state)) 00179 { 00180 if (startTree) 00181 connectionPoint_ = std::make_pair(connectOther->state, motion->state); 00182 else 00183 connectionPoint_ = std::make_pair(motion->state, connectOther->state); 00184 00185 /* extract the motions and put them in solution vector */ 00186 00187 std::vector<Motion*> mpath1; 00188 while (motion != NULL) 00189 { 00190 mpath1.push_back(motion); 00191 motion = motion->parent; 00192 } 00193 00194 std::vector<Motion*> mpath2; 00195 while (connectOther != NULL) 00196 { 00197 mpath2.push_back(connectOther); 00198 connectOther = connectOther->parent; 00199 } 00200 00201 if (startTree) 00202 mpath1.swap(mpath2); 00203 00204 PathGeometric *path = new PathGeometric(si_); 00205 path->getStates().reserve(mpath1.size() + mpath2.size()); 00206 for (int i = mpath1.size() - 1 ; i >= 0 ; --i) 00207 path->append(mpath1[i]->state); 00208 for (unsigned int i = 0 ; i < mpath2.size() ; ++i) 00209 path->append(mpath2[i]->state); 00210 00211 pdef_->addSolutionPath(base::PathPtr(path), false, 0.0, getName()); 00212 solved = true; 00213 break; 00214 } 00215 } 00216 } 00217 else 00218 ecell->data->score *= failedExpansionScoreFactor_; 00219 } 00220 else 00221 ecell->data->score *= failedExpansionScoreFactor_; 00222 disc.updateCell(ecell); 00223 } 00224 00225 si_->freeState(xstate); 00226 00227 OMPL_INFORM("%s: Created %u (%u start + %u goal) states in %u cells (%u start (%u on boundary) + %u goal (%u on boundary))", 00228 getName().c_str(), 00229 dStart_.getMotionCount() + dGoal_.getMotionCount(), dStart_.getMotionCount(), dGoal_.getMotionCount(), 00230 dStart_.getCellCount() + dGoal_.getCellCount(), dStart_.getCellCount(), dStart_.getGrid().countExternal(), 00231 dGoal_.getCellCount(), dGoal_.getGrid().countExternal()); 00232 00233 return solved ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT; 00234 } 00235 00236 void ompl::geometric::BKPIECE1::freeMotion(Motion *motion) 00237 { 00238 if (motion->state) 00239 si_->freeState(motion->state); 00240 delete motion; 00241 } 00242 00243 void ompl::geometric::BKPIECE1::clear() 00244 { 00245 Planner::clear(); 00246 00247 sampler_.reset(); 00248 dStart_.clear(); 00249 dGoal_.clear(); 00250 connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL); 00251 } 00252 00253 void ompl::geometric::BKPIECE1::getPlannerData(base::PlannerData &data) const 00254 { 00255 Planner::getPlannerData(data); 00256 dStart_.getPlannerData(data, 1, true, NULL); 00257 dGoal_.getPlannerData(data, 2, false, NULL); 00258 00259 // Insert the edge connecting the two trees 00260 data.addEdge (data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second)); 00261 }