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 ®ion, 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 }