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