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