ompl/control/planners/est/src/EST.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: Ryan Luna */ 00036 00037 #include "ompl/control/planners/est/EST.h" 00038 #include "ompl/base/goals/GoalSampleableRegion.h" 00039 #include "ompl/tools/config/SelfConfig.h" 00040 #include <limits> 00041 #include <cassert> 00042 00043 ompl::control::EST::EST(const SpaceInformationPtr &si) : base::Planner(si, "EST") 00044 { 00045 specs_.approximateSolutions = true; 00046 goalBias_ = 0.05; 00047 maxDistance_ = 0.0; 00048 siC_ = si.get(); 00049 lastGoalMotion_ = NULL; 00050 00051 Planner::declareParam<double>("range", this, &EST::setRange, &EST::getRange, "0.:1.:10000."); 00052 Planner::declareParam<double>("goal_bias", this, &EST::setGoalBias, &EST::getGoalBias, "0.:.05:1."); 00053 } 00054 00055 ompl::control::EST::~EST() 00056 { 00057 freeMemory(); 00058 } 00059 00060 void ompl::control::EST::setup() 00061 { 00062 Planner::setup(); 00063 tools::SelfConfig sc(si_, getName()); 00064 sc.configureProjectionEvaluator(projectionEvaluator_); 00065 sc.configurePlannerRange(maxDistance_); 00066 00067 tree_.grid.setDimension(projectionEvaluator_->getDimension()); 00068 } 00069 00070 void ompl::control::EST::clear() 00071 { 00072 Planner::clear(); 00073 sampler_.reset(); 00074 controlSampler_.reset(); 00075 freeMemory(); 00076 tree_.grid.clear(); 00077 tree_.size = 0; 00078 pdf_.clear (); 00079 lastGoalMotion_ = NULL; 00080 } 00081 00082 void ompl::control::EST::freeMemory() 00083 { 00084 for (Grid<MotionInfo>::iterator it = tree_.grid.begin(); it != tree_.grid.end() ; ++it) 00085 { 00086 for (unsigned int i = 0 ; i < it->second->data.size() ; ++i) 00087 { 00088 if (it->second->data[i]->state) 00089 si_->freeState(it->second->data[i]->state); 00090 if (it->second->data[i]->control) 00091 siC_->freeControl(it->second->data[i]->control); 00092 delete it->second->data[i]; 00093 } 00094 } 00095 } 00096 00097 ompl::base::PlannerStatus ompl::control::EST::solve(const base::PlannerTerminationCondition &ptc) 00098 { 00099 checkValidity(); 00100 base::Goal *goal = pdef_->getGoal().get(); 00101 base::GoalSampleableRegion *goal_s = dynamic_cast<base::GoalSampleableRegion*>(goal); 00102 00103 // Initializing tree with start state(s) 00104 while (const base::State *st = pis_.nextStart()) 00105 { 00106 Motion *motion = new Motion(siC_); 00107 si_->copyState(motion->state, st); 00108 siC_->nullControl(motion->control); 00109 addMotion(motion); 00110 } 00111 00112 if (tree_.grid.size() == 0) 00113 { 00114 OMPL_ERROR("%s: There are no valid initial states!", getName().c_str()); 00115 return base::PlannerStatus::INVALID_START; 00116 } 00117 00118 // Ensure that we have a state sampler AND a control sampler 00119 if (!sampler_) 00120 sampler_ = si_->allocValidStateSampler(); 00121 if (!controlSampler_) 00122 controlSampler_ = siC_->allocDirectedControlSampler(); 00123 00124 OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), tree_.size); 00125 00126 Motion *solution = NULL; 00127 Motion *approxsol = NULL; 00128 double approxdif = std::numeric_limits<double>::infinity(); 00129 Motion *rmotion = new Motion(siC_); 00130 bool solved = false; 00131 00132 while (!ptc) 00133 { 00134 // Select a state to expand the tree from 00135 Motion *existing = selectMotion(); 00136 assert (existing); 00137 00138 // sample a random state (with goal biasing) near the state selected for expansion 00139 if (goal_s && rng_.uniform01() < goalBias_ && goal_s->canSample()) 00140 goal_s->sampleGoal(rmotion->state); 00141 else 00142 { 00143 if (!sampler_->sampleNear(rmotion->state, existing->state, maxDistance_)) 00144 continue; 00145 } 00146 00147 // Extend a motion toward the state we just sampled 00148 unsigned int duration = controlSampler_->sampleTo(rmotion->control, existing->control, 00149 existing->state, rmotion->state); 00150 00151 // If the system was propagated for a meaningful amount of time, save into the tree 00152 if (duration >= siC_->getMinControlDuration()) 00153 { 00154 // create a motion to the resulting state 00155 Motion *motion = new Motion(siC_); 00156 si_->copyState(motion->state, rmotion->state); 00157 siC_->copyControl(motion->control, rmotion->control); 00158 motion->steps = duration; 00159 motion->parent = existing; 00160 00161 // save the state 00162 addMotion(motion); 00163 00164 // Check if this state is the goal state, or improves the best solution so far 00165 double dist = 0.0; 00166 solved = goal->isSatisfied(motion->state, &dist); 00167 if (solved) 00168 { 00169 approxdif = dist; 00170 solution = motion; 00171 break; 00172 } 00173 if (dist < approxdif) 00174 { 00175 approxdif = dist; 00176 approxsol = motion; 00177 } 00178 } 00179 } 00180 00181 bool approximate = false; 00182 if (solution == NULL) 00183 { 00184 solution = approxsol; 00185 approximate = true; 00186 } 00187 00188 // Constructing the solution path 00189 if (solution != NULL) 00190 { 00191 lastGoalMotion_ = solution; 00192 00193 std::vector<Motion*> mpath; 00194 while (solution != NULL) 00195 { 00196 mpath.push_back(solution); 00197 solution = solution->parent; 00198 } 00199 00200 PathControl *path = new PathControl(si_); 00201 for (int i = mpath.size() - 1 ; i >= 0 ; --i) 00202 if (mpath[i]->parent) 00203 path->append(mpath[i]->state, mpath[i]->control, mpath[i]->steps * siC_->getPropagationStepSize()); 00204 else 00205 path->append(mpath[i]->state); 00206 solved = true; 00207 pdef_->addSolutionPath(base::PathPtr(path), approximate, approxdif, getName()); 00208 } 00209 00210 // Cleaning up memory 00211 if (rmotion->state) 00212 si_->freeState(rmotion->state); 00213 if (rmotion->control) 00214 siC_->freeControl(rmotion->control); 00215 delete rmotion; 00216 00217 OMPL_INFORM("%s: Created %u states in %u cells", getName().c_str(), tree_.size, tree_.grid.size()); 00218 00219 return base::PlannerStatus(solved, approximate); 00220 } 00221 00222 ompl::control::EST::Motion* ompl::control::EST::selectMotion() 00223 { 00224 GridCell* cell = pdf_.sample(rng_.uniform01()); 00225 return cell && !cell->data.empty() ? cell->data[rng_.uniformInt(0, cell->data.size() - 1)] : NULL; 00226 } 00227 00228 void ompl::control::EST::addMotion(Motion *motion) 00229 { 00230 Grid<MotionInfo>::Coord coord; 00231 projectionEvaluator_->computeCoordinates(motion->state, coord); 00232 GridCell* cell = tree_.grid.getCell(coord); 00233 if (cell) 00234 { 00235 cell->data.push_back(motion); 00236 pdf_.update(cell->data.elem_, 1.0/cell->data.size()); 00237 } 00238 else 00239 { 00240 cell = tree_.grid.createCell(coord); 00241 cell->data.push_back(motion); 00242 tree_.grid.add(cell); 00243 cell->data.elem_ = pdf_.add(cell, 1.0); 00244 } 00245 tree_.size++; 00246 } 00247 00248 void ompl::control::EST::getPlannerData(base::PlannerData &data) const 00249 { 00250 Planner::getPlannerData(data); 00251 00252 std::vector<MotionInfo> motions; 00253 tree_.grid.getContent(motions); 00254 00255 double stepSize = siC_->getPropagationStepSize(); 00256 00257 if (lastGoalMotion_) 00258 data.addGoalVertex(base::PlannerDataVertex(lastGoalMotion_->state)); 00259 00260 for (unsigned int i = 0 ; i < motions.size() ; ++i) 00261 for (unsigned int j = 0 ; j < motions[i].size() ; ++j) 00262 { 00263 if (motions[i][j]->parent) 00264 { 00265 if (data.hasControls()) 00266 data.addEdge (base::PlannerDataVertex (motions[i][j]->parent->state), 00267 base::PlannerDataVertex (motions[i][j]->state), 00268 PlannerDataEdgeControl(motions[i][j]->control, motions[i][j]->steps * stepSize)); 00269 else 00270 data.addEdge (base::PlannerDataVertex (motions[i][j]->parent->state), 00271 base::PlannerDataVertex (motions[i][j]->state)); 00272 } 00273 else 00274 data.addStartVertex (base::PlannerDataVertex (motions[i][j]->state)); 00275 } 00276 }