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