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 <queue>
00042 #include <cassert>
00043 
00044 ompl::base::SpaceInformation::SpaceInformation(const StateManifoldPtr &manifold) :
00045     stateManifold_(manifold), motionValidator_(new DiscreteMotionValidator(this)), setup_(false), msg_("SpaceInformation")
00046 {
00047     if (!stateManifold_)
00048         throw Exception("Invalid manifold definition");
00049 }
00050 
00051 void ompl::base::SpaceInformation::setup(void)
00052 {
00053     if (!stateValidityChecker_)
00054     {
00055         stateValidityChecker_.reset(new AllValidStateValidityChecker(this));
00056         msg_.warn("State validity checker not set! No collision checking is performed");
00057     }
00058 
00059     if (!motionValidator_)
00060         motionValidator_.reset(new DiscreteMotionValidator(this));
00061 
00062     stateManifold_->setup();
00063     if (stateManifold_->getDimension() <= 0)
00064         throw Exception("The dimension of the state manifold we plan in must be > 0");
00065 
00066     setup_ = true;
00067 }
00068 
00069 bool ompl::base::SpaceInformation::isSetup(void) const
00070 {
00071     return setup_;
00072 }
00073 
00074 void ompl::base::SpaceInformation::setStateValidityChecker(const StateValidityCheckerFn &svc)
00075 {
00076     class BoostFnStateValidityChecker : public StateValidityChecker
00077     {
00078     public:
00079 
00080         BoostFnStateValidityChecker(SpaceInformation* si,
00081                                     const StateValidityCheckerFn &fn) : StateValidityChecker(si), fn_(fn)
00082         {
00083         }
00084 
00085         virtual bool isValid(const State *state) const
00086         {
00087             return fn_(state);
00088         }
00089 
00090     protected:
00091 
00092         StateValidityCheckerFn fn_;
00093     };
00094 
00095     if (!svc)
00096         throw Exception("Invalid function definition for state validity checking");
00097 
00098     setStateValidityChecker(StateValidityCheckerPtr(dynamic_cast<StateValidityChecker*>(new BoostFnStateValidityChecker(this, svc))));
00099 }
00100 
00101 unsigned int ompl::base::SpaceInformation::randomBounceMotion(const ManifoldStateSamplerPtr &mss, const State *start, unsigned int steps, std::vector<State*> &states, bool alloc) const
00102 {
00103     if (alloc)
00104     {
00105         states.resize(steps);
00106         for (unsigned int i = 0 ; i < steps ; ++i)
00107             states[i] = allocState();
00108     }
00109     else
00110         if (states.size() < steps)
00111             steps = states.size();
00112 
00113     const State *prev = start;
00114     std::pair<State*, double> lastValid;
00115 
00116     unsigned int j = 0;
00117     for (unsigned int i = 0 ; i < steps ; ++i)
00118     {
00119         mss->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     return j;
00125 }
00126 
00127 bool ompl::base::SpaceInformation::searchValidNearby(const ValidStateSamplerPtr &sampler, State *state, const State *near, double distance) const
00128 {
00129     if (state != near)
00130         copyState(state, near);
00131 
00132     // fix bounds, if needed
00133     if (!satisfiesBounds(state))
00134         enforceBounds(state);
00135 
00136     bool result = isValid(state);
00137 
00138     if (!result)
00139     {
00140         // try to find a valid state nearby
00141         State *temp = cloneState(state);
00142         result = sampler->sampleNear(state, temp, distance);
00143         freeState(temp);
00144     }
00145 
00146     return result;
00147 }
00148 
00149 bool ompl::base::SpaceInformation::searchValidNearby(State *state, const State *near, double distance, unsigned int attempts) const
00150 {
00151     if (satisfiesBounds(near) && isValid(near))
00152     {
00153         if (state != near)
00154             copyState(state, near);
00155         return true;
00156     }
00157     else
00158     {
00159         // try to find a valid state nearby
00160         UniformValidStateSampler *uvss = new UniformValidStateSampler(this);
00161         uvss->setNrAttempts(attempts);
00162         return searchValidNearby(ValidStateSamplerPtr(uvss), state, near, distance);
00163     }
00164 }
00165 
00166 unsigned int ompl::base::SpaceInformation::getMotionStates(const State *s1, const State *s2, std::vector<State*> &states, unsigned int count, bool endpoints, bool alloc) const
00167 {
00168     // 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
00169     count++;
00170 
00171     if (count < 2)
00172     {
00173         unsigned int added = 0;
00174 
00175         // if they want endpoints, then at most endpoints are included
00176         if (endpoints)
00177         {
00178             if (alloc)
00179             {
00180                 states.resize(2);
00181                 states[0] = allocState();
00182                 states[1] = allocState();
00183             }
00184             if (states.size() > 0)
00185             {
00186                 copyState(states[0], s1);
00187                 added++;
00188             }
00189 
00190             if (states.size() > 1)
00191             {
00192                 copyState(states[1], s2);
00193                 added++;
00194             }
00195         }
00196         else
00197             if (alloc)
00198                 states.resize(0);
00199         return added;
00200     }
00201 
00202     if (alloc)
00203     {
00204         states.resize(count + (endpoints ? 1 : -1));
00205         if (endpoints)
00206             states[0] = allocState();
00207     }
00208 
00209     unsigned int added = 0;
00210 
00211     if (endpoints && states.size() > 0)
00212     {
00213         copyState(states[0], s1);
00214         added++;
00215     }
00216 
00217     /* find the states in between */
00218     for (unsigned int j = 1 ; j < count && added < states.size() ; ++j)
00219     {
00220         if (alloc)
00221             states[added] = allocState();
00222         stateManifold_->interpolate(s1, s2, (double)j / (double)count, states[added]);
00223         added++;
00224     }
00225 
00226     if (added < states.size() && endpoints)
00227     {
00228         if (alloc)
00229             states[added] = allocState();
00230         copyState(states[added], s2);
00231         added++;
00232     }
00233 
00234     return added;
00235 }
00236 
00237 
00238 bool ompl::base::SpaceInformation::checkMotion(const std::vector<State*> &states, unsigned int count, unsigned int &firstInvalidStateIndex) const
00239 {
00240     assert(states.size() >= count);
00241     for (unsigned int i = 0 ; i < count ; ++i)
00242         if (!isValid(states[i]))
00243         {
00244             firstInvalidStateIndex = i;
00245             return false;
00246         }
00247     return true;
00248 }
00249 
00250 bool ompl::base::SpaceInformation::checkMotion(const std::vector<State*> &states, unsigned int count) const
00251 {
00252     assert(states.size() >= count);
00253     if (count > 0)
00254     {
00255         if (count > 1)
00256         {
00257             if (!isValid(states.front()))
00258                 return false;
00259             if (!isValid(states[count - 1]))
00260                 return false;
00261 
00262             // we have 2 or more states, and the first and last states are valid
00263 
00264             if (count > 2)
00265             {
00266                 std::queue< std::pair<int, int> > pos;
00267                 pos.push(std::make_pair(0, count - 1));
00268 
00269                 while (!pos.empty())
00270                 {
00271                     std::pair<int, int> x = pos.front();
00272 
00273                     int mid = (x.first + x.second) / 2;
00274                     if (!isValid(states[mid]))
00275                         return false;
00276 
00277                     if (x.first < mid - 1)
00278                         pos.push(std::make_pair(x.first, mid));
00279                     if (x.second > mid + 1)
00280                         pos.push(std::make_pair(mid, x.second));
00281                 }
00282             }
00283         }
00284         else
00285             return isValid(states.front());
00286     }
00287     return true;
00288 }
00289 
00290 ompl::base::ValidStateSamplerPtr ompl::base::SpaceInformation::allocValidStateSampler(void) const
00291 {
00292     if (vssa_)
00293         return vssa_(this);
00294     else
00295         return ValidStateSamplerPtr(new UniformValidStateSampler(this));
00296 }
00297 
00298 void ompl::base::SpaceInformation::printSettings(std::ostream &out) const
00299 {
00300     out << "State space settings:" << std::endl;
00301     out << "  - dimension: " << stateManifold_->getDimension() << std::endl;
00302     out << "  - extent: " << stateManifold_->getMaximumExtent() << std::endl;
00303     out << "  - state validity check resolution: " << (getStateValidityCheckingResolution() * 100.0) << '%' << std::endl;
00304     out << "  - valid segment count factor: " << stateManifold_->getValidSegmentCountFactor() << std::endl;
00305     out << "  - state manifold:" << std::endl;
00306     stateManifold_->printSettings(out);
00307 }
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator