ompl/base/src/SpaceInformation.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/SpaceInformation.h"
00038 #include "ompl/base/samplers/UniformValidStateSampler.h"
00039 #include "ompl/base/DiscreteMotionValidator.h"
00040 #include "ompl/base/spaces/ReedsSheppStateSpace.h"
00041 #include "ompl/base/spaces/DubinsStateSpace.h"
00042 #include "ompl/util/Exception.h"
00043 #include "ompl/util/Time.h"
00044 #include "ompl/tools/config/MagicConstants.h"
00045 #include <queue>
00046 #include <cassert>
00047 
00048 ompl::base::SpaceInformation::SpaceInformation(const StateSpacePtr &space) :
00049     stateSpace_(space), setup_(false)
00050 {
00051     if (!stateSpace_)
00052         throw Exception("Invalid space definition");
00053     setDefaultMotionValidator();
00054     params_.include(stateSpace_->params());
00055 }
00056 
00057 void ompl::base::SpaceInformation::setup()
00058 {
00059     if (!stateValidityChecker_)
00060     {
00061         stateValidityChecker_.reset(new AllValidStateValidityChecker(this));
00062         OMPL_WARN("State validity checker not set! No collision checking is performed");
00063     }
00064 
00065     if (!motionValidator_)
00066         setDefaultMotionValidator();
00067 
00068     stateSpace_->setup();
00069     if (stateSpace_->getDimension() <= 0)
00070         throw Exception("The dimension of the state space we plan in must be > 0");
00071 
00072     params_.clear();
00073     params_.include(stateSpace_->params());
00074 
00075     setup_ = true;
00076 }
00077 
00078 bool ompl::base::SpaceInformation::isSetup() const
00079 {
00080     return setup_;
00081 }
00082 
00083 void ompl::base::SpaceInformation::setStateValidityChecker(const StateValidityCheckerFn &svc)
00084 {
00085     class BoostFnStateValidityChecker : public StateValidityChecker
00086     {
00087     public:
00088 
00089         BoostFnStateValidityChecker(SpaceInformation *si,
00090                                     const StateValidityCheckerFn &fn) : StateValidityChecker(si), fn_(fn)
00091         {
00092         }
00093 
00094         virtual bool isValid(const State *state) const
00095         {
00096             return fn_(state);
00097         }
00098 
00099     protected:
00100 
00101         StateValidityCheckerFn fn_;
00102     };
00103 
00104     if (!svc)
00105         throw Exception("Invalid function definition for state validity checking");
00106 
00107     setStateValidityChecker(StateValidityCheckerPtr(dynamic_cast<StateValidityChecker*>(new BoostFnStateValidityChecker(this, svc))));
00108 }
00109 
00110 void ompl::base::SpaceInformation::setDefaultMotionValidator()
00111 {
00112     if (dynamic_cast<ReedsSheppStateSpace*>(stateSpace_.get()))
00113          motionValidator_.reset(new ReedsSheppMotionValidator(this));
00114      else if (dynamic_cast<DubinsStateSpace*>(stateSpace_.get()))
00115          motionValidator_.reset(new DubinsMotionValidator(this));
00116      else
00117          motionValidator_.reset(new DiscreteMotionValidator(this));
00118 }
00119 
00120 
00121 void ompl::base::SpaceInformation::setValidStateSamplerAllocator(const ValidStateSamplerAllocator &vssa)
00122 {
00123     vssa_ = vssa;
00124     setup_ = false;
00125 }
00126 
00127 void ompl::base::SpaceInformation::clearValidStateSamplerAllocator()
00128 {
00129     vssa_ = ValidStateSamplerAllocator();
00130     setup_ = false;
00131 }
00132 
00133 unsigned int ompl::base::SpaceInformation::randomBounceMotion(const StateSamplerPtr &sss, const State *start, unsigned int steps, std::vector<State*> &states, bool alloc) const
00134 {
00135     if (alloc)
00136     {
00137         states.resize(steps);
00138         for (unsigned int i = 0 ; i < steps ; ++i)
00139             states[i] = allocState();
00140     }
00141     else
00142         if (states.size() < steps)
00143             steps = states.size();
00144 
00145     const State *prev = start;
00146     std::pair<State*, double> lastValid;
00147     unsigned int j = 0;
00148     for (unsigned int i = 0 ; i < steps ; ++i)
00149     {
00150         sss->sampleUniform(states[j]);
00151         lastValid.first = states[j];
00152         if (checkMotion(prev, states[j], lastValid) || lastValid.second > std::numeric_limits<double>::epsilon())
00153             prev = states[j++];
00154     }
00155 
00156     return j;
00157 }
00158 
00159 bool ompl::base::SpaceInformation::searchValidNearby(const ValidStateSamplerPtr &sampler, State *state, const State *near, double distance) const
00160 {
00161     if (state != near)
00162         copyState(state, near);
00163 
00164     // fix bounds, if needed
00165     if (!satisfiesBounds(state))
00166         enforceBounds(state);
00167 
00168     bool result = isValid(state);
00169 
00170     if (!result)
00171     {
00172         // try to find a valid state nearby
00173         State *temp = cloneState(state);
00174         result = sampler->sampleNear(state, temp, distance);
00175         freeState(temp);
00176     }
00177 
00178     return result;
00179 }
00180 
00181 bool ompl::base::SpaceInformation::searchValidNearby(State *state, const State *near, double distance, unsigned int attempts) const
00182 {
00183     if (satisfiesBounds(near) && isValid(near))
00184     {
00185         if (state != near)
00186             copyState(state, near);
00187         return true;
00188     }
00189     else
00190     {
00191         // try to find a valid state nearby
00192         UniformValidStateSampler *uvss = new UniformValidStateSampler(this);
00193         uvss->setNrAttempts(attempts);
00194         return searchValidNearby(ValidStateSamplerPtr(uvss), state, near, distance);
00195     }
00196 }
00197 
00198 unsigned int ompl::base::SpaceInformation::getMotionStates(const State *s1, const State *s2, std::vector<State*> &states, unsigned int count, bool endpoints, bool alloc) const
00199 {
00200     // add 1 to the number of states we want to add between s1 & s2. This gives us the number of segments to split the motion into
00201     count++;
00202 
00203     if (count < 2)
00204     {
00205         unsigned int added = 0;
00206 
00207         // if they want endpoints, then at most endpoints are included
00208         if (endpoints)
00209         {
00210             if (alloc)
00211             {
00212                 states.resize(2);
00213                 states[0] = allocState();
00214                 states[1] = allocState();
00215             }
00216             if (states.size() > 0)
00217             {
00218                 copyState(states[0], s1);
00219                 added++;
00220             }
00221 
00222             if (states.size() > 1)
00223             {
00224                 copyState(states[1], s2);
00225                 added++;
00226             }
00227         }
00228         else
00229             if (alloc)
00230                 states.resize(0);
00231         return added;
00232     }
00233 
00234     if (alloc)
00235     {
00236         states.resize(count + (endpoints ? 1 : -1));
00237         if (endpoints)
00238             states[0] = allocState();
00239     }
00240 
00241     unsigned int added = 0;
00242 
00243     if (endpoints && states.size() > 0)
00244     {
00245         copyState(states[0], s1);
00246         added++;
00247     }
00248 
00249     /* find the states in between */
00250     for (unsigned int j = 1 ; j < count && added < states.size() ; ++j)
00251     {
00252         if (alloc)
00253             states[added] = allocState();
00254         stateSpace_->interpolate(s1, s2, (double)j / (double)count, states[added]);
00255         added++;
00256     }
00257 
00258     if (added < states.size() && endpoints)
00259     {
00260         if (alloc)
00261             states[added] = allocState();
00262         copyState(states[added], s2);
00263         added++;
00264     }
00265 
00266     return added;
00267 }
00268 
00269 
00270 bool ompl::base::SpaceInformation::checkMotion(const std::vector<State*> &states, unsigned int count, unsigned int &firstInvalidStateIndex) const
00271 {
00272     assert(states.size() >= count);
00273     for (unsigned int i = 0 ; i < count ; ++i)
00274         if (!isValid(states[i]))
00275         {
00276             firstInvalidStateIndex = i;
00277             return false;
00278         }
00279     return true;
00280 }
00281 
00282 bool ompl::base::SpaceInformation::checkMotion(const std::vector<State*> &states, unsigned int count) const
00283 {
00284     assert(states.size() >= count);
00285     if (count > 0)
00286     {
00287         if (count > 1)
00288         {
00289             if (!isValid(states.front()))
00290                 return false;
00291             if (!isValid(states[count - 1]))
00292                 return false;
00293 
00294             // we have 2 or more states, and the first and last states are valid
00295 
00296             if (count > 2)
00297             {
00298                 std::queue< std::pair<int, int> > pos;
00299                 pos.push(std::make_pair(0, count - 1));
00300 
00301                 while (!pos.empty())
00302                 {
00303                     std::pair<int, int> x = pos.front();
00304 
00305                     int mid = (x.first + x.second) / 2;
00306                     if (!isValid(states[mid]))
00307                         return false;
00308 
00309                     if (x.first < mid - 1)
00310                         pos.push(std::make_pair(x.first, mid));
00311                     if (x.second > mid + 1)
00312                         pos.push(std::make_pair(mid, x.second));
00313                 }
00314             }
00315         }
00316         else
00317             return isValid(states.front());
00318     }
00319     return true;
00320 }
00321 
00322 ompl::base::ValidStateSamplerPtr ompl::base::SpaceInformation::allocValidStateSampler() const
00323 {
00324     if (vssa_)
00325         return vssa_(this);
00326     else
00327         return ValidStateSamplerPtr(new UniformValidStateSampler(this));
00328 }
00329 
00330 double ompl::base::SpaceInformation::probabilityOfValidState(unsigned int attempts) const
00331 {
00332     if (attempts == 0)
00333         return 0.0;
00334 
00335     unsigned int valid = 0;
00336     unsigned int invalid = 0;
00337 
00338     StateSamplerPtr ss = allocStateSampler();
00339     State *s = allocState();
00340 
00341     for (unsigned int i = 0 ; i < attempts ; ++i)
00342     {
00343         ss->sampleUniform(s);
00344         if (isValid(s))
00345             ++valid;
00346         else
00347             ++invalid;
00348     }
00349 
00350     freeState(s);
00351 
00352     return (double)valid / (double)(valid + invalid);
00353 }
00354 
00355 double ompl::base::SpaceInformation::averageValidMotionLength(unsigned int attempts) const
00356 {
00357     // take the square root here because we in fact have a nested for loop
00358     // where each loop executes #attempts steps (the sample() function of the UniformValidStateSampler if a for loop too)
00359     attempts = std::max((unsigned int)floor(sqrt((double)attempts) + 0.5), 2u);
00360 
00361     StateSamplerPtr ss = allocStateSampler();
00362     UniformValidStateSampler *uvss = new UniformValidStateSampler(this);
00363     uvss->setNrAttempts(attempts);
00364 
00365     State *s1 = allocState();
00366     State *s2 = allocState();
00367 
00368     std::pair<State*, double> lastValid;
00369     lastValid.first = NULL;
00370 
00371     double d = 0.0;
00372     unsigned int count = 0;
00373     for (unsigned int i = 0 ; i < attempts ; ++i)
00374         if (uvss->sample(s1))
00375         {
00376             ++count;
00377             ss->sampleUniform(s2);
00378             if (checkMotion(s1, s2, lastValid))
00379                 d += distance(s1, s2);
00380             else
00381                 d += distance(s1, s2) * lastValid.second;
00382         }
00383 
00384     freeState(s2);
00385     freeState(s1);
00386     delete uvss;
00387 
00388     if (count > 0)
00389         return d / (double)count;
00390     else
00391         return 0.0;
00392 }
00393 
00394 void ompl::base::SpaceInformation::samplesPerSecond(double &uniform, double &near, double &gaussian, unsigned int attempts) const
00395 {
00396     StateSamplerPtr ss = allocStateSampler();
00397     std::vector<State*> states(attempts + 1);
00398     allocStates(states);
00399 
00400     time::point start = time::now();
00401     for (unsigned int i = 0 ; i < attempts ; ++i)
00402         ss->sampleUniform(states[i]);
00403     uniform = (double)attempts / time::seconds(time::now() - start);
00404 
00405     double d = getMaximumExtent() / 10.0;
00406     ss->sampleUniform(states[attempts]);
00407 
00408     start = time::now();
00409     for (unsigned int i = 1 ; i <= attempts ; ++i)
00410         ss->sampleUniformNear(states[i - 1], states[i], d);
00411     near = (double)attempts / time::seconds(time::now() - start);
00412 
00413     start = time::now();
00414     for (unsigned int i = 1 ; i <= attempts ; ++i)
00415         ss->sampleGaussian(states[i - 1], states[i], d);
00416     gaussian = (double)attempts / time::seconds(time::now() - start);
00417 
00418     freeStates(states);
00419 }
00420 
00421 void ompl::base::SpaceInformation::printSettings(std::ostream &out) const
00422 {
00423     out << "Settings for the state space '" << stateSpace_->getName() << "'" << std::endl;
00424     out << "  - state validity check resolution: " << (getStateValidityCheckingResolution() * 100.0) << '%' << std::endl;
00425     out << "  - valid segment count factor: " << stateSpace_->getValidSegmentCountFactor() << std::endl;
00426     out << "  - state space:" << std::endl;
00427     stateSpace_->printSettings(out);
00428     out << std::endl << "Declared parameters:" << std::endl;
00429     params_.print(out);
00430     ValidStateSamplerPtr vss = allocValidStateSampler();
00431     out << "Valid state sampler named " << vss->getName() << " with parameters:" << std::endl;
00432     vss->params().print(out);
00433 }
00434 
00435 void ompl::base::SpaceInformation::printProperties(std::ostream &out) const
00436 {
00437     out << "Properties of the state space '" << stateSpace_->getName() << "'" << std::endl;
00438     out << "  - signature: ";
00439     std::vector<int> sig;
00440     stateSpace_->computeSignature(sig);
00441     for (std::size_t i = 0 ; i < sig.size() ; ++i)
00442         out << sig[i] << " ";
00443     out << std::endl;
00444     out << "  - dimension: " << stateSpace_->getDimension() << std::endl;
00445     out << "  - extent: " << stateSpace_->getMaximumExtent() << std::endl;
00446     if (isSetup())
00447     {
00448         bool result = true;
00449         try
00450         {
00451             stateSpace_->sanityChecks();
00452         }
00453         catch(Exception &e)
00454         {
00455             result = false;
00456             out << std::endl << "  - SANITY CHECKS FOR STATE SPACE ***DID NOT PASS*** (" << e.what() << ")" << std::endl << std::endl;
00457             OMPL_ERROR(e.what());
00458         }
00459         if (result)
00460             out << "  - sanity checks for state space passed" << std::endl;
00461         out << "  - probability of valid states: " << probabilityOfValidState(magic::TEST_STATE_COUNT) << std::endl;
00462         out << "  - average length of a valid motion: " << averageValidMotionLength(magic::TEST_STATE_COUNT) << std::endl;
00463         double uniform, near, gaussian;
00464         samplesPerSecond(uniform, near, gaussian, magic::TEST_STATE_COUNT);
00465         out << "  - average number of samples drawn per second: sampleUniform()=" << uniform << " sampleUniformNear()=" << near << " sampleGaussian()=" << gaussian << std::endl;
00466     }
00467     else
00468         out << "Call setup() before to get more information" << std::endl;
00469 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines