All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator
SpaceInformation.h
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 #ifndef OMPL_BASE_SPACE_INFORMATION_
00038 #define OMPL_BASE_SPACE_INFORMATION_
00039 
00040 #include "ompl/base/State.h"
00041 #include "ompl/base/StateValidityChecker.h"
00042 #include "ompl/base/MotionValidator.h"
00043 #include "ompl/base/StateSpace.h"
00044 #include "ompl/base/ValidStateSampler.h"
00045 
00046 #include "ompl/util/ClassForward.h"
00047 #include "ompl/util/Console.h"
00048 #include "ompl/util/Exception.h"
00049 
00050 #include <boost/noncopyable.hpp>
00051 #include <boost/function.hpp>
00052 #include <boost/bind.hpp>
00053 
00054 #include <utility>
00055 #include <cstdlib>
00056 #include <vector>
00057 #include <iostream>
00058 
00060 namespace ompl
00061 {
00062 
00067     namespace base
00068     {
00069 
00071         ClassForward(SpaceInformation);
00072 
00079         typedef boost::function1<bool, const State*> StateValidityCheckerFn;
00080 
00081 
00085         class SpaceInformation : private boost::noncopyable
00086         {
00087         public:
00088 
00090             SpaceInformation(const StateSpacePtr &space);
00091 
00092             virtual ~SpaceInformation(void)
00093             {
00094             }
00095 
00097             bool isValid(const State *state) const
00098             {
00099                 return stateValidityChecker_->isValid(state);
00100             }
00101 
00103             const StateSpacePtr& getStateSpace(void) const
00104             {
00105                 return stateSpace_;
00106             }
00107 
00112             bool equalStates(const State *state1, const State *state2) const
00113             {
00114                 return stateSpace_->equalStates(state1, state2);
00115             }
00116 
00118             bool satisfiesBounds(const State *state) const
00119             {
00120                 return stateSpace_->satisfiesBounds(state);
00121             }
00122 
00124             double distance(const State *state1, const State *state2) const
00125             {
00126                 return stateSpace_->distance(state1, state2);
00127             }
00128 
00130             void enforceBounds(State *state) const
00131             {
00132                 stateSpace_->enforceBounds(state);
00133             }
00134 
00136             void printState(const State *state, std::ostream &out = std::cout) const
00137             {
00138                 stateSpace_->printState(state, out);
00139             }
00140 
00149             void setStateValidityChecker(const StateValidityCheckerPtr &svc)
00150             {
00151                 stateValidityChecker_ = svc;
00152                 setup_ = false;
00153             }
00154 
00160             void setStateValidityChecker(const StateValidityCheckerFn &svc);
00161 
00163             const StateValidityCheckerPtr& getStateValidityChecker(void) const
00164             {
00165                 return stateValidityChecker_;
00166             }
00167 
00171             void setMotionValidator(const MotionValidatorPtr &mv)
00172             {
00173                 motionValidator_ = mv;
00174                 setup_ = false;
00175             }
00176 
00178             const MotionValidatorPtr& getMotionValidator(void) const
00179             {
00180                 return motionValidator_;
00181             }
00182 
00189             void setStateValidityCheckingResolution(double resolution)
00190             {
00191                 stateSpace_->setLongestValidSegmentFraction(resolution);
00192                 setup_ = false;
00193             }
00194 
00199             double getStateValidityCheckingResolution(void) const
00200             {
00201                 return stateSpace_->getLongestValidSegmentFraction();
00202             }
00203 
00204 
00208             unsigned int getStateDimension(void) const
00209             {
00210                 return stateSpace_->getDimension();
00211             }
00212 
00217             State* allocState(void) const
00218             {
00219                 return stateSpace_->allocState();
00220             }
00221 
00223             void allocStates(std::vector<State*> &states) const
00224             {
00225                 for (unsigned int i = 0 ; i < states.size() ; ++i)
00226                     states[i] = stateSpace_->allocState();
00227             }
00228 
00230             void freeState(State *state) const
00231             {
00232                 stateSpace_->freeState(state);
00233             }
00234 
00236             void freeStates(std::vector<State*> &states) const
00237             {
00238                 for (unsigned int i = 0 ; i < states.size() ; ++i)
00239                     stateSpace_->freeState(states[i]);
00240             }
00241 
00243             void copyState(State *destination, const State *source) const
00244             {
00245                 stateSpace_->copyState(destination, source);
00246             }
00247 
00249             State* cloneState(const State *source) const
00250             {
00251                 State *copy = stateSpace_->allocState();
00252                 stateSpace_->copyState(copy, source);
00253                 return copy;
00254             }
00255 
00263             StateSamplerPtr allocStateSampler(void) const
00264             {
00265                 return stateSpace_->allocStateSampler();
00266             }
00267 
00271             ValidStateSamplerPtr allocValidStateSampler(void) const;
00272 
00273 
00276             void setValidStateSamplerAllocator(const ValidStateSamplerAllocator &vssa)
00277             {
00278                 vssa_ = vssa;
00279                 setup_ = false;
00280             }
00281 
00290             double getMaximumExtent(void) const
00291             {
00292                 return stateSpace_->getMaximumExtent();
00293             }
00294 
00302             bool searchValidNearby(State *state, const State *near, double distance, unsigned int attempts) const;
00303 
00311             bool searchValidNearby(const ValidStateSamplerPtr &sampler, State *state, const State *near, double distance) const;
00312 
00319             unsigned int randomBounceMotion(const StateSamplerPtr &sss, const State *start, unsigned int steps, std::vector<State*> &states, bool alloc) const;
00320 
00327             bool checkMotion(const State *s1, const State *s2, std::pair<State*, double> &lastValid) const
00328             {
00329                 return motionValidator_->checkMotion(s1, s2, lastValid);
00330             }
00331 
00332 
00334             bool checkMotion(const State *s1, const State *s2) const
00335             {
00336                 return motionValidator_->checkMotion(s1, s2);
00337             }
00338 
00344             bool checkMotion(const std::vector<State*> &states, unsigned int count, unsigned int &firstInvalidStateIndex) const;
00345 
00347             bool checkMotion(const std::vector<State*> &states, unsigned int count) const;
00348 
00357             unsigned int getMotionStates(const State *s1, const State *s2, std::vector<State*> &states, unsigned int count, bool endpoints, bool alloc) const;
00358 
00365             double probabilityOfValidState(unsigned int attempts) const;
00366 
00368             double averageValidMotionLength(unsigned int attempts) const;
00369 
00373             virtual void printSettings(std::ostream &out = std::cout) const;
00374 
00376             virtual void printProperties(std::ostream &out = std::cout) const;
00377 
00382             virtual void setup(void);
00383 
00385             bool isSetup(void) const;
00386 
00387         protected:
00388 
00390             StateSpacePtr              stateSpace_;
00391 
00393             StateValidityCheckerPtr    stateValidityChecker_;
00394 
00396             MotionValidatorPtr         motionValidator_;
00397 
00399             bool                       setup_;
00400 
00402             ValidStateSamplerAllocator vssa_;
00403 
00405             msg::Interface             msg_;
00406         };
00407 
00408     }
00409 
00410 }
00411 
00412 #endif