ompl/base/src/ProblemDefinition.cpp
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 #include "ompl/base/ProblemDefinition.h" 00038 #include "ompl/base/goals/GoalState.h" 00039 #include "ompl/base/goals/GoalStates.h" 00040 #include "ompl/base/OptimizationObjective.h" 00041 #include "ompl/control/SpaceInformation.h" 00042 #include "ompl/control/PathControl.h" 00043 #include "ompl/tools/config/MagicConstants.h" 00044 #include <sstream> 00045 #include <algorithm> 00046 00047 #include <boost/thread/mutex.hpp> 00048 00050 namespace ompl 00051 { 00052 namespace base 00053 { 00054 00055 class ProblemDefinition::PlannerSolutionSet 00056 { 00057 public: 00058 00059 PlannerSolutionSet() 00060 { 00061 } 00062 00063 void add(const PlannerSolution &s) 00064 { 00065 boost::mutex::scoped_lock slock(lock_); 00066 int index = solutions_.size(); 00067 solutions_.push_back(s); 00068 solutions_.back().index_ = index; 00069 std::sort(solutions_.begin(), solutions_.end()); 00070 } 00071 00072 void clear() 00073 { 00074 boost::mutex::scoped_lock slock(lock_); 00075 solutions_.clear(); 00076 } 00077 00078 std::vector<PlannerSolution> getSolutions() 00079 { 00080 boost::mutex::scoped_lock slock(lock_); 00081 std::vector<PlannerSolution> copy = solutions_; 00082 return copy; 00083 } 00084 00085 bool isApproximate() 00086 { 00087 boost::mutex::scoped_lock slock(lock_); 00088 bool result = false; 00089 if (!solutions_.empty()) 00090 result = solutions_[0].approximate_; 00091 return result; 00092 } 00093 00094 bool isOptimized() 00095 { 00096 boost::mutex::scoped_lock slock(lock_); 00097 bool result = false; 00098 if (!solutions_.empty()) 00099 result = solutions_[0].optimized_; 00100 return result; 00101 } 00102 00103 double getDifference() 00104 { 00105 boost::mutex::scoped_lock slock(lock_); 00106 double diff = -1.0; 00107 if (!solutions_.empty()) 00108 diff = solutions_[0].difference_; 00109 return diff; 00110 } 00111 00112 PathPtr getTopSolution() 00113 { 00114 boost::mutex::scoped_lock slock(lock_); 00115 PathPtr copy; 00116 if (!solutions_.empty()) 00117 copy = solutions_[0].path_; 00118 return copy; 00119 } 00120 00121 bool getTopSolution(PlannerSolution& solution) 00122 { 00123 boost::mutex::scoped_lock slock(lock_); 00124 00125 if (!solutions_.empty()) 00126 { 00127 solution = solutions_[0]; 00128 return true; 00129 } 00130 else 00131 { 00132 return false; 00133 } 00134 } 00135 00136 std::size_t getSolutionCount() 00137 { 00138 boost::mutex::scoped_lock slock(lock_); 00139 std::size_t result = solutions_.size(); 00140 return result; 00141 } 00142 00143 private: 00144 00145 std::vector<PlannerSolution> solutions_; 00146 boost::mutex lock_; 00147 }; 00148 } 00149 } 00151 00152 bool ompl::base::PlannerSolution::operator<(const PlannerSolution &b) const 00153 { 00154 if (!approximate_ && b.approximate_) 00155 return true; 00156 if (approximate_ && !b.approximate_) 00157 return false; 00158 if (approximate_ && b.approximate_) 00159 return difference_ < b.difference_; 00160 if (optimized_ && !b.optimized_) 00161 return true; 00162 if (!optimized_ && b.optimized_) 00163 return false; 00164 return opt_ ? opt_->isCostBetterThan(cost_, b.cost_) : length_ < b.length_; 00165 } 00166 00167 ompl::base::ProblemDefinition::ProblemDefinition(const SpaceInformationPtr &si) : si_(si), solutions_(new PlannerSolutionSet()) 00168 { 00169 } 00170 00171 void ompl::base::ProblemDefinition::setStartAndGoalStates(const State *start, const State *goal, const double threshold) 00172 { 00173 clearStartStates(); 00174 addStartState(start); 00175 setGoalState(goal, threshold); 00176 } 00177 00178 void ompl::base::ProblemDefinition::setGoalState(const State *goal, const double threshold) 00179 { 00180 clearGoal(); 00181 GoalState *gs = new GoalState(si_); 00182 gs->setState(goal); 00183 gs->setThreshold(threshold); 00184 setGoal(GoalPtr(gs)); 00185 } 00186 00187 bool ompl::base::ProblemDefinition::hasStartState(const State *state, unsigned int *startIndex) 00188 { 00189 for (unsigned int i = 0 ; i < startStates_.size() ; ++i) 00190 if (si_->equalStates(state, startStates_[i])) 00191 { 00192 if (startIndex) 00193 *startIndex = i; 00194 return true; 00195 } 00196 return false; 00197 } 00198 00199 bool ompl::base::ProblemDefinition::fixInvalidInputState(State *state, double dist, bool start, unsigned int attempts) 00200 { 00201 bool result = false; 00202 00203 bool b = si_->satisfiesBounds(state); 00204 bool v = false; 00205 if (b) 00206 { 00207 v = si_->isValid(state); 00208 if (!v) 00209 OMPL_DEBUG("%s state is not valid", start ? "Start" : "Goal"); 00210 } 00211 else 00212 OMPL_DEBUG("%s state is not within space bounds", start ? "Start" : "Goal"); 00213 00214 if (!b || !v) 00215 { 00216 std::stringstream ss; 00217 si_->printState(state, ss); 00218 ss << " within distance " << dist; 00219 OMPL_DEBUG("Attempting to fix %s state %s", start ? "start" : "goal", ss.str().c_str()); 00220 00221 State *temp = si_->allocState(); 00222 if (si_->searchValidNearby(temp, state, dist, attempts)) 00223 { 00224 si_->copyState(state, temp); 00225 result = true; 00226 } 00227 else 00228 OMPL_WARN("Unable to fix %s state", start ? "start" : "goal"); 00229 si_->freeState(temp); 00230 } 00231 00232 return result; 00233 } 00234 00235 bool ompl::base::ProblemDefinition::fixInvalidInputStates(double distStart, double distGoal, unsigned int attempts) 00236 { 00237 bool result = true; 00238 00239 // fix start states 00240 for (unsigned int i = 0 ; i < startStates_.size() ; ++i) 00241 if (!fixInvalidInputState(startStates_[i], distStart, true, attempts)) 00242 result = false; 00243 00244 // fix goal state 00245 GoalState *goal = dynamic_cast<GoalState*>(goal_.get()); 00246 if (goal) 00247 { 00248 if (!fixInvalidInputState(const_cast<State*>(goal->getState()), distGoal, false, attempts)) 00249 result = false; 00250 } 00251 00252 // fix goal state 00253 GoalStates *goals = dynamic_cast<GoalStates*>(goal_.get()); 00254 if (goals) 00255 { 00256 for (unsigned int i = 0; i < goals->getStateCount(); ++i) 00257 if (!fixInvalidInputState(const_cast<State*>(goals->getState(i)), distGoal, false, attempts)) 00258 result = false; 00259 } 00260 00261 return result; 00262 } 00263 00264 void ompl::base::ProblemDefinition::getInputStates(std::vector<const State*> &states) const 00265 { 00266 states.clear(); 00267 for (unsigned int i = 0 ; i < startStates_.size() ; ++i) 00268 states.push_back(startStates_[i]); 00269 00270 GoalState *goal = dynamic_cast<GoalState*>(goal_.get()); 00271 if (goal) 00272 states.push_back(goal->getState()); 00273 00274 GoalStates *goals = dynamic_cast<GoalStates*>(goal_.get()); 00275 if (goals) 00276 for (unsigned int i = 0; i < goals->getStateCount(); ++i) 00277 states.push_back (goals->getState(i)); 00278 } 00279 00280 ompl::base::PathPtr ompl::base::ProblemDefinition::isStraightLinePathValid() const 00281 { 00282 PathPtr path; 00283 if (control::SpaceInformationPtr sic = boost::dynamic_pointer_cast<control::SpaceInformation, SpaceInformation>(si_)) 00284 { 00285 unsigned int startIndex; 00286 if (isTrivial(&startIndex, NULL)) 00287 { 00288 control::PathControl *pc = new control::PathControl(sic); 00289 pc->append(startStates_[startIndex]); 00290 control::Control *null = sic->allocControl(); 00291 sic->nullControl(null); 00292 pc->append(startStates_[startIndex], null, 0.0); 00293 sic->freeControl(null); 00294 path.reset(pc); 00295 } 00296 else 00297 { 00298 control::Control *nc = sic->allocControl(); 00299 State *result1 = sic->allocState(); 00300 State *result2 = sic->allocState(); 00301 sic->nullControl(nc); 00302 00303 for (unsigned int k = 0 ; k < startStates_.size() && !path ; ++k) 00304 { 00305 const State *start = startStates_[k]; 00306 if (start && si_->isValid(start) && si_->satisfiesBounds(start)) 00307 { 00308 sic->copyState(result1, start); 00309 for (unsigned int i = 0 ; i < sic->getMaxControlDuration() && !path ; ++i) 00310 if (sic->propagateWhileValid(result1, nc, 1, result2)) 00311 { 00312 if (goal_->isSatisfied(result2)) 00313 { 00314 control::PathControl *pc = new control::PathControl(sic); 00315 pc->append(start); 00316 pc->append(result2, nc, (i + 1) * sic->getPropagationStepSize()); 00317 path.reset(pc); 00318 break; 00319 } 00320 std::swap(result1, result2); 00321 } 00322 } 00323 } 00324 sic->freeState(result1); 00325 sic->freeState(result2); 00326 sic->freeControl(nc); 00327 } 00328 } 00329 else 00330 { 00331 std::vector<const State*> states; 00332 GoalState *goal = dynamic_cast<GoalState*>(goal_.get()); 00333 if (goal) 00334 if (si_->isValid(goal->getState()) && si_->satisfiesBounds(goal->getState())) 00335 states.push_back(goal->getState()); 00336 GoalStates *goals = dynamic_cast<GoalStates*>(goal_.get()); 00337 if (goals) 00338 for (unsigned int i = 0; i < goals->getStateCount(); ++i) 00339 if (si_->isValid(goals->getState(i)) && si_->satisfiesBounds(goals->getState(i))) 00340 states.push_back(goals->getState(i)); 00341 00342 if (states.empty()) 00343 { 00344 unsigned int startIndex; 00345 if (isTrivial(&startIndex)) 00346 { 00347 geometric::PathGeometric *pg = new geometric::PathGeometric(si_, startStates_[startIndex], startStates_[startIndex]); 00348 path.reset(pg); 00349 } 00350 } 00351 else 00352 { 00353 for (unsigned int i = 0 ; i < startStates_.size() && !path ; ++i) 00354 { 00355 const State *start = startStates_[i]; 00356 if (start && si_->isValid(start) && si_->satisfiesBounds(start)) 00357 { 00358 for (unsigned int j = 0 ; j < states.size() && !path ; ++j) 00359 if (si_->checkMotion(start, states[j])) 00360 { 00361 geometric::PathGeometric *pg = new geometric::PathGeometric(si_, start, states[j]); 00362 path.reset(pg); 00363 break; 00364 } 00365 } 00366 } 00367 } 00368 } 00369 00370 return path; 00371 } 00372 00373 bool ompl::base::ProblemDefinition::isTrivial(unsigned int *startIndex, double *distance) const 00374 { 00375 if (!goal_) 00376 { 00377 OMPL_ERROR("Goal undefined"); 00378 return false; 00379 } 00380 00381 for (unsigned int i = 0 ; i < startStates_.size() ; ++i) 00382 { 00383 const State *start = startStates_[i]; 00384 if (start && si_->isValid(start) && si_->satisfiesBounds(start)) 00385 { 00386 double dist; 00387 if (goal_->isSatisfied(start, &dist)) 00388 { 00389 if (startIndex) 00390 *startIndex = i; 00391 if (distance) 00392 *distance = dist; 00393 return true; 00394 } 00395 } 00396 else 00397 { 00398 OMPL_ERROR("Initial state is in collision!"); 00399 } 00400 } 00401 00402 return false; 00403 } 00404 00405 bool ompl::base::ProblemDefinition::hasSolution() const 00406 { 00407 return solutions_->getSolutionCount() > 0; 00408 } 00409 00410 std::size_t ompl::base::ProblemDefinition::getSolutionCount() const 00411 { 00412 return solutions_->getSolutionCount(); 00413 } 00414 00415 ompl::base::PathPtr ompl::base::ProblemDefinition::getSolutionPath() const 00416 { 00417 return solutions_->getTopSolution(); 00418 } 00419 00420 bool ompl::base::ProblemDefinition::getSolution(PlannerSolution& solution) const 00421 { 00422 return solutions_->getTopSolution(solution); 00423 } 00424 00425 void ompl::base::ProblemDefinition::addSolutionPath(const PathPtr &path, bool approximate, double difference, const std::string& plannerName) const 00426 { 00427 PlannerSolution sol(path); 00428 if (approximate) 00429 sol.setApproximate(difference); 00430 sol.setPlannerName(plannerName); 00431 addSolutionPath(sol); 00432 } 00433 00434 void ompl::base::ProblemDefinition::addSolutionPath(const PlannerSolution &sol) const 00435 { 00436 if (sol.approximate_) 00437 OMPL_INFORM("ProblemDefinition: Adding approximate solution from planner %s", sol.plannerName_.c_str()); 00438 solutions_->add(sol); 00439 } 00440 00441 bool ompl::base::ProblemDefinition::hasApproximateSolution() const 00442 { 00443 return solutions_->isApproximate(); 00444 } 00445 00446 bool ompl::base::ProblemDefinition::hasOptimizedSolution() const 00447 { 00448 return solutions_->isOptimized(); 00449 } 00450 00451 double ompl::base::ProblemDefinition::getSolutionDifference() const 00452 { 00453 return solutions_->getDifference(); 00454 } 00455 00456 std::vector<ompl::base::PlannerSolution> ompl::base::ProblemDefinition::getSolutions() const 00457 { 00458 return solutions_->getSolutions(); 00459 } 00460 00461 void ompl::base::ProblemDefinition::clearSolutionPaths() const 00462 { 00463 solutions_->clear(); 00464 } 00465 00466 void ompl::base::ProblemDefinition::print(std::ostream &out) const 00467 { 00468 out << "Start states:" << std::endl; 00469 for (unsigned int i = 0 ; i < startStates_.size() ; ++i) 00470 si_->printState(startStates_[i], out); 00471 if (goal_) 00472 goal_->print(out); 00473 else 00474 out << "Goal = NULL" << std::endl; 00475 if (optimizationObjective_) 00476 out << "Average state cost: " << optimizationObjective_->averageStateCost(magic::TEST_STATE_COUNT) << std::endl; 00477 out << "There are " << solutions_->getSolutionCount() << " solutions" << std::endl; 00478 } 00479 00480 bool ompl::base::ProblemDefinition::hasSolutionNonExistenceProof() const 00481 { 00482 return nonExistenceProof_.get(); 00483 } 00484 00485 void ompl::base::ProblemDefinition::clearSolutionNonExistenceProof() 00486 { 00487 nonExistenceProof_.reset(); 00488 } 00489 00490 const ompl::base::SolutionNonExistenceProofPtr& ompl::base::ProblemDefinition::getSolutionNonExistenceProof() const 00491 { 00492 return nonExistenceProof_; 00493 } 00494 00495 void ompl::base::ProblemDefinition::setSolutionNonExistenceProof(const ompl::base::SolutionNonExistenceProofPtr& nonExistenceProof) 00496 { 00497 nonExistenceProof_ = nonExistenceProof; 00498 }