ompl/base/OptimizationObjective.h
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2012, 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 #ifndef OMPL_BASE_OPTIMIZATION_OBJECTIVE_
00038 #define OMPL_BASE_OPTIMIZATION_OBJECTIVE_
00039 
00040 #include "ompl/base/Cost.h"
00041 #include "ompl/base/SpaceInformation.h"
00042 #include "ompl/util/ClassForward.h"
00043 #include <boost/noncopyable.hpp>
00044 #include <boost/concept_check.hpp>
00045 
00046 namespace ompl
00047 {
00048     namespace base
00049     {
00050         class Goal;
00051 
00053         typedef boost::function<Cost (const State*, const Goal*)> CostToGoHeuristic;
00054 
00056 
00057         OMPL_CLASS_FORWARD(OptimizationObjective);
00059 
00066         class OptimizationObjective : private boost::noncopyable
00067         {
00068         public:
00070             OptimizationObjective(const SpaceInformationPtr &si);
00071 
00072             virtual ~OptimizationObjective()
00073             {
00074             }
00075 
00077             const std::string& getDescription() const;
00078 
00080             virtual bool isSatisfied(Cost c) const;
00081 
00083             Cost getCostThreshold() const;
00084 
00086             void setCostThreshold(Cost c);
00087 
00089             virtual bool isCostBetterThan(Cost c1, Cost c2) const;
00090 
00092             virtual Cost stateCost(const State *s) const = 0;
00093 
00095             virtual Cost motionCost(const State *s1, const State *s2) const = 0;
00096 
00098             virtual Cost combineCosts(Cost c1, Cost c2) const;
00099 
00101             virtual Cost identityCost() const;
00102 
00104             virtual Cost infiniteCost() const;
00105 
00107             virtual Cost initialCost(const State *s) const;
00108 
00110             virtual Cost terminalCost(const State *s) const;
00111 
00113             virtual bool isSymmetric() const;
00114 
00116             virtual Cost averageStateCost(unsigned int numStates) const;
00117 
00119             void setCostToGoHeuristic(const CostToGoHeuristic& costToGo);
00120 
00122             Cost costToGo(const State *state, const Goal *goal) const;
00123 
00125             virtual Cost motionCostHeuristic(const State *s1, const State *s2) const;
00126 
00128             const SpaceInformationPtr& getSpaceInformation() const;
00129 
00130         protected:
00132             SpaceInformationPtr si_;
00133 
00135             std::string         description_;
00136 
00138             Cost                threshold_;
00139 
00141             CostToGoHeuristic   costToGoFn_;
00142         };
00143 
00152         Cost goalRegionCostToGo(const State *state, const Goal *goal);
00153 
00155         class MultiOptimizationObjective : public OptimizationObjective
00156         {
00157         public:
00158             MultiOptimizationObjective(const SpaceInformationPtr &si);
00159 
00161             void addObjective(const OptimizationObjectivePtr& objective,
00162                               double weight);
00163 
00165             std::size_t getObjectiveCount() const;
00166 
00168             const OptimizationObjectivePtr& getObjective(unsigned int idx) const;
00169 
00171             double getObjectiveWeight(unsigned int idx) const;
00172 
00174             void setObjectiveWeight(unsigned int idx, double weight);
00175 
00177             void lock();
00178 
00180             bool isLocked() const;
00181 
00186             virtual Cost stateCost(const State *s) const;
00187 
00192             virtual Cost motionCost(const State *s1, const State *s2) const;
00193 
00194         protected:
00195 
00197             struct Component
00198             {
00199                 Component(const OptimizationObjectivePtr& obj, double weight);
00200                 OptimizationObjectivePtr objective;
00201                 double weight;
00202             };
00203 
00205             std::vector<Component> components_;
00206 
00208             bool                   locked_;
00209 
00210             // Friend functions for operator overloads for easy multiobjective creation
00211             friend OptimizationObjectivePtr operator+(const OptimizationObjectivePtr &a,
00212                                                       const OptimizationObjectivePtr &b);
00213 
00214             friend OptimizationObjectivePtr operator*(double w, const OptimizationObjectivePtr &a);
00215 
00216             friend OptimizationObjectivePtr operator*(const OptimizationObjectivePtr &a, double w);
00217         };
00218 
00220         OptimizationObjectivePtr operator+(const OptimizationObjectivePtr &a,
00221                                            const OptimizationObjectivePtr &b);
00222 
00224         OptimizationObjectivePtr operator*(double w, const OptimizationObjectivePtr &a);
00225 
00227         OptimizationObjectivePtr operator*(const OptimizationObjectivePtr &a, double w);
00228     }
00229 }
00230 
00231 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines