ompl/control/planners/pdst/src/PDST.cpp
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2013, 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: Jonathan Sobieski, Mark Moll */ 00036 00037 #include "ompl/tools/config/SelfConfig.h" 00038 #include "ompl/control/planners/pdst/PDST.h" 00039 00040 ompl::control::PDST::PDST(const SpaceInformationPtr &si) 00041 : base::Planner(si, "PDST"), siC_(si.get()), bsp_(NULL), goalBias_(0.05), 00042 goalSampler_(NULL), iteration_(1), lastGoalMotion_(NULL) 00043 { 00044 Planner::declareParam<double>("goal_bias", this, &PDST::setGoalBias, &PDST::getGoalBias, "0.:.05:1."); 00045 } 00046 00047 ompl::control::PDST::~PDST() 00048 { 00049 freeMemory(); 00050 } 00051 00052 ompl::base::PlannerStatus ompl::control::PDST::solve(const base::PlannerTerminationCondition &ptc) 00053 { 00054 // Make sure the planner is configured correctly. 00055 // ompl::base::Planner::checkValidity checks that there is at least one 00056 // start state and an ompl::base::Goal object specified and throws an 00057 // exception if this is not the case. 00058 checkValidity(); 00059 00060 // depending on how the planning problem is set up, this may be necessary 00061 bsp_->bounds_ = projectionEvaluator_->getBounds(); 00062 base::Goal *goal = pdef_->getGoal().get(); 00063 goalSampler_ = dynamic_cast<ompl::base::GoalSampleableRegion*>(goal); 00064 00065 // Ensure that we have a state sampler AND a control sampler 00066 if (!sampler_) 00067 sampler_ = si_->allocStateSampler(); 00068 if (!controlSampler_) 00069 controlSampler_ = siC_->allocDirectedControlSampler(); 00070 00071 // Initialize to correct values depending on whether or not previous calls to solve 00072 // generated an approximate or exact solution. If solve is being called for the first 00073 // time then initializes hasSolution to false and isApproximate to true. 00074 double distanceToGoal, closestDistanceToGoal = std::numeric_limits<double>::infinity(); 00075 bool hasSolution = lastGoalMotion_ != NULL; 00076 bool isApproximate = !hasSolution || !goal->isSatisfied(lastGoalMotion_->endState_, &closestDistanceToGoal); 00077 unsigned int ndim = projectionEvaluator_->getDimension(); 00078 00079 // If an exact solution has already been found, do not run for another iteration. 00080 if (hasSolution && !isApproximate) 00081 return ompl::base::PlannerStatus::EXACT_SOLUTION; 00082 00083 // Initialize tree with start state(s) 00084 while (const base::State *st = pis_.nextStart()) 00085 { 00086 Motion *startMotion = new Motion(si_->cloneState(st)); 00087 bsp_->addMotion(startMotion); 00088 startMotion->heapElement_ = priorityQueue_.insert(startMotion); 00089 } 00090 00091 if (priorityQueue_.empty()) 00092 { 00093 OMPL_ERROR("%s: There are no valid initial states!", getName().c_str()); 00094 return base::PlannerStatus::INVALID_START; 00095 } 00096 00097 OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), priorityQueue_.size()); 00098 00099 base::State *tmpState1 = si_->allocState(), *tmpState2 = si_->allocState(); 00100 base::EuclideanProjection tmpProj1(ndim), tmpProj2(ndim); 00101 while (!ptc) 00102 { 00103 // Get the top priority path. 00104 Motion *motionSelected = priorityQueue_.top()->data; 00105 motionSelected->updatePriority(); 00106 priorityQueue_.update(motionSelected->heapElement_); 00107 00108 Motion *newMotion = propagateFrom(motionSelected, tmpState1, tmpState2); 00109 if (newMotion == NULL) 00110 continue; 00111 00112 addMotion(newMotion, bsp_, tmpState1, tmpState2, tmpProj1, tmpProj2); 00113 00114 // Check if the newMotion reached the goal. 00115 hasSolution = goal->isSatisfied(newMotion->endState_, &distanceToGoal); 00116 if (hasSolution) 00117 { 00118 closestDistanceToGoal = distanceToGoal; 00119 lastGoalMotion_ = newMotion; 00120 isApproximate = false; 00121 break; 00122 } 00123 else if (distanceToGoal < closestDistanceToGoal) 00124 { 00125 closestDistanceToGoal = distanceToGoal; 00126 lastGoalMotion_ = newMotion; 00127 } 00128 00129 // subdivide cell that contained selected motion, put motions of that 00130 // cell in subcells and split motions so that they contained within 00131 // one subcell 00132 Cell *cellSelected = motionSelected->cell_; 00133 std::vector<Motion*> motions; 00134 cellSelected->subdivide(ndim); 00135 motions.swap(cellSelected->motions_); 00136 for (std::vector<Motion*>::iterator m = motions.begin() ; m != motions.end() ; ++m) 00137 addMotion(*m, cellSelected, tmpState1, tmpState2, tmpProj1, tmpProj2); 00138 } 00139 00140 if (lastGoalMotion_ != NULL) 00141 hasSolution = true; 00142 00143 // If a solution path has been computed, save it in the problem definition object. 00144 if (hasSolution) 00145 { 00146 Motion *m; 00147 std::vector<unsigned int> durations(1, 00148 findDurationAndAncestor(lastGoalMotion_, lastGoalMotion_->endState_, tmpState1, m)); 00149 std::vector<Motion*> mpath(1, m); 00150 00151 while (m->parent_) 00152 { 00153 durations.push_back(findDurationAndAncestor(m->parent_, m->startState_, tmpState1, m)); 00154 mpath.push_back(m); 00155 } 00156 00157 PathControl *path = new PathControl(si_); 00158 double dt = siC_->getPropagationStepSize(); 00159 path->append(mpath.back()->endState_); 00160 for (int i = (int) mpath.size() - 2; i > 0; i--) 00161 path->append(mpath[i-1]->startState_, mpath[i]->control_, durations[i] * dt); 00162 path->append(lastGoalMotion_->endState_, mpath[0]->control_, durations[0] * dt); 00163 pdef_->addSolutionPath(base::PathPtr(path), isApproximate, closestDistanceToGoal, getName()); 00164 } 00165 00166 si_->freeState(tmpState1); 00167 si_->freeState(tmpState2); 00168 00169 OMPL_INFORM("%s: Created %u states and %u cells", getName().c_str(), priorityQueue_.size(), bsp_->size()); 00170 00171 return base::PlannerStatus(hasSolution, isApproximate); 00172 } 00173 00174 ompl::control::PDST::Motion* ompl::control::PDST::propagateFrom( 00175 Motion *motion, base::State *start, base::State *rnd) 00176 { 00177 // sample a point along the trajectory given by motion 00178 unsigned int prevDuration = motion->controlDuration_; 00179 if (motion->controlDuration_ > 1) 00180 prevDuration = rng_.uniformInt(1, motion->controlDuration_); 00181 if (prevDuration == motion->controlDuration_) 00182 start = motion->endState_; 00183 else 00184 siC_->propagate(motion->startState_, motion->control_, prevDuration, start); 00185 // generate a random state 00186 if (goalSampler_ && rng_.uniform01() < goalBias_ && goalSampler_->canSample()) 00187 goalSampler_->sampleGoal(rnd); 00188 else 00189 sampler_->sampleUniform(rnd); 00190 // generate a random control 00191 Control *control = siC_->allocControl(); 00192 unsigned int duration = controlSampler_->sampleTo(control, motion->control_, start, rnd); 00193 // return new motion if duration is large enough 00194 if (duration < siC_->getMinControlDuration()) 00195 { 00196 siC_->freeControl(control); 00197 return NULL; 00198 } 00199 return new Motion(si_->cloneState(start), si_->cloneState(rnd), 00200 control, duration, ++iteration_, motion); 00201 } 00202 00203 void ompl::control::PDST::addMotion(Motion *motion, Cell *bsp, base::State *prevState, base::State *state, 00204 base::EuclideanProjection& prevProj, base::EuclideanProjection& proj) 00205 { 00206 // If the motion is at most 1 step, then it cannot be split across cell bounds. 00207 if (motion->controlDuration_ <= 1) 00208 { 00209 projectionEvaluator_->project(motion->endState_, proj); 00210 bsp->stab(proj)->addMotion(motion); 00211 updateHeapElement(motion); 00212 return; 00213 } 00214 00215 Cell *cell = NULL, *prevCell = NULL; 00216 si_->copyState(prevState, motion->startState_); 00217 // propagate the motion, check for cell boundary crossings, and split as necessary 00218 for (unsigned int i = 0, duration = 0 ; i < motion->controlDuration_ - 1 ; ++i, ++duration) 00219 { 00220 siC_->propagate(prevState, motion->control_, 1, state); 00221 projectionEvaluator_->project(state, proj); 00222 cell = bsp->stab(proj); 00223 if (duration > 0 && cell != prevCell) 00224 { 00225 Motion *newMotion = new Motion(motion->startState_, si_->cloneState(prevState), 00226 motion->control_, duration, motion->priority_, motion->parent_); 00227 newMotion->isSplit_ = true; 00228 prevCell->addMotion(newMotion); 00229 updateHeapElement(newMotion); 00230 motion->startState_ = newMotion->endState_; 00231 motion->controlDuration_ -= duration; 00232 motion->parent_ = newMotion; 00233 duration = 0; 00234 } 00235 std::swap(state, prevState); 00236 std::swap(cell, prevCell); 00237 proj.swap(prevProj); 00238 } 00239 prevCell->addMotion(motion); 00240 updateHeapElement(motion); 00241 } 00242 00243 00244 unsigned int ompl::control::PDST::findDurationAndAncestor(Motion *motion, base::State *state, 00245 base::State *scratch, Motion*& ancestor) const 00246 { 00247 const double eps = std::numeric_limits<float>::epsilon(); 00248 unsigned int duration; 00249 ancestor = motion; 00250 if (state == motion->endState_ || motion->controlDuration_ == 0 || 00251 si_->distance(motion->endState_, state) < eps) 00252 duration = motion->controlDuration_; 00253 else if (motion->controlDuration_ > 0 && 00254 si_->distance(motion->startState_, state) < eps) 00255 duration = 0; 00256 else 00257 { 00258 duration = 1; 00259 si_->copyState(scratch, motion->startState_); 00260 for (; duration <= motion->controlDuration_; ++duration) 00261 { 00262 siC_->propagate(scratch, motion->control_, 1, scratch); 00263 if (si_->distance(scratch, state) < eps) 00264 break; 00265 } 00266 } 00267 if (duration <= motion->controlDuration_) 00268 { 00269 // ancestor points to the start of a split motion; duration is computed 00270 // relative to start state of ancestor motion 00271 while (ancestor->parent_ && ancestor->control_ == ancestor->parent_->control_) 00272 { 00273 ancestor = ancestor->parent_; 00274 duration += ancestor->controlDuration_; 00275 } 00276 return duration; 00277 } 00278 // motion is no longer the parent of the motion starting at 00279 // state because it has been split afterwards, so look for 00280 // the starting point in the parent of motion. 00281 return findDurationAndAncestor(motion->parent_, state, scratch, ancestor); 00282 } 00283 00284 void ompl::control::PDST::clear() 00285 { 00286 Planner::clear(); 00287 sampler_.reset(); 00288 controlSampler_.reset(); 00289 iteration_ = 1; 00290 lastGoalMotion_ = NULL; 00291 freeMemory(); 00292 bsp_ = new Cell(1., projectionEvaluator_->getBounds(), 0); 00293 } 00294 00295 void ompl::control::PDST::freeMemory() 00296 { 00297 // Iterate over the elements in the priority queue and clear it 00298 std::vector<Motion*> motions; 00299 motions.reserve(priorityQueue_.size()); 00300 priorityQueue_.getContent(motions); 00301 for (std::vector<Motion*>::iterator it = motions.begin() ; it < motions.end() ; ++it) 00302 { 00303 if ((*it)->startState_ != (*it)->endState_) 00304 si_->freeState((*it)->startState_); 00305 if (!(*it)->isSplit_) 00306 { 00307 si_->freeState((*it)->endState_); 00308 if ((*it)->control_) 00309 siC_->freeControl((*it)->control_); 00310 } 00311 delete *it; 00312 } 00313 priorityQueue_.clear(); // clears the Element objects in the priority queue 00314 delete bsp_; 00315 bsp_ = NULL; 00316 } 00317 00318 void ompl::control::PDST::setup() 00319 { 00320 Planner::setup(); 00321 tools::SelfConfig sc(si_, getName()); 00322 sc.configureProjectionEvaluator(projectionEvaluator_); 00323 if (!projectionEvaluator_->hasBounds()) 00324 projectionEvaluator_->inferBounds(); 00325 if (!projectionEvaluator_->hasBounds()) 00326 throw Exception("PDST requires a projection evaluator that specifies bounds for the projected space"); 00327 if (bsp_) 00328 delete bsp_; 00329 bsp_ = new Cell(1., projectionEvaluator_->getBounds(), 0); 00330 lastGoalMotion_ = NULL; 00331 } 00332 00333 void ompl::control::PDST::getPlannerData(ompl::base::PlannerData &data) const 00334 { 00335 base::Planner::getPlannerData(data); 00336 00337 double dt = siC_->getPropagationStepSize(); 00338 base::State *scratch = si_->allocState(); 00339 std::vector<Motion*> motions; 00340 motions.reserve(priorityQueue_.size()); 00341 priorityQueue_.getContent(motions); 00342 00343 // Add goal vertex 00344 if (lastGoalMotion_ != NULL) 00345 data.addGoalVertex(lastGoalMotion_->endState_); 00346 00347 for (std::vector<Motion*>::iterator it = motions.begin(); it < motions.end(); ++it) 00348 if (!(*it)->isSplit_) 00349 { 00350 // We only add one edge for each motion that has been split into smaller segments 00351 Motion *cur = *it, *ancestor; 00352 unsigned int duration = findDurationAndAncestor(cur, cur->endState_, scratch, ancestor); 00353 00354 if (cur->parent_ == NULL) 00355 data.addStartVertex(base::PlannerDataVertex(cur->endState_)); 00356 else if (data.hasControls()) 00357 { 00358 data.addEdge(base::PlannerDataVertex(ancestor->startState_), 00359 base::PlannerDataVertex(cur->endState_), 00360 PlannerDataEdgeControl(cur->control_, duration * dt)); 00361 if (ancestor->parent_) 00362 { 00363 // Include an edge between start state of parent ancestor motion and 00364 // the start state of the ancestor motion, which lies somewhere on 00365 // the parent ancestor motion. 00366 cur = ancestor; 00367 duration = findDurationAndAncestor(cur->parent_, cur->startState_, scratch, ancestor); 00368 data.addEdge(base::PlannerDataVertex(ancestor->startState_), 00369 base::PlannerDataVertex(cur->startState_), 00370 PlannerDataEdgeControl(ancestor->control_, duration * dt)); 00371 } 00372 } 00373 else 00374 { 00375 data.addEdge(base::PlannerDataVertex(ancestor->startState_), 00376 base::PlannerDataVertex(cur->endState_)); 00377 if (ancestor->parent_) 00378 { 00379 // Include an edge between start state of parent ancestor motion and 00380 // the start state of the ancestor motion, which lies somewhere on 00381 // the parent ancestor motion. 00382 cur = ancestor; 00383 findDurationAndAncestor(cur->parent_, cur->startState_, scratch, ancestor); 00384 data.addEdge(base::PlannerDataVertex(ancestor->startState_), 00385 base::PlannerDataVertex(cur->startState_)); 00386 } 00387 } 00388 } 00389 00390 si_->freeState(scratch); 00391 } 00392 00393 void ompl::control::PDST::Cell::subdivide(unsigned int spaceDimension) 00394 { 00395 double childVolume = .5 * volume_; 00396 unsigned int nextSplitDimension = (splitDimension_ + 1) % spaceDimension; 00397 splitValue_ = .5 * (bounds_.low[splitDimension_] + bounds_.high[splitDimension_]); 00398 00399 left_ = new Cell(childVolume, bounds_, nextSplitDimension); 00400 left_->bounds_.high[splitDimension_] = splitValue_; 00401 left_->motions_.reserve(motions_.size()); 00402 right_ = new Cell(childVolume, bounds_, nextSplitDimension); 00403 right_->bounds_.low[splitDimension_] = splitValue_; 00404 right_->motions_.reserve(motions_.size()); 00405 }