ompl/control/planners/syclop/src/SyclopEST.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/SyclopEST.h" 00038 #include "ompl/base/goals/GoalSampleableRegion.h" 00039 00040 void ompl::control::SyclopEST::setup() 00041 { 00042 Syclop::setup(); 00043 sampler_ = si_->allocStateSampler(); 00044 controlSampler_ = siC_->allocControlSampler(); 00045 lastGoalMotion_ = NULL; 00046 } 00047 00048 void ompl::control::SyclopEST::clear() 00049 { 00050 Syclop::clear(); 00051 freeMemory(); 00052 motions_.clear(); 00053 lastGoalMotion_ = NULL; 00054 } 00055 00056 void ompl::control::SyclopEST::getPlannerData(base::PlannerData &data) const 00057 { 00058 Planner::getPlannerData(data); 00059 00060 double delta = siC_->getPropagationStepSize(); 00061 00062 if (lastGoalMotion_) 00063 data.addGoalVertex(lastGoalMotion_->state); 00064 00065 for (size_t i = 0; i < motions_.size(); ++i) 00066 { 00067 if (motions_[i]->parent) 00068 { 00069 if (data.hasControls()) 00070 data.addEdge (base::PlannerDataVertex(motions_[i]->parent->state), 00071 base::PlannerDataVertex(motions_[i]->state), 00072 control::PlannerDataEdgeControl (motions_[i]->control, motions_[i]->steps * delta)); 00073 else 00074 data.addEdge (base::PlannerDataVertex(motions_[i]->parent->state), 00075 base::PlannerDataVertex(motions_[i]->state)); 00076 } 00077 else 00078 data.addStartVertex (base::PlannerDataVertex(motions_[i]->state)); 00079 } 00080 } 00081 00082 ompl::control::Syclop::Motion* ompl::control::SyclopEST::addRoot(const base::State *s) 00083 { 00084 Motion *motion = new Motion(siC_); 00085 si_->copyState(motion->state, s); 00086 siC_->nullControl(motion->control); 00087 motions_.push_back(motion); 00088 return motion; 00089 } 00090 00091 void ompl::control::SyclopEST::selectAndExtend(Region ®ion, std::vector<Motion*>& newMotions) 00092 { 00093 Motion *treeMotion = region.motions[rng_.uniformInt(0, region.motions.size()-1)]; 00094 Control *rctrl = siC_->allocControl(); 00095 base::State *newState = si_->allocState(); 00096 00097 controlSampler_->sample(rctrl, treeMotion->state); 00098 unsigned int duration = controlSampler_->sampleStepCount(siC_->getMinControlDuration(), siC_->getMaxControlDuration()); 00099 duration = siC_->propagateWhileValid(treeMotion->state, rctrl, duration, newState); 00100 00101 if (duration >= siC_->getMinControlDuration()) 00102 { 00103 Motion *motion = new Motion(siC_); 00104 si_->copyState(motion->state, newState); 00105 siC_->copyControl(motion->control, rctrl); 00106 motion->steps = duration; 00107 motion->parent = treeMotion; 00108 motions_.push_back(motion); 00109 newMotions.push_back(motion); 00110 00111 lastGoalMotion_ = motion; 00112 } 00113 00114 siC_->freeControl(rctrl); 00115 si_->freeState(newState); 00116 } 00117 00118 void ompl::control::SyclopEST::freeMemory() 00119 { 00120 for (std::vector<Motion*>::iterator i = motions_.begin(); i != motions_.end(); ++i) 00121 { 00122 Motion *m = *i; 00123 if (m->state) 00124 si_->freeState(m->state); 00125 if (m->control) 00126 siC_->freeControl(m->control); 00127 delete m; 00128 } 00129 }