All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
SyclopRRT.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: Matt Maly */
00036 
00037 #include "ompl/control/planners/syclop/SyclopRRT.h"
00038 #include "ompl/base/GoalSampleableRegion.h"
00039 #include "ompl/datastructures/NearestNeighborsGNAT.h"
00040 
00041 void ompl::control::SyclopRRT::setup(void)
00042 {
00043     Syclop::setup();
00044     sampler_ = si_->allocStateSampler();
00045     controlSampler_ = siC_->allocDirectedControlSampler();
00046 
00047     // Create a default GNAT nearest neighbors structure if the user doesn't want
00048     // the default regionalNN check from the discretization
00049     if (!nn_ && !regionalNN_)
00050     {
00051         nn_.reset(new NearestNeighborsGNAT<Motion*>());
00052         nn_->setDistanceFunction(boost::bind(&SyclopRRT::distanceFunction, this, _1, _2));
00053     }
00054 }
00055 
00056 void ompl::control::SyclopRRT::clear(void)
00057 {
00058     Syclop::clear();
00059     freeMemory();
00060     if (nn_)
00061         nn_->clear();
00062 }
00063 
00064 void ompl::control::SyclopRRT::getPlannerData(base::PlannerData& data) const
00065 {
00066     Planner::getPlannerData(data);
00067     std::vector<Motion*> motions;
00068     if (nn_)
00069         nn_->list(motions);
00070     if (PlannerData *cpd = dynamic_cast<control::PlannerData*>(&data))
00071     {
00072         const double delta = siC_->getPropagationStepSize();
00073 
00074         for (std::vector<Motion*>::const_iterator i = motions.begin(); i != motions.end(); ++i)
00075         {
00076             const Motion* m = *i;
00077             if (m->parent)
00078                 cpd->recordEdge(m->parent->state, m->state, m->control, m->steps * delta);
00079             else
00080                 cpd->recordEdge(NULL, m->state, NULL, 0.);
00081         }
00082     }
00083     else
00084     {
00085         for (std::vector<Motion*>::const_iterator i = motions.begin(); i != motions.end(); ++i)
00086         {
00087             const Motion* m = *i;
00088             data.recordEdge(m->parent ? m->parent->state : NULL, m->state);
00089         }
00090     }
00091 }
00092 
00093 ompl::control::Syclop::Motion* ompl::control::SyclopRRT::addRoot(const base::State* s)
00094 {
00095     Motion* motion = new Motion(siC_);
00096     si_->copyState(motion->state, s);
00097     siC_->nullControl(motion->control);
00098 
00099     if (nn_)
00100         nn_->add(motion);
00101     return motion;
00102 }
00103 
00104 void ompl::control::SyclopRRT::selectAndExtend(Region& region, std::vector<Motion*>& newMotions)
00105 {
00106     Motion* rmotion = new Motion(siC_);
00107     base::StateSamplerPtr sampler(si_->allocStateSampler());
00108     decomp_->sampleFromRegion(region.index, sampler, rmotion->state);
00109 
00110     Motion* nmotion;
00111     if (regionalNN_)
00112     {
00113         /* Instead of querying the nearest neighbors datastructure over the entire tree of motions,
00114          * here we perform a linear search over all motions in the selected region and its neighbors. */
00115         std::vector<int> searchRegions;
00116         decomp_->getNeighbors(region.index, searchRegions);
00117         searchRegions.push_back(region.index);
00118 
00119         std::vector<Motion*> motions;
00120         for (std::vector<int>::const_iterator i = searchRegions.begin(); i != searchRegions.end(); ++i)
00121         {
00122             const std::vector<Motion*>& regionMotions = getRegionFromIndex(*i).motions;
00123             motions.insert(motions.end(), regionMotions.begin(), regionMotions.end());
00124         }
00125 
00126         std::vector<Motion*>::const_iterator i = motions.begin();
00127         nmotion = *i;
00128         double minDistance = distanceFunction(rmotion, nmotion);
00129         ++i;
00130         while (i != motions.end())
00131         {
00132             Motion* m = *i;
00133             const double dist = distanceFunction(rmotion, m);
00134             if (dist < minDistance)
00135             {
00136                 nmotion = m;
00137                 minDistance = dist;
00138             }
00139             ++i;
00140         }
00141     }
00142     else
00143     {
00144         assert (nn_);
00145         nmotion = nn_->nearest(rmotion);
00146     }
00147 
00148     base::State* newState = si_->allocState();
00149 
00150     unsigned int duration = controlSampler_->sampleTo(rmotion->control, nmotion->control, nmotion->state, rmotion->state);
00151 
00152     duration = siC_->propagateWhileValid(nmotion->state, rmotion->control, duration, newState);
00153 
00154     if (duration >= siC_->getMinControlDuration())
00155     {
00156         Motion* motion = new Motion(siC_);
00157         si_->copyState(motion->state, newState);
00158         siC_->copyControl(motion->control, rmotion->control);
00159         motion->steps = duration;
00160         motion->parent = nmotion;
00161         newMotions.push_back(motion);
00162         if (nn_)
00163             nn_->add(motion);
00164     }
00165 
00166     si_->freeState(rmotion->state);
00167     siC_->freeControl(rmotion->control);
00168     delete rmotion;
00169     si_->freeState(newState);
00170 }
00171 
00172 void ompl::control::SyclopRRT::freeMemory(void)
00173 {
00174     if (nn_)
00175     {
00176         std::vector<Motion*> motions;
00177         nn_->list(motions);
00178         for (std::vector<Motion*>::iterator i = motions.begin(); i != motions.end(); ++i)
00179         {
00180             Motion* m = *i;
00181             if (m->state)
00182                 si_->freeState(m->state);
00183             if (m->control)
00184                 siC_->freeControl(m->control);
00185             delete m;
00186         }
00187     }
00188 }
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends