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