All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
pRRT.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/rrt/pRRT.h"
00038 #include "ompl/datastructures/NearestNeighborsSqrtApprox.h"
00039 #include "ompl/base/GoalSampleableRegion.h"
00040 #include <boost/thread/thread.hpp>
00041 #include <limits>
00042 
00043 void ompl::geometric::pRRT::setup(void)
00044 {
00045     Planner::setup();
00046     checkMotionLength(this, maxDistance_);
00047 
00048     if (!nn_)
00049         nn_.reset(new NearestNeighborsSqrtApprox<Motion*>());
00050     nn_->setDistanceFunction(boost::bind(&pRRT::distanceFunction, this, _1, _2));
00051 }
00052 
00053 void ompl::geometric::pRRT::clear(void)
00054 {
00055     Planner::clear();
00056     samplerArray_.clear();
00057     freeMemory();
00058     if (nn_)
00059         nn_->clear();
00060 }
00061 
00062 void ompl::geometric::pRRT::freeMemory(void)
00063 {
00064     if (nn_)
00065     {
00066         std::vector<Motion*> motions;
00067         nn_->list(motions);
00068         for (unsigned int i = 0 ; i < motions.size() ; ++i)
00069         {
00070             if (motions[i]->state)
00071                 si_->freeState(motions[i]->state);
00072             delete motions[i];
00073         }
00074     }
00075 }
00076 
00077 void ompl::geometric::pRRT::threadSolve(unsigned int tid, const base::PlannerTerminationCondition &ptc, SolutionInfo *sol)
00078 {
00079     checkValidity();
00080     base::Goal                 *goal   = pdef_->getGoal().get();
00081     base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal);
00082     RNG                         rng;
00083 
00084     Motion *rmotion   = new Motion(si_);
00085     base::State *rstate = rmotion->state;
00086     base::State *xstate = si_->allocState();
00087 
00088     while (sol->solution == NULL && ptc() == false)
00089     {
00090         /* sample random state (with goal biasing) */
00091         if (goal_s && rng.uniform01() < goalBias_ && goal_s->canSample())
00092             goal_s->sampleGoal(rstate);
00093         else
00094             samplerArray_[tid]->sampleUniform(rstate);
00095 
00096         /* find closest state in the tree */
00097         nnLock_.lock();
00098         Motion *nmotion = nn_->nearest(rmotion);
00099         nnLock_.unlock();
00100         base::State *dstate = rstate;
00101 
00102         /* find state to add */
00103         double d = si_->distance(nmotion->state, rstate);
00104         if (d > maxDistance_)
00105         {
00106             si_->getStateManifold()->interpolate(nmotion->state, rstate, maxDistance_ / d, xstate);
00107             dstate = xstate;
00108         }
00109 
00110         if (si_->checkMotion(nmotion->state, dstate))
00111         {
00112             /* create a motion */
00113             Motion *motion = new Motion(si_);
00114             si_->copyState(motion->state, dstate);
00115             motion->parent = nmotion;
00116 
00117             nnLock_.lock();
00118             nn_->add(motion);
00119             nnLock_.unlock();
00120 
00121             double dist = 0.0;
00122             bool solved = goal->isSatisfied(motion->state, &dist);
00123             if (solved)
00124             {
00125                 sol->lock.lock();
00126                 sol->approxdif = dist;
00127                 sol->solution = motion;
00128                 sol->lock.unlock();
00129                 break;
00130             }
00131             if (dist < sol->approxdif)
00132             {
00133                 sol->lock.lock();
00134                 if (dist < sol->approxdif)
00135                 {
00136                     sol->approxdif = dist;
00137                     sol->approxsol = motion;
00138                 }
00139                 sol->lock.unlock();
00140             }
00141         }
00142     }
00143 
00144     si_->freeState(xstate);
00145     if (rmotion->state)
00146         si_->freeState(rmotion->state);
00147     delete rmotion;
00148 }
00149 
00150 bool ompl::geometric::pRRT::solve(const base::PlannerTerminationCondition &ptc)
00151 {
00152     base::GoalRegion *goal = dynamic_cast<base::GoalRegion*>(pdef_->getGoal().get());
00153 
00154     if (!goal)
00155     {
00156         msg_.error("Goal undefined");
00157         return false;
00158     }
00159 
00160     samplerArray_.resize(threadCount_);
00161 
00162     while (const base::State *st = pis_.nextStart())
00163     {
00164         Motion *motion = new Motion(si_);
00165         si_->copyState(motion->state, st);
00166         nn_->add(motion);
00167     }
00168 
00169     if (nn_->size() == 0)
00170     {
00171         msg_.error("There are no valid initial states!");
00172         return false;
00173     }
00174 
00175     msg_.inform("Starting with %u states", nn_->size());
00176 
00177     SolutionInfo sol;
00178     sol.solution = NULL;
00179     sol.approxsol = NULL;
00180     sol.approxdif = std::numeric_limits<double>::infinity();
00181 
00182     std::vector<boost::thread*> th(threadCount_);
00183     for (unsigned int i = 0 ; i < threadCount_ ; ++i)
00184         th[i] = new boost::thread(boost::bind(&pRRT::threadSolve, this, i, ptc, &sol));
00185     for (unsigned int i = 0 ; i < threadCount_ ; ++i)
00186     {
00187         th[i]->join();
00188         delete th[i];
00189     }
00190 
00191     bool approximate = false;
00192     if (sol.solution == NULL)
00193     {
00194         sol.solution = sol.approxsol;
00195         approximate = true;
00196     }
00197 
00198     if (sol.solution != NULL)
00199     {
00200         /* construct the solution path */
00201         std::vector<Motion*> mpath;
00202         while (sol.solution != NULL)
00203         {
00204             mpath.push_back(sol.solution);
00205             sol.solution = sol.solution->parent;
00206         }
00207 
00208         /* set the solution path */
00209         PathGeometric *path = new PathGeometric(si_);
00210            for (int i = mpath.size() - 1 ; i >= 0 ; --i)
00211             path->states.push_back(si_->cloneState(mpath[i]->state));
00212 
00213         goal->setDifference(sol.approxdif);
00214         goal->setSolutionPath(base::PathPtr(path), approximate);
00215 
00216         if (approximate)
00217             msg_.warn("Found approximate solution");
00218     }
00219 
00220     msg_.inform("Created %u states", nn_->size());
00221 
00222     return goal->isAchieved();
00223 }
00224 
00225 void ompl::geometric::pRRT::getPlannerData(base::PlannerData &data) const
00226 {
00227     Planner::getPlannerData(data);
00228 
00229     std::vector<Motion*> motions;
00230     if (nn_)
00231         nn_->list(motions);
00232 
00233     for (unsigned int i = 0 ; i < motions.size() ; ++i)
00234         data.recordEdge(motions[i]->parent ? motions[i]->parent->state : NULL, motions[i]->state);
00235 }
00236 
00237 void ompl::geometric::pRRT::setThreadCount(unsigned int nthreads)
00238 {
00239     assert(nthreads > 0);
00240     threadCount_ = nthreads;
00241 }
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator