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