ompl/base/src/OptimizationObjective.cpp
00001 /********************************************************************* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Copyright (c) 2008, Willow Garage, Inc. 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 Willow Garage 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: Luis G. Torres, Ioan Sucan */ 00036 00037 #include "ompl/base/OptimizationObjective.h" 00038 #include "ompl/tools/config/MagicConstants.h" 00039 #include "ompl/base/goals/GoalRegion.h" 00040 #include <limits> 00041 00042 ompl::base::OptimizationObjective::OptimizationObjective(const SpaceInformationPtr &si) : 00043 si_(si), 00044 threshold_(0.0) 00045 { 00046 } 00047 00048 const std::string& ompl::base::OptimizationObjective::getDescription() const 00049 { 00050 return description_; 00051 } 00052 00053 bool ompl::base::OptimizationObjective::isSatisfied(Cost c) const 00054 { 00055 return this->isCostBetterThan(c, threshold_); 00056 } 00057 00058 ompl::base::Cost ompl::base::OptimizationObjective::getCostThreshold() const 00059 { 00060 return threshold_; 00061 } 00062 00063 void ompl::base::OptimizationObjective::setCostThreshold(Cost c) 00064 { 00065 threshold_ = c; 00066 } 00067 00068 bool ompl::base::OptimizationObjective::isCostBetterThan(Cost c1, Cost c2) const 00069 { 00070 return c1.value() + magic::BETTER_PATH_COST_MARGIN < c2.value(); 00071 } 00072 00073 ompl::base::Cost ompl::base::OptimizationObjective::combineCosts(Cost c1, Cost c2) const 00074 { 00075 return Cost(c1.value() + c2.value()); 00076 } 00077 00078 ompl::base::Cost ompl::base::OptimizationObjective::identityCost() const 00079 { 00080 return Cost(0.0); 00081 } 00082 00083 ompl::base::Cost ompl::base::OptimizationObjective::infiniteCost() const 00084 { 00085 return Cost(std::numeric_limits<double>::infinity()); 00086 } 00087 00088 ompl::base::Cost ompl::base::OptimizationObjective::initialCost(const State *s) const 00089 { 00090 return identityCost(); 00091 } 00092 00093 ompl::base::Cost ompl::base::OptimizationObjective::terminalCost(const State *s) const 00094 { 00095 return identityCost(); 00096 } 00097 00098 bool ompl::base::OptimizationObjective::isSymmetric() const 00099 { 00100 return si_->getStateSpace()->hasSymmetricInterpolate(); 00101 } 00102 00103 ompl::base::Cost ompl::base::OptimizationObjective::averageStateCost(unsigned int numStates) const 00104 { 00105 StateSamplerPtr ss = si_->allocStateSampler(); 00106 State *state = si_->allocState(); 00107 Cost totalCost(this->identityCost()); 00108 00109 for (unsigned int i = 0 ; i < numStates ; ++i) 00110 { 00111 ss->sampleUniform(state); 00112 totalCost = this->combineCosts(totalCost, this->stateCost(state)); 00113 } 00114 00115 si_->freeState(state); 00116 00117 return Cost(totalCost.value() / (double)numStates); 00118 } 00119 00120 void ompl::base::OptimizationObjective::setCostToGoHeuristic(const CostToGoHeuristic& costToGo) 00121 { 00122 costToGoFn_ = costToGo; 00123 } 00124 00125 ompl::base::Cost ompl::base::OptimizationObjective::costToGo(const State *state, const Goal *goal) const 00126 { 00127 if (costToGoFn_) 00128 return costToGoFn_(state, goal); 00129 else 00130 return this->identityCost(); // assumes that identity < all costs 00131 } 00132 00133 ompl::base::Cost ompl::base::OptimizationObjective::motionCostHeuristic(const State *s1, const State *s2) const 00134 { 00135 return this->identityCost(); // assumes that identity < all costs 00136 } 00137 00138 const ompl::base::SpaceInformationPtr& ompl::base::OptimizationObjective::getSpaceInformation() const 00139 { 00140 return si_; 00141 } 00142 00143 ompl::base::Cost ompl::base::goalRegionCostToGo(const State *state, const Goal *goal) 00144 { 00145 const GoalRegion *goalRegion = goal->as<GoalRegion>(); 00146 00147 // Ensures that all states within the goal region's threshold to 00148 // have a cost-to-go of exactly zero. 00149 return Cost(std::max(goalRegion->distanceGoal(state) - goalRegion->getThreshold(), 00150 0.0)); 00151 } 00152 00153 ompl::base::MultiOptimizationObjective::MultiOptimizationObjective(const SpaceInformationPtr &si) : 00154 OptimizationObjective(si), 00155 locked_(false) 00156 { 00157 } 00158 00159 ompl::base::MultiOptimizationObjective::Component:: 00160 Component(const OptimizationObjectivePtr& obj, double weight) : 00161 objective(obj), weight(weight) 00162 { 00163 } 00164 00165 void ompl::base::MultiOptimizationObjective::addObjective(const OptimizationObjectivePtr& objective, 00166 double weight) 00167 { 00168 if (locked_) 00169 { 00170 throw Exception("This optimization objective is locked. No further objectives can be added."); 00171 } 00172 else 00173 components_.push_back(Component(objective, weight)); 00174 } 00175 00176 std::size_t ompl::base::MultiOptimizationObjective::getObjectiveCount() const 00177 { 00178 return components_.size(); 00179 } 00180 00181 const ompl::base::OptimizationObjectivePtr& ompl::base::MultiOptimizationObjective::getObjective(unsigned int idx) const 00182 { 00183 if (components_.size() > idx) 00184 return components_[idx].objective; 00185 else 00186 throw Exception("Objective index does not exist."); 00187 } 00188 00189 double ompl::base::MultiOptimizationObjective::getObjectiveWeight(unsigned int idx) const 00190 { 00191 if (components_.size() > idx) 00192 return components_[idx].weight; 00193 else 00194 throw Exception("Objective index does not exist."); 00195 } 00196 00197 void ompl::base::MultiOptimizationObjective::setObjectiveWeight(unsigned int idx, 00198 double weight) 00199 { 00200 if (components_.size() > idx) 00201 components_[idx].weight = weight; 00202 else 00203 throw Exception("Objecitve index does not exist."); 00204 } 00205 00206 void ompl::base::MultiOptimizationObjective::lock() 00207 { 00208 locked_ = true; 00209 } 00210 00211 bool ompl::base::MultiOptimizationObjective::isLocked() const 00212 { 00213 return locked_; 00214 } 00215 00216 ompl::base::Cost ompl::base::MultiOptimizationObjective::stateCost(const State *s) const 00217 { 00218 Cost c = this->identityCost(); 00219 for (std::vector<Component>::const_iterator comp = components_.begin(); 00220 comp != components_.end(); 00221 ++comp) 00222 { 00223 c = Cost(c.value() + comp->weight * (comp->objective->stateCost(s).value())); 00224 } 00225 00226 return c; 00227 } 00228 00229 ompl::base::Cost ompl::base::MultiOptimizationObjective::motionCost(const State *s1, 00230 const State *s2) const 00231 { 00232 Cost c = this->identityCost(); 00233 for (std::vector<Component>::const_iterator comp = components_.begin(); 00234 comp != components_.end(); 00235 ++comp) 00236 { 00237 c = Cost(c.value() + comp->weight * (comp->objective->motionCost(s1, s2).value())); 00238 } 00239 00240 return c; 00241 } 00242 00243 ompl::base::OptimizationObjectivePtr ompl::base::operator+(const OptimizationObjectivePtr &a, 00244 const OptimizationObjectivePtr &b) 00245 { 00246 std::vector<MultiOptimizationObjective::Component> components; 00247 00248 if (a) 00249 { 00250 if (MultiOptimizationObjective *mult = dynamic_cast<MultiOptimizationObjective*>(a.get())) 00251 { 00252 for (std::size_t i = 0; i < mult->getObjectiveCount(); ++i) 00253 { 00254 components.push_back(MultiOptimizationObjective:: 00255 Component(mult->getObjective(i), 00256 mult->getObjectiveWeight(i))); 00257 } 00258 } 00259 else 00260 components.push_back(MultiOptimizationObjective::Component(a, 1.0)); 00261 } 00262 00263 if (b) 00264 { 00265 if (MultiOptimizationObjective *mult = dynamic_cast<MultiOptimizationObjective*>(b.get())) 00266 { 00267 for (std::size_t i = 0; i < mult->getObjectiveCount(); ++i) 00268 { 00269 components.push_back(MultiOptimizationObjective::Component(mult->getObjective(i), 00270 mult->getObjectiveWeight(i))); 00271 } 00272 } 00273 else 00274 components.push_back(MultiOptimizationObjective::Component(b, 1.0)); 00275 } 00276 00277 MultiOptimizationObjective *multObj = new MultiOptimizationObjective(a->getSpaceInformation()); 00278 00279 for (std::vector<MultiOptimizationObjective::Component>::const_iterator comp = components.begin(); 00280 comp != components.end(); 00281 ++comp) 00282 { 00283 multObj->addObjective(comp->objective, comp->weight); 00284 } 00285 00286 return OptimizationObjectivePtr(multObj); 00287 } 00288 00289 ompl::base::OptimizationObjectivePtr ompl::base::operator*(double weight, 00290 const OptimizationObjectivePtr &a) 00291 { 00292 std::vector<MultiOptimizationObjective::Component> components; 00293 00294 if (a) 00295 { 00296 if (MultiOptimizationObjective *mult = dynamic_cast<MultiOptimizationObjective*>(a.get())) 00297 { 00298 for (std::size_t i = 0; i < mult->getObjectiveCount(); ++i) 00299 { 00300 components.push_back(MultiOptimizationObjective 00301 ::Component(mult->getObjective(i), 00302 weight * mult->getObjectiveWeight(i))); 00303 } 00304 } 00305 else 00306 components.push_back(MultiOptimizationObjective::Component(a, weight)); 00307 } 00308 00309 MultiOptimizationObjective *multObj = new MultiOptimizationObjective(a->getSpaceInformation()); 00310 00311 for (std::vector<MultiOptimizationObjective::Component>::const_iterator comp = components.begin(); 00312 comp != components.end(); 00313 ++comp) 00314 { 00315 multObj->addObjective(comp->objective, comp->weight); 00316 } 00317 00318 return OptimizationObjectivePtr(multObj); 00319 } 00320 00321 ompl::base::OptimizationObjectivePtr ompl::base::operator*(const OptimizationObjectivePtr &a, 00322 double weight) 00323 { 00324 return weight * a; 00325 }