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/tools/config/SelfConfig.h"
00040 
00041 void ompl::control::SyclopRRT::setup()
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(tools::SelfConfig::getDefaultNearestNeighbors<Motion*>(si_->getStateSpace()));
00053         nn_->setDistanceFunction(boost::bind(&SyclopRRT::distanceFunction, this, _1, _2));
00054     }
00055 }
00056 
00057 void ompl::control::SyclopRRT::clear()
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     std::vector<double> coord(decomp_->getDimension());
00110     decomp_->sampleFromRegion(region.index, rng_, coord);
00111     decomp_->sampleFullState(sampler, coord, rmotion->state);
00112 
00113     Motion *nmotion;
00114     if (regionalNN_)
00115     {
00116         /* Instead of querying the nearest neighbors datastructure over the entire tree of motions,
00117          * here we perform a linear search over all motions in the selected region and its neighbors. */
00118         std::vector<int> searchRegions;
00119         decomp_->getNeighbors(region.index, searchRegions);
00120         searchRegions.push_back(region.index);
00121 
00122         std::vector<Motion*> motions;
00123         for (std::vector<int>::const_iterator i = searchRegions.begin(); i != searchRegions.end(); ++i)
00124         {
00125             const std::vector<Motion*>& regionMotions = getRegionFromIndex(*i).motions;
00126             motions.insert(motions.end(), regionMotions.begin(), regionMotions.end());
00127         }
00128 
00129         std::vector<Motion*>::const_iterator i = motions.begin();
00130         nmotion = *i;
00131         double minDistance = distanceFunction(rmotion, nmotion);
00132         ++i;
00133         while (i != motions.end())
00134         {
00135             Motion *m = *i;
00136             const double dist = distanceFunction(rmotion, m);
00137             if (dist < minDistance)
00138             {
00139                 nmotion = m;
00140                 minDistance = dist;
00141             }
00142             ++i;
00143         }
00144     }
00145     else
00146     {
00147         assert (nn_);
00148         nmotion = nn_->nearest(rmotion);
00149     }
00150 
00151     unsigned int duration = controlSampler_->sampleTo(rmotion->control, nmotion->control, nmotion->state, rmotion->state);
00152     if (duration >= siC_->getMinControlDuration())
00153     {
00154         rmotion->steps = duration;
00155         rmotion->parent = nmotion;
00156         newMotions.push_back(rmotion);
00157         if (nn_)
00158             nn_->add(rmotion);
00159         lastGoalMotion_ = rmotion;
00160     }
00161     else
00162     {
00163         si_->freeState(rmotion->state);
00164         siC_->freeControl(rmotion->control);
00165         delete rmotion;
00166     }
00167 }
00168 
00169 void ompl::control::SyclopRRT::freeMemory()
00170 {
00171     if (nn_)
00172     {
00173         std::vector<Motion*> motions;
00174         nn_->list(motions);
00175         for (std::vector<Motion*>::iterator i = motions.begin(); i != motions.end(); ++i)
00176         {
00177             Motion *m = *i;
00178             if (m->state)
00179                 si_->freeState(m->state);
00180             if (m->control)
00181                 siC_->freeControl(m->control);
00182             delete m;
00183         }
00184     }
00185 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines