ompl/base/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     {
00070 
00071         OMPL_CLASS_FORWARD(SpaceInformation);
00073 
00080         typedef boost::function<bool(const State*)> StateValidityCheckerFn;
00081 
00082 
00086         class SpaceInformation : private boost::noncopyable
00087         {
00088         public:
00089 
00091             SpaceInformation(const StateSpacePtr &space);
00092 
00093             virtual ~SpaceInformation()
00094             {
00095             }
00096 
00098             bool isValid(const State *state) const
00099             {
00100                 return stateValidityChecker_->isValid(state);
00101             }
00102 
00104             const StateSpacePtr& getStateSpace() const
00105             {
00106                 return stateSpace_;
00107             }
00108 
00113             bool equalStates(const State *state1, const State *state2) const
00114             {
00115                 return stateSpace_->equalStates(state1, state2);
00116             }
00117 
00119             bool satisfiesBounds(const State *state) const
00120             {
00121                 return stateSpace_->satisfiesBounds(state);
00122             }
00123 
00125             double distance(const State *state1, const State *state2) const
00126             {
00127                 return stateSpace_->distance(state1, state2);
00128             }
00129 
00131             void enforceBounds(State *state) const
00132             {
00133                 stateSpace_->enforceBounds(state);
00134             }
00135 
00137             void printState(const State *state, std::ostream &out = std::cout) const
00138             {
00139                 stateSpace_->printState(state, out);
00140             }
00141 
00150             void setStateValidityChecker(const StateValidityCheckerPtr &svc)
00151             {
00152                 stateValidityChecker_ = svc;
00153                 setup_ = false;
00154             }
00155 
00161             void setStateValidityChecker(const StateValidityCheckerFn &svc);
00162 
00164             const StateValidityCheckerPtr& getStateValidityChecker() const
00165             {
00166                 return stateValidityChecker_;
00167             }
00168 
00172             void setMotionValidator(const MotionValidatorPtr &mv)
00173             {
00174                 motionValidator_ = mv;
00175                 setup_ = false;
00176             }
00177 
00179             const MotionValidatorPtr& getMotionValidator() const
00180             {
00181                 return motionValidator_;
00182             }
00183 
00190             void setStateValidityCheckingResolution(double resolution)
00191             {
00192                 stateSpace_->setLongestValidSegmentFraction(resolution);
00193                 setup_ = false;
00194             }
00195 
00200             double getStateValidityCheckingResolution() const
00201             {
00202                 return stateSpace_->getLongestValidSegmentFraction();
00203             }
00204 
00205 
00209             unsigned int getStateDimension() const
00210             {
00211                 return stateSpace_->getDimension();
00212             }
00213 
00218             State* allocState() const
00219             {
00220                 return stateSpace_->allocState();
00221             }
00222 
00224             void allocStates(std::vector<State*> &states) const
00225             {
00226                 for (unsigned int i = 0 ; i < states.size() ; ++i)
00227                     states[i] = stateSpace_->allocState();
00228             }
00229 
00231             void freeState(State *state) const
00232             {
00233                 stateSpace_->freeState(state);
00234             }
00235 
00237             void freeStates(std::vector<State*> &states) const
00238             {
00239                 for (unsigned int i = 0 ; i < states.size() ; ++i)
00240                     stateSpace_->freeState(states[i]);
00241             }
00242 
00244             void copyState(State *destination, const State *source) const
00245             {
00246                 stateSpace_->copyState(destination, source);
00247             }
00248 
00250             State* cloneState(const State *source) const
00251             {
00252                 State *copy = stateSpace_->allocState();
00253                 stateSpace_->copyState(copy, source);
00254                 return copy;
00255             }
00256 
00264             StateSamplerPtr allocStateSampler() const
00265             {
00266                 return stateSpace_->allocStateSampler();
00267             }
00268 
00272             ValidStateSamplerPtr allocValidStateSampler() const;
00273 
00276             void setValidStateSamplerAllocator(const ValidStateSamplerAllocator &vssa);
00277 
00279             void clearValidStateSamplerAllocator();
00280 
00289             double getMaximumExtent() const
00290             {
00291                 return stateSpace_->getMaximumExtent();
00292             }
00293 
00301             bool searchValidNearby(State *state, const State *near, double distance, unsigned int attempts) const;
00302 
00310             bool searchValidNearby(const ValidStateSamplerPtr &sampler, State *state, const State *near, double distance) const;
00311 
00318             unsigned int randomBounceMotion(const StateSamplerPtr &sss, const State *start, unsigned int steps, std::vector<State*> &states, bool alloc) const;
00319 
00326             bool checkMotion(const State *s1, const State *s2, std::pair<State*, double> &lastValid) const
00327             {
00328                 return motionValidator_->checkMotion(s1, s2, lastValid);
00329             }
00330 
00331 
00333             bool checkMotion(const State *s1, const State *s2) const
00334             {
00335                 return motionValidator_->checkMotion(s1, s2);
00336             }
00337 
00343             bool checkMotion(const std::vector<State*> &states, unsigned int count, unsigned int &firstInvalidStateIndex) const;
00344 
00346             bool checkMotion(const std::vector<State*> &states, unsigned int count) const;
00347 
00358             unsigned int getMotionStates(const State *s1, const State *s2, std::vector<State*> &states, unsigned int count, bool endpoints, bool alloc) const;
00359 
00366             double probabilityOfValidState(unsigned int attempts) const;
00367 
00369             double averageValidMotionLength(unsigned int attempts) const;
00370 
00372             void samplesPerSecond(double &uniform, double &near, double &gaussian, unsigned int attempts) const;
00373 
00375             virtual void printSettings(std::ostream &out = std::cout) const;
00376 
00378             virtual void printProperties(std::ostream &out = std::cout) const;
00379 
00381             ParamSet& params()
00382             {
00383                 return params_;
00384             }
00385 
00387             const ParamSet& params() const
00388             {
00389                 return params_;
00390             }
00391 
00396             virtual void setup();
00397 
00399             bool isSetup() const;
00400 
00401         protected:
00403             void setDefaultMotionValidator();
00404 
00406             StateSpacePtr              stateSpace_;
00407 
00409             StateValidityCheckerPtr    stateValidityChecker_;
00410 
00412             MotionValidatorPtr         motionValidator_;
00413 
00415             bool                       setup_;
00416 
00418             ValidStateSamplerAllocator vssa_;
00419 
00421             ParamSet                   params_;
00422         };
00423 
00424     }
00425 
00426 }
00427 
00428 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines