ompl/base/ProblemDefinition.h
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2010, 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: Ioan Sucan */
00036 
00037 #ifndef OMPL_BASE_PROBLEM_DEFINITION_
00038 #define OMPL_BASE_PROBLEM_DEFINITION_
00039 
00040 #include "ompl/base/State.h"
00041 #include "ompl/base/Goal.h"
00042 #include "ompl/base/Path.h"
00043 #include "ompl/base/Cost.h"
00044 #include "ompl/base/SpaceInformation.h"
00045 #include "ompl/base/SolutionNonExistenceProof.h"
00046 #include "ompl/util/Console.h"
00047 #include "ompl/util/ClassForward.h"
00048 #include "ompl/base/ScopedState.h"
00049 
00050 #include <vector>
00051 #include <cstdlib>
00052 #include <iostream>
00053 #include <limits>
00054 
00055 #include <boost/noncopyable.hpp>
00056 
00057 namespace ompl
00058 {
00059     namespace base
00060     {
00061 
00063 
00064         OMPL_CLASS_FORWARD(ProblemDefinition);
00065         OMPL_CLASS_FORWARD(OptimizationObjective);
00067 
00072         struct PlannerSolution
00073         {
00075             PlannerSolution(const PathPtr &path) :
00076                 index_(-1), path_(path), length_(path->length()),
00077                 approximate_(false), difference_(-1), optimized_(false)
00078             {
00079             }
00080 
00082             bool operator==(const PlannerSolution &p) const
00083             {
00084                 return path_ == p.path_;
00085             }
00086 
00088             bool operator<(const PlannerSolution &b) const;
00089 
00091             void setApproximate(double difference)
00092             {
00093                 approximate_ = true;
00094                 difference_ = difference;
00095             }
00096 
00098             void setOptimized(const OptimizationObjectivePtr &opt, Cost cost, bool meetsObjective)
00099             {
00100                 opt_ = opt;
00101                 cost_ = cost;
00102                 optimized_ = meetsObjective;
00103             }
00104 
00106             void setPlannerName(const std::string &name)
00107             {
00108                 plannerName_ = name;
00109             }
00110 
00112             int     index_;
00113 
00115             PathPtr path_;
00116 
00118             double  length_;
00119 
00121             bool    approximate_;
00122 
00124             double  difference_;
00125 
00127             bool    optimized_;
00128 
00130             OptimizationObjectivePtr opt_;
00131 
00133             Cost cost_;
00134 
00136             std::string plannerName_;
00137         };
00138 
00139         class Planner;
00140 
00143         typedef boost::function<void(const Planner*, const std::vector<const base::State*> &, const Cost)> ReportIntermediateSolutionFn;
00144 
00145         OMPL_CLASS_FORWARD(OptimizationObjective);
00146 
00150         class ProblemDefinition : private boost::noncopyable
00151         {
00152         public:
00153 
00155             ProblemDefinition(const SpaceInformationPtr &si);
00156 
00157             virtual ~ProblemDefinition()
00158             {
00159                 clearStartStates();
00160             }
00161 
00163             const SpaceInformationPtr& getSpaceInformation() const
00164             {
00165                 return si_;
00166             }
00167 
00169             void addStartState(const State *state)
00170             {
00171                 startStates_.push_back(si_->cloneState(state));
00172             }
00173 
00175             void addStartState(const ScopedState<> &state)
00176             {
00177                 startStates_.push_back(si_->cloneState(state.get()));
00178             }
00179 
00183             bool hasStartState(const State *state, unsigned int *startIndex = NULL);
00184 
00186             void clearStartStates()
00187             {
00188                 for (unsigned int i = 0 ; i < startStates_.size() ; ++i)
00189                     si_->freeState(startStates_[i]);
00190                 startStates_.clear();
00191             }
00192 
00194             unsigned int getStartStateCount() const
00195             {
00196                 return startStates_.size();
00197             }
00198 
00200             const State* getStartState(unsigned int index) const
00201             {
00202                 return startStates_[index];
00203             }
00204 
00206             State* getStartState(unsigned int index)
00207             {
00208                 return startStates_[index];
00209             }
00210 
00212             void setGoal(const GoalPtr &goal)
00213             {
00214                 goal_ = goal;
00215             }
00216 
00218             void clearGoal()
00219             {
00220                 goal_.reset();
00221             }
00222 
00224             const GoalPtr& getGoal() const
00225             {
00226                 return goal_;
00227             }
00228 
00233             void getInputStates(std::vector<const State*> &states) const;
00234 
00242             void setStartAndGoalStates(const State *start, const State *goal, const double threshold = std::numeric_limits<double>::epsilon());
00243 
00245             void setGoalState(const State *goal, const double threshold = std::numeric_limits<double>::epsilon());
00246 
00248             void setStartAndGoalStates(const ScopedState<> &start, const ScopedState<> &goal, const double threshold = std::numeric_limits<double>::epsilon())
00249             {
00250                 setStartAndGoalStates(start.get(), goal.get(), threshold);
00251             }
00252 
00254             void setGoalState(const ScopedState<> &goal, const double threshold = std::numeric_limits<double>::epsilon())
00255             {
00256                 setGoalState(goal.get(), threshold);
00257             }
00258 
00260             bool hasOptimizationObjective() const
00261             {
00262                 return optimizationObjective_.get();
00263             }
00264 
00266             const OptimizationObjectivePtr& getOptimizationObjective() const
00267             {
00268                 return optimizationObjective_;
00269             }
00270 
00272             void setOptimizationObjective(const OptimizationObjectivePtr &optimizationObjective)
00273             {
00274                 optimizationObjective_ = optimizationObjective;
00275             }
00276 
00279             const ReportIntermediateSolutionFn& getIntermediateSolutionCallback() const
00280             {
00281                 return intermediateSolutionCallback_;
00282             }
00283 
00285             void setIntermediateSolutionCallback(const ReportIntermediateSolutionFn &callback) {
00286                 intermediateSolutionCallback_ = callback;
00287              }
00288 
00294             bool isTrivial(unsigned int *startIndex = NULL, double *distance = NULL) const;
00295 
00308             PathPtr isStraightLinePathValid() const;
00309 
00314             bool fixInvalidInputStates(double distStart, double distGoal, unsigned int attempts);
00315 
00317             bool hasSolution() const;
00318 
00322             bool hasApproximateSolution() const;
00323 
00325             double getSolutionDifference() const;
00326 
00328             bool hasOptimizedSolution() const;
00329 
00334             PathPtr getSolutionPath() const;
00335 
00339             bool getSolution(PlannerSolution& solution) const;
00340 
00346             void addSolutionPath(const PathPtr &path, bool approximate = false, double difference = -1.0, const std::string& plannerName = "Unknown") const;
00347 
00349             void addSolutionPath(const PlannerSolution &sol) const;
00350 
00352             std::size_t getSolutionCount() const;
00353 
00355             std::vector<PlannerSolution> getSolutions() const;
00356 
00358             void clearSolutionPaths() const;
00359 
00361             bool hasSolutionNonExistenceProof() const;
00362 
00364             void clearSolutionNonExistenceProof();
00365 
00367             const SolutionNonExistenceProofPtr& getSolutionNonExistenceProof() const;
00368 
00370             void setSolutionNonExistenceProof(const SolutionNonExistenceProofPtr& nonExistenceProof);
00371 
00373             void print(std::ostream &out = std::cout) const;
00374 
00375         protected:
00376 
00378             bool fixInvalidInputState(State *state, double dist, bool start, unsigned int attempts);
00379 
00381             SpaceInformationPtr          si_;
00382 
00384             std::vector<State*>          startStates_;
00385 
00387             GoalPtr                      goal_;
00388 
00390             SolutionNonExistenceProofPtr nonExistenceProof_;
00391 
00393             OptimizationObjectivePtr     optimizationObjective_;
00394 
00396             ReportIntermediateSolutionFn     intermediateSolutionCallback_;
00397 
00398         private:
00399 
00401             OMPL_CLASS_FORWARD(PlannerSolutionSet);
00403 
00405             PlannerSolutionSetPtr        solutions_;
00406         };
00407     }
00408 }
00409 
00410 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines