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