1 /********************************************************************* 2 * Software License Agreement (BSD License) 3 * 4 * Copyright (c) 2010, Rice University 5 * All rights reserved. 6 * 7 * Redistribution and use in source and binary forms, with or without 8 * modification, are permitted provided that the following conditions 9 * are met: 10 * 11 * * Redistributions of source code must retain the above copyright 12 * notice, this list of conditions and the following disclaimer. 13 * * Redistributions in binary form must reproduce the above 14 * copyright notice, this list of conditions and the following 15 * disclaimer in the documentation and/or other materials provided 16 * with the distribution. 17 * * Neither the name of the Rice University nor the names of its 18 * contributors may be used to endorse or promote products derived 19 * from this software without specific prior written permission. 20 * 21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 * POSSIBILITY OF SUCH DAMAGE. 33 *********************************************************************/ 34 35 /* Author: Ioan Sucan */ 36 37 #ifndef OMPL_BASE_SPACE_INFORMATION_ 38 #define OMPL_BASE_SPACE_INFORMATION_ 39 40 #include "ompl/base/State.h" 41 #include "ompl/base/StateValidityChecker.h" 42 #include "ompl/base/MotionValidator.h" 43 #include "ompl/base/StateSpace.h" 44 #include "ompl/base/ValidStateSampler.h" 45 46 #include "ompl/util/ClassForward.h" 47 #include "ompl/util/Console.h" 48 #include "ompl/util/Exception.h" 49 50 #include <functional> 51 #include <utility> 52 #include <cstdlib> 53 #include <vector> 54 #include <iostream> 55 56 /** \brief Main namespace. Contains everything in this library */ 57 namespace ompl 58 { 59 /** \brief This namespace contains sampling based planning 60 routines shared by both planning under geometric constraints 61 (geometric) and planning under differential constraints 62 (dynamic) */ 63 namespace base 64 { 65 /// @cond IGNORE 66 /** \brief Forward declaration of ompl::base::SpaceInformation */ 67 OMPL_CLASS_FORWARD(SpaceInformation); 68 /// @endcond 69 70 /** \class ompl::base::SpaceInformationPtr 71 \brief A shared pointer wrapper for ompl::base::SpaceInformation */ 72 73 /** \brief If no state validity checking class is specified 74 (StateValidityChecker), a std::function can be specified 75 instead */ 76 using StateValidityCheckerFn = std::function<bool(const State *)>; 77 78 /** \brief The base class for space information. This contains 79 all the information about the space planning is done in. 80 setup() needs to be called as well, before use */ 81 class SpaceInformation 82 { 83 public: 84 // non-copyable 85 SpaceInformation(const SpaceInformation &) = delete; 86 SpaceInformation &operator=(const SpaceInformation &) = delete; 87 88 /** \brief Constructor. Sets the instance of the state space to plan with. */ 89 SpaceInformation(StateSpacePtr space); 90 91 virtual ~SpaceInformation() = default; 92 93 /** \brief Check if a given state is valid or not */ isValid(const State * state)94 bool isValid(const State *state) const 95 { 96 return stateValidityChecker_->isValid(state); 97 } 98 99 /** \brief Return the instance of the used state space */ getStateSpace()100 const StateSpacePtr &getStateSpace() const 101 { 102 return stateSpace_; 103 } 104 105 /** @name Topology-specific state operations (as in the state space) 106 @{ */ 107 108 /** \brief Check if two states are the same */ equalStates(const State * state1,const State * state2)109 bool equalStates(const State *state1, const State *state2) const 110 { 111 return stateSpace_->equalStates(state1, state2); 112 } 113 114 /** \brief Check if a state is inside the bounding box */ satisfiesBounds(const State * state)115 bool satisfiesBounds(const State *state) const 116 { 117 return stateSpace_->satisfiesBounds(state); 118 } 119 120 /** \brief Compute the distance between two states */ distance(const State * state1,const State * state2)121 double distance(const State *state1, const State *state2) const 122 { 123 return stateSpace_->distance(state1, state2); 124 } 125 126 /** \brief Bring the state within the bounds of the state space */ enforceBounds(State * state)127 void enforceBounds(State *state) const 128 { 129 stateSpace_->enforceBounds(state); 130 } 131 132 /** \brief Print a state to a stream */ 133 void printState(const State *state, std::ostream &out = std::cout) const 134 { 135 stateSpace_->printState(state, out); 136 } 137 138 /** @} */ 139 140 /** @name Configuration of state validity checking 141 @{ */ 142 143 /** \brief Set the instance of the state validity checker 144 to use. Parallel implementations of planners assume 145 this validity checker is thread safe. */ setStateValidityChecker(const StateValidityCheckerPtr & svc)146 void setStateValidityChecker(const StateValidityCheckerPtr &svc) 147 { 148 stateValidityChecker_ = svc; 149 setup_ = false; 150 } 151 152 /** \brief If no state validity checking class is 153 specified (StateValidityChecker), a function can 154 be specified instead. This version however incurs a 155 small additional overhead when calling the function, 156 since there is one more level of indirection */ 157 void setStateValidityChecker(const StateValidityCheckerFn &svc); 158 159 /** \brief Return the instance of the used state validity checker */ getStateValidityChecker()160 const StateValidityCheckerPtr &getStateValidityChecker() const 161 { 162 return stateValidityChecker_; 163 } 164 165 /** \brief Set the instance of the motion validity checker 166 to use. Parallel implementations of planners assume 167 this validity checker is thread safe. */ setMotionValidator(const MotionValidatorPtr & mv)168 void setMotionValidator(const MotionValidatorPtr &mv) 169 { 170 motionValidator_ = mv; 171 setup_ = false; 172 } 173 174 /** \brief Return the instance of the used state validity checker */ getMotionValidator()175 const MotionValidatorPtr &getMotionValidator() const 176 { 177 return motionValidator_; 178 } 179 180 /** \brief Return the non-const instance of the used state validity checker */ getMotionValidator()181 MotionValidatorPtr& getMotionValidator() 182 { 183 return motionValidator_; 184 } 185 186 /** \brief Set the resolution at which state validity 187 needs to be verified in order for a motion between two 188 states to be considered valid. This value is specified 189 as a fraction of the space's extent. This call is only 190 applicable if a ompl::base::DiscreteMotionValidator is 191 used. See \ref stateValidation. */ setStateValidityCheckingResolution(double resolution)192 void setStateValidityCheckingResolution(double resolution) 193 { 194 stateSpace_->setLongestValidSegmentFraction(resolution); 195 setup_ = false; 196 } 197 198 /** \brief Get the resolution at which state validity is 199 verified. This call is only applicable if a 200 ompl::base::DiscreteMotionValidator is used. See \ref 201 stateValidation. */ getStateValidityCheckingResolution()202 double getStateValidityCheckingResolution() const 203 { 204 return stateSpace_->getLongestValidSegmentFraction(); 205 } 206 207 /** @}*/ 208 209 /** \brief Return the dimension of the state space */ getStateDimension()210 unsigned int getStateDimension() const 211 { 212 return stateSpace_->getDimension(); 213 } 214 215 /** \brief Get a measure of the space (this can be thought of as a generalization of volume) */ getSpaceMeasure()216 double getSpaceMeasure() const 217 { 218 return stateSpace_->getMeasure(); 219 } 220 221 /** @name State memory management 222 @{ */ 223 224 /** \brief Allocate memory for a state */ allocState()225 State *allocState() const 226 { 227 return stateSpace_->allocState(); 228 } 229 230 /** \brief Allocate memory for each element of the array \e states */ allocStates(std::vector<State * > & states)231 void allocStates(std::vector<State *> &states) const 232 { 233 for (auto &state : states) 234 state = stateSpace_->allocState(); 235 } 236 237 /** \brief Free the memory of a state */ freeState(State * state)238 void freeState(State *state) const 239 { 240 stateSpace_->freeState(state); 241 } 242 243 /** \brief Free the memory of an array of states */ freeStates(std::vector<State * > & states)244 void freeStates(std::vector<State *> &states) const 245 { 246 for (auto &state : states) 247 stateSpace_->freeState(state); 248 } 249 250 /** \brief Copy a state to another */ copyState(State * destination,const State * source)251 void copyState(State *destination, const State *source) const 252 { 253 stateSpace_->copyState(destination, source); 254 } 255 256 /** \brief Clone a state */ cloneState(const State * source)257 State *cloneState(const State *source) const 258 { 259 return stateSpace_->cloneState(source); 260 } 261 262 /** @} */ 263 264 /** @name Sampling of valid states 265 @{ */ 266 267 /** \brief Allocate a uniform state sampler for the state space */ allocStateSampler()268 StateSamplerPtr allocStateSampler() const 269 { 270 return stateSpace_->allocStateSampler(); 271 } 272 273 /** \brief Allocate an instance of a valid state sampler for this space. If setValidStateSamplerAllocator() 274 was previously called, 275 the specified allocator is used to produce the state sampler. Otherwise, a 276 ompl::base::UniformValidStateSampler() is 277 allocated. */ 278 ValidStateSamplerPtr allocValidStateSampler() const; 279 280 /** \brief Set the allocator to use for a valid state sampler. This replaces the default uniform valid state 281 sampler. This call can be made at any time, but it should not be changed while 282 ompl::base::Planner::solve() is executing */ 283 void setValidStateSamplerAllocator(const ValidStateSamplerAllocator &vssa); 284 285 /** \brief Clear the allocator used for the valid state sampler. This will revert to using the uniform valid 286 * state sampler (the default). */ 287 void clearValidStateSamplerAllocator(); 288 289 /** @}*/ 290 291 /** @name Primitives typically used by motion planners 292 @{ */ 293 294 /** \brief Get the maximum extent of the space we are 295 planning in. This is the maximum distance that could 296 be reported between any two given states */ getMaximumExtent()297 double getMaximumExtent() const 298 { 299 return stateSpace_->getMaximumExtent(); 300 } 301 302 /** \brief Find a valid state near a given one. If the given state is valid, it will be returned itself. 303 * The two passed state pointers need not point to different memory. Returns true on success. 304 * \param state the location at which to store the valid state, if one is found. This location may be 305 * modified even if no valid state is found. 306 * \param near a state that may be invalid near which we would like to find a valid state 307 * \param distance the maximum allowed distance between \e state and \e near 308 * \param attempts the algorithm works by sampling states near state \e near. This parameter defines the 309 * maximum number of sampling attempts 310 */ 311 bool searchValidNearby(State *state, const State *near, double distance, unsigned int attempts) const; 312 313 /** \brief Find a valid state near a given one. If the given state is valid, it will be returned itself. 314 * The two passed state pointers need not point to different memory. Returns true on success. 315 * \param sampler the valid state sampler to use when attemting to find a valid sample. 316 * \param state the location at which to store the valid state, if one is found. This location may be 317 * modified even if no valid state is found. 318 * \param near a state that may be invalid near which we would like to find a valid state 319 * \param distance the maximum allowed distance between \e state and \e near 320 */ 321 bool searchValidNearby(const ValidStateSamplerPtr &sampler, State *state, const State *near, 322 double distance) const; 323 324 /** \brief Produce a valid motion starting at \e start by randomly bouncing off of invalid states. The start 325 * state \e start is not included in the computed motion (\e states). Returns the number of elements written 326 * to \e states (less or equal to \e steps). 327 * \param sss the state space sampler to use 328 * \param start the state at which to start bouncing 329 * \param steps the number of bouncing steps to take 330 * \param states the location at which generated states will be stored 331 * \param alloc flag indicating whether memory should be allocated for \e states */ 332 unsigned int randomBounceMotion(const StateSamplerPtr &sss, const State *start, unsigned int steps, 333 std::vector<State *> &states, bool alloc) const; 334 335 /** \brief Incrementally check if the path between two motions is valid. Also compute the last state that 336 was 337 valid and the time of that state. The time is used to parametrize the motion from s1 to s2, s1 being at 338 t = 339 0 and s2 being at t = 1. This function assumes s1 is valid. 340 \param s1 start state of the motion to be checked (assumed to be valid) 341 \param s2 final state of the motion to be checked 342 \param lastValid first: storage for the last valid state (may be nullptr); this need not be different 343 from \e s1 or \e s2. second: the time (between 0 and 1) of the last valid state, on the motion from \e 344 s1 to \e s2 */ checkMotion(const State * s1,const State * s2,std::pair<State *,double> & lastValid)345 virtual bool checkMotion(const State *s1, const State *s2, std::pair<State *, double> &lastValid) const 346 { 347 return motionValidator_->checkMotion(s1, s2, lastValid); 348 } 349 350 /** \brief Check if the path between two states (from \e s1 to \e s2) is valid, using the MotionValidator. 351 * This function assumes \e s1 is valid. */ checkMotion(const State * s1,const State * s2)352 virtual bool checkMotion(const State *s1, const State *s2) const 353 { 354 return motionValidator_->checkMotion(s1, s2); 355 } 356 357 /** \brief Incrementally check if a sequence of states is valid. Given a vector of states, this routine only 358 checks the first \e count elements and marks the index of the first invalid state 359 \param states the array of states to be checked 360 \param count the number of states to be checked in the array (0 to \e count) 361 \param firstInvalidStateIndex location to store the first invalid state index. Unmodified if the 362 function returns true */ 363 bool checkMotion(const std::vector<State *> &states, unsigned int count, 364 unsigned int &firstInvalidStateIndex) const; 365 366 /** \brief Check if a sequence of states is valid using subdivision. */ 367 bool checkMotion(const std::vector<State *> &states, unsigned int count) const; 368 369 /** \brief Get \e count states that make up a motion between \e s1 and \e s2. Returns the number of states 370 that were added to \e states. These states are not checked for validity. 371 If \e states.size() >= count or \e alloc is true, the returned value is equal to \e count (or \e count + 372 2, if \e endpoints is true). 373 Otherwise, fewer states can be returned. 374 \param s1 the start state of the considered motion 375 \param s2 the end state of the considered motion 376 \param states the computed set of states along the specified motion 377 \param count the number of intermediate states to compute 378 \param endpoints flag indicating whether \e s1 and \e s2 are to be included in states 379 \param alloc flag indicating whether memory is to be allocated automatically 380 */ 381 virtual unsigned int getMotionStates(const State *s1, const State *s2, std::vector<State *> &states, 382 unsigned int count, bool endpoints, bool alloc) const; 383 384 /** \brief Get the total number of motion segments checked by the MotionValidator so far */ getCheckedMotionCount()385 unsigned int getCheckedMotionCount() const 386 { 387 return motionValidator_->getCheckedMotionCount(); 388 } 389 390 /** @}*/ 391 392 /** @name Routines for inferring information about the state space 393 @{ */ 394 395 /** \brief Estimate probability of sampling a valid state. setup() is assumed to have been called. */ 396 double probabilityOfValidState(unsigned int attempts) const; 397 398 /** \brief Estimate the length of a valid motion. setup() is assumed to have been called.*/ 399 double averageValidMotionLength(unsigned int attempts) const; 400 401 /** \brief Estimate the number of samples that can be drawn per second, using the sampler returned by 402 * allocStateSampler() */ 403 void samplesPerSecond(double &uniform, double &near, double &gaussian, unsigned int attempts) const; 404 405 /** \brief Print information about the current instance of the state space */ 406 virtual void printSettings(std::ostream &out = std::cout) const; 407 408 /** \brief Print properties of the current instance of the state space */ 409 virtual void printProperties(std::ostream &out = std::cout) const; 410 411 /** \brief Get the combined parameters for the classes that the space information manages */ params()412 ParamSet ¶ms() 413 { 414 return params_; 415 } 416 417 /** \brief Get the combined parameters for the classes that the space information manages */ params()418 const ParamSet ¶ms() const 419 { 420 return params_; 421 } 422 423 /** \brief Perform additional setup tasks (run once, 424 before use). If state validity checking resolution has 425 not been set, estimateMaxResolution() is called to 426 estimate it. */ 427 virtual void setup(); 428 429 /** \brief Return true if setup was called */ 430 bool isSetup() const; 431 432 protected: 433 /** \brief Set default motion validator for the state space */ 434 void setDefaultMotionValidator(); 435 436 /** \brief The state space planning is to be performed in */ 437 StateSpacePtr stateSpace_; 438 439 /** \brief The instance of the state validity checker used for determining the validity of states in the 440 * planning process */ 441 StateValidityCheckerPtr stateValidityChecker_; 442 443 /** \brief The instance of the motion validator to use when determining the validity of motions in the 444 * planning process */ 445 MotionValidatorPtr motionValidator_; 446 447 /** \brief Flag indicating whether setup() has been called on this instance */ 448 bool setup_; 449 450 /** \brief The optional valid state sampler allocator */ 451 ValidStateSamplerAllocator vssa_; 452 453 /** \brief Combined parameters for the contained classes */ 454 ParamSet params_; 455 }; 456 } 457 } 458 459 #endif 460