ompl/geometric/planners/kpiece/src/LBKPIECE1.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/LBKPIECE1.h" 00038 #include "ompl/base/goals/GoalSampleableRegion.h" 00039 #include "ompl/tools/config/SelfConfig.h" 00040 #include <cassert> 00041 00042 ompl::geometric::LBKPIECE1::LBKPIECE1(const base::SpaceInformationPtr &si) : base::Planner(si, "LBKPIECE1"), 00043 dStart_(boost::bind(&LBKPIECE1::freeMotion, this, _1)), 00044 dGoal_(boost::bind(&LBKPIECE1::freeMotion, this, _1)) 00045 { 00046 specs_.recognizedGoal = base::GOAL_SAMPLEABLE_REGION; 00047 00048 minValidPathFraction_ = 0.5; 00049 maxDistance_ = 0.0; 00050 connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL); 00051 00052 Planner::declareParam<double>("range", this, &LBKPIECE1::setRange, &LBKPIECE1::getRange, "0.:1.:10000"); 00053 Planner::declareParam<double>("border_fraction", this, &LBKPIECE1::setBorderFraction, &LBKPIECE1::getBorderFraction, "0.:.05:1."); 00054 Planner::declareParam<double>("min_valid_path_fraction", this, &LBKPIECE1::setMinValidPathFraction, &LBKPIECE1::getMinValidPathFraction); 00055 } 00056 00057 ompl::geometric::LBKPIECE1::~LBKPIECE1() 00058 { 00059 } 00060 00061 void ompl::geometric::LBKPIECE1::setup() 00062 { 00063 Planner::setup(); 00064 tools::SelfConfig sc(si_, getName()); 00065 sc.configureProjectionEvaluator(projectionEvaluator_); 00066 sc.configurePlannerRange(maxDistance_); 00067 00068 if (minValidPathFraction_ < std::numeric_limits<double>::epsilon() || minValidPathFraction_ > 1.0) 00069 throw Exception("The minimum valid path fraction must be in the range (0,1]"); 00070 00071 dStart_.setDimension(projectionEvaluator_->getDimension()); 00072 dGoal_.setDimension(projectionEvaluator_->getDimension()); 00073 } 00074 00075 ompl::base::PlannerStatus ompl::geometric::LBKPIECE1::solve(const base::PlannerTerminationCondition &ptc) 00076 { 00077 checkValidity(); 00078 base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get()); 00079 00080 if (!goal) 00081 { 00082 OMPL_ERROR("%s: Unknown type of goal", getName().c_str()); 00083 return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE; 00084 } 00085 00086 Discretization<Motion>::Coord xcoord; 00087 00088 while (const base::State *st = pis_.nextStart()) 00089 { 00090 Motion *motion = new Motion(si_); 00091 si_->copyState(motion->state, st); 00092 motion->root = st; 00093 motion->valid = true; 00094 projectionEvaluator_->computeCoordinates(motion->state, xcoord); 00095 dStart_.addMotion(motion, xcoord); 00096 } 00097 00098 if (dStart_.getMotionCount() == 0) 00099 { 00100 OMPL_ERROR("%s: Motion planning start tree could not be initialized!", getName().c_str()); 00101 return base::PlannerStatus::INVALID_START; 00102 } 00103 00104 if (!goal->couldSample()) 00105 { 00106 OMPL_ERROR("%s: Insufficient states in sampleable goal region", getName().c_str()); 00107 return base::PlannerStatus::INVALID_GOAL; 00108 } 00109 00110 if (!sampler_) 00111 sampler_ = si_->allocStateSampler(); 00112 00113 OMPL_INFORM("%s: Starting planning with %d states already in datastructure", getName().c_str(), (int)(dStart_.getMotionCount() + dGoal_.getMotionCount())); 00114 00115 base::State *xstate = si_->allocState(); 00116 bool startTree = true; 00117 bool solved = false; 00118 00119 while (ptc == false) 00120 { 00121 Discretization<Motion> &disc = startTree ? dStart_ : dGoal_; 00122 startTree = !startTree; 00123 Discretization<Motion> &otherDisc = startTree ? dStart_ : dGoal_; 00124 disc.countIteration(); 00125 00126 // if we have not sampled too many goals already 00127 if (dGoal_.getMotionCount() == 0 || pis_.getSampledGoalsCount() < dGoal_.getMotionCount() / 2) 00128 { 00129 const base::State *st = dGoal_.getMotionCount() == 0 ? pis_.nextGoal(ptc) : pis_.nextGoal(); 00130 if (st) 00131 { 00132 Motion *motion = new Motion(si_); 00133 si_->copyState(motion->state, st); 00134 motion->root = motion->state; 00135 motion->valid = true; 00136 projectionEvaluator_->computeCoordinates(motion->state, xcoord); 00137 dGoal_.addMotion(motion, xcoord); 00138 } 00139 if (dGoal_.getMotionCount() == 0) 00140 { 00141 OMPL_ERROR("%s: Unable to sample any valid states for goal tree", getName().c_str()); 00142 break; 00143 } 00144 } 00145 00146 Discretization<Motion>::Cell *ecell = NULL; 00147 Motion *existing = NULL; 00148 disc.selectMotion(existing, ecell); 00149 assert(existing); 00150 sampler_->sampleUniformNear(xstate, existing->state, maxDistance_); 00151 00152 /* create a motion */ 00153 Motion *motion = new Motion(si_); 00154 si_->copyState(motion->state, xstate); 00155 motion->parent = existing; 00156 motion->root = existing->root; 00157 existing->children.push_back(motion); 00158 projectionEvaluator_->computeCoordinates(motion->state, xcoord); 00159 disc.addMotion(motion, xcoord); 00160 00161 /* attempt to connect trees */ 00162 Discretization<Motion>::Cell *ocell = otherDisc.getGrid().getCell(xcoord); 00163 if (ocell && !ocell->data->motions.empty()) 00164 { 00165 Motion *connectOther = ocell->data->motions[rng_.uniformInt(0, ocell->data->motions.size() - 1)]; 00166 00167 if (goal->isStartGoalPairValid(startTree ? connectOther->root : motion->root, startTree ? motion->root : connectOther->root)) 00168 { 00169 Motion *connect = new Motion(si_); 00170 si_->copyState(connect->state, connectOther->state); 00171 connect->parent = motion; 00172 connect->root = motion->root; 00173 motion->children.push_back(connect); 00174 projectionEvaluator_->computeCoordinates(connect->state, xcoord); 00175 disc.addMotion(connect, xcoord); 00176 00177 if (isPathValid(disc, connect, xstate) && isPathValid(otherDisc, connectOther, xstate)) 00178 { 00179 if (startTree) 00180 connectionPoint_ = std::make_pair(connectOther->state, motion->state); 00181 else 00182 connectionPoint_ = std::make_pair(motion->state, connectOther->state); 00183 00184 /* extract the motions and put them in solution vector */ 00185 00186 std::vector<Motion*> mpath1; 00187 while (motion != NULL) 00188 { 00189 mpath1.push_back(motion); 00190 motion = motion->parent; 00191 } 00192 00193 std::vector<Motion*> mpath2; 00194 while (connectOther != NULL) 00195 { 00196 mpath2.push_back(connectOther); 00197 connectOther = connectOther->parent; 00198 } 00199 00200 if (startTree) 00201 mpath1.swap(mpath2); 00202 00203 PathGeometric *path = new PathGeometric(si_); 00204 path->getStates().reserve(mpath1.size() + mpath2.size()); 00205 for (int i = mpath1.size() - 1 ; i >= 0 ; --i) 00206 path->append(mpath1[i]->state); 00207 for (unsigned int i = 0 ; i < mpath2.size() ; ++i) 00208 path->append(mpath2[i]->state); 00209 00210 pdef_->addSolutionPath(base::PathPtr(path), false, 0.0, getName()); 00211 solved = true; 00212 break; 00213 } 00214 } 00215 } 00216 } 00217 00218 si_->freeState(xstate); 00219 00220 OMPL_INFORM("%s: Created %u (%u start + %u goal) states in %u cells (%u start (%u on boundary) + %u goal (%u on boundary))", 00221 getName().c_str(), 00222 dStart_.getMotionCount() + dGoal_.getMotionCount(), dStart_.getMotionCount(), dGoal_.getMotionCount(), 00223 dStart_.getCellCount() + dGoal_.getCellCount(), dStart_.getCellCount(), dStart_.getGrid().countExternal(), 00224 dGoal_.getCellCount(), dGoal_.getGrid().countExternal()); 00225 00226 return solved ? base::PlannerStatus::EXACT_SOLUTION : base::PlannerStatus::TIMEOUT; 00227 } 00228 00229 bool ompl::geometric::LBKPIECE1::isPathValid(Discretization<Motion> &disc, Motion *motion, base::State *temp) 00230 { 00231 std::vector<Motion*> mpath; 00232 00233 /* construct the solution path */ 00234 while (motion != NULL) 00235 { 00236 mpath.push_back(motion); 00237 motion = motion->parent; 00238 } 00239 00240 std::pair<base::State*, double> lastValid; 00241 lastValid.first = temp; 00242 00243 /* check the path */ 00244 for (int i = mpath.size() - 1 ; i >= 0 ; --i) 00245 if (!mpath[i]->valid) 00246 { 00247 if (si_->checkMotion(mpath[i]->parent->state, mpath[i]->state, lastValid)) 00248 mpath[i]->valid = true; 00249 else 00250 { 00251 Motion *parent = mpath[i]->parent; 00252 removeMotion(disc, mpath[i]); 00253 00254 // add the valid part of the path, if sufficiently long 00255 if (lastValid.second > minValidPathFraction_) 00256 { 00257 Motion *reAdd = new Motion(si_); 00258 si_->copyState(reAdd->state, lastValid.first); 00259 reAdd->parent = parent; 00260 reAdd->root = parent->root; 00261 parent->children.push_back(reAdd); 00262 reAdd->valid = true; 00263 Discretization<Motion>::Coord coord; 00264 projectionEvaluator_->computeCoordinates(reAdd->state, coord); 00265 disc.addMotion(reAdd, coord); 00266 } 00267 00268 return false; 00269 } 00270 } 00271 return true; 00272 } 00273 00274 void ompl::geometric::LBKPIECE1::removeMotion(Discretization<Motion> &disc, Motion *motion) 00275 { 00276 /* remove from grid */ 00277 00278 Discretization<Motion>::Coord coord; 00279 projectionEvaluator_->computeCoordinates(motion->state, coord); 00280 disc.removeMotion(motion, coord); 00281 00282 /* remove self from parent list */ 00283 00284 if (motion->parent) 00285 { 00286 for (unsigned int i = 0 ; i < motion->parent->children.size() ; ++i) 00287 if (motion->parent->children[i] == motion) 00288 { 00289 motion->parent->children.erase(motion->parent->children.begin() + i); 00290 break; 00291 } 00292 } 00293 00294 /* remove children */ 00295 for (unsigned int i = 0 ; i < motion->children.size() ; ++i) 00296 { 00297 motion->children[i]->parent = NULL; 00298 removeMotion(disc, motion->children[i]); 00299 } 00300 00301 freeMotion(motion); 00302 } 00303 00304 00305 void ompl::geometric::LBKPIECE1::freeMotion(Motion *motion) 00306 { 00307 if (motion->state) 00308 si_->freeState(motion->state); 00309 delete motion; 00310 } 00311 00312 void ompl::geometric::LBKPIECE1::clear() 00313 { 00314 Planner::clear(); 00315 00316 sampler_.reset(); 00317 dStart_.clear(); 00318 dGoal_.clear(); 00319 connectionPoint_ = std::make_pair<base::State*, base::State*>(NULL, NULL); 00320 } 00321 00322 void ompl::geometric::LBKPIECE1::getPlannerData(base::PlannerData &data) const 00323 { 00324 Planner::getPlannerData(data); 00325 dStart_.getPlannerData(data, 1, true, NULL); 00326 dGoal_.getPlannerData(data, 2, false, NULL); 00327 00328 // Insert the edge connecting the two trees 00329 data.addEdge (data.vertexIndex(connectionPoint_.first), data.vertexIndex(connectionPoint_.second)); 00330 }