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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines