All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
SpaceInformation.h
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 <boost/noncopyable.hpp>
51 #include <boost/function.hpp>
52 #include <boost/bind.hpp>
53 
54 #include <utility>
55 #include <cstdlib>
56 #include <vector>
57 #include <iostream>
58 
60 namespace ompl
61 {
62 
67  namespace base
68  {
70 
71  OMPL_CLASS_FORWARD(SpaceInformation);
73 
80  typedef boost::function<bool(const State*)> StateValidityCheckerFn;
81 
82 
86  class SpaceInformation : private boost::noncopyable
87  {
88  public:
89 
91  SpaceInformation(const StateSpacePtr &space);
92 
93  virtual ~SpaceInformation(void)
94  {
95  }
96 
98  bool isValid(const State *state) const
99  {
100  return stateValidityChecker_->isValid(state);
101  }
102 
104  const StateSpacePtr& getStateSpace(void) const
105  {
106  return stateSpace_;
107  }
108 
113  bool equalStates(const State *state1, const State *state2) const
114  {
115  return stateSpace_->equalStates(state1, state2);
116  }
117 
119  bool satisfiesBounds(const State *state) const
120  {
121  return stateSpace_->satisfiesBounds(state);
122  }
123 
125  double distance(const State *state1, const State *state2) const
126  {
127  return stateSpace_->distance(state1, state2);
128  }
129 
131  void enforceBounds(State *state) const
132  {
133  stateSpace_->enforceBounds(state);
134  }
135 
137  void printState(const State *state, std::ostream &out = std::cout) const
138  {
139  stateSpace_->printState(state, out);
140  }
141 
151  {
152  stateValidityChecker_ = svc;
153  setup_ = false;
154  }
155 
162 
165  {
166  return stateValidityChecker_;
167  }
168 
173  {
174  motionValidator_ = mv;
175  setup_ = false;
176  }
177 
180  {
181  return motionValidator_;
182  }
183 
190  void setStateValidityCheckingResolution(double resolution)
191  {
192  stateSpace_->setLongestValidSegmentFraction(resolution);
193  setup_ = false;
194  }
195 
201  {
202  return stateSpace_->getLongestValidSegmentFraction();
203  }
204 
205 
209  unsigned int getStateDimension(void) const
210  {
211  return stateSpace_->getDimension();
212  }
213 
218  State* allocState(void) const
219  {
220  return stateSpace_->allocState();
221  }
222 
224  void allocStates(std::vector<State*> &states) const
225  {
226  for (unsigned int i = 0 ; i < states.size() ; ++i)
227  states[i] = stateSpace_->allocState();
228  }
229 
231  void freeState(State *state) const
232  {
233  stateSpace_->freeState(state);
234  }
235 
237  void freeStates(std::vector<State*> &states) const
238  {
239  for (unsigned int i = 0 ; i < states.size() ; ++i)
240  stateSpace_->freeState(states[i]);
241  }
242 
244  void copyState(State *destination, const State *source) const
245  {
246  stateSpace_->copyState(destination, source);
247  }
248 
250  State* cloneState(const State *source) const
251  {
252  State *copy = stateSpace_->allocState();
253  stateSpace_->copyState(copy, source);
254  return copy;
255  }
256 
265  {
266  return stateSpace_->allocStateSampler();
267  }
268 
273 
277 
280 
289  double getMaximumExtent(void) const
290  {
291  return stateSpace_->getMaximumExtent();
292  }
293 
301  bool searchValidNearby(State *state, const State *near, double distance, unsigned int attempts) const;
302 
310  bool searchValidNearby(const ValidStateSamplerPtr &sampler, State *state, const State *near, double distance) const;
311 
318  unsigned int randomBounceMotion(const StateSamplerPtr &sss, const State *start, unsigned int steps, std::vector<State*> &states, bool alloc) const;
319 
326  bool checkMotion(const State *s1, const State *s2, std::pair<State*, double> &lastValid) const
327  {
328  return motionValidator_->checkMotion(s1, s2, lastValid);
329  }
330 
331 
333  bool checkMotion(const State *s1, const State *s2) const
334  {
335  return motionValidator_->checkMotion(s1, s2);
336  }
337 
343  bool checkMotion(const std::vector<State*> &states, unsigned int count, unsigned int &firstInvalidStateIndex) const;
344 
346  bool checkMotion(const std::vector<State*> &states, unsigned int count) const;
347 
356  unsigned int getMotionStates(const State *s1, const State *s2, std::vector<State*> &states, unsigned int count, bool endpoints, bool alloc) const;
357 
364  double probabilityOfValidState(unsigned int attempts) const;
365 
367  double averageValidMotionLength(unsigned int attempts) const;
368 
370  void samplesPerSecond(double &uniform, double &near, double &gaussian, unsigned int attempts) const;
371 
373  virtual void printSettings(std::ostream &out = std::cout) const;
374 
376  virtual void printProperties(std::ostream &out = std::cout) const;
377 
380  {
381  return params_;
382  }
383 
385  const ParamSet& params(void) const
386  {
387  return params_;
388  }
389 
394  virtual void setup(void);
395 
397  bool isSetup(void) const;
398 
399  protected:
401  void setDefaultMotionValidator(void);
402 
405 
408 
411 
413  bool setup_;
414 
417 
420  };
421 
422  }
423 
424 }
425 
426 #endif
double distance(const State *state1, const State *state2) const
Compute the distance between two states.
boost::function< ValidStateSamplerPtr(const SpaceInformation *)> ValidStateSamplerAllocator
Definition of a function that can allocate a valid state sampler.
ValidStateSamplerPtr allocValidStateSampler(void) const
Allocate an instance of a valid state sampler for this space. If setValidStateSamplerAllocator() was ...
bool isSetup(void) const
Return true if setup was called.
bool searchValidNearby(State *state, const State *near, double distance, unsigned int attempts) const
Find a valid state near a given one. If the given state is valid, it will be returned itself...
const StateSpacePtr & getStateSpace(void) const
Return the instance of the used state space.
void setStateValidityCheckingResolution(double resolution)
Set the resolution at which state validity needs to be verified in order for a motion between two sta...
A boost shared pointer wrapper for ompl::base::ValidStateSampler.
void allocStates(std::vector< State * > &states) const
Allocate memory for each element of the array states.
A boost shared pointer wrapper for ompl::base::StateSpace.
MotionValidatorPtr motionValidator_
The instance of the motion validator to use when determining the validity of motions in the planning ...
A boost shared pointer wrapper for ompl::base::StateSampler.
virtual void printSettings(std::ostream &out=std::cout) const
Print information about the current instance of the state space.
void setStateValidityChecker(const StateValidityCheckerPtr &svc)
Set the instance of the state validity checker to use. Parallel implementations of planners assume th...
bool isValid(const State *state) const
Check if a given state is valid or not.
unsigned int getStateDimension(void) const
Return the dimension of the state space.
double averageValidMotionLength(unsigned int attempts) const
Estimate the length of a valid motion. setup() is assumed to have been called.
boost::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a boost function can be spec...
State * cloneState(const State *source) const
Clone a state.
unsigned int randomBounceMotion(const StateSamplerPtr &sss, const State *start, unsigned int steps, std::vector< State * > &states, bool alloc) const
Produce a valid motion starting at start by randomly bouncing off of invalid states. The start state start is not included in the computed motion (states). Returns the number of elements written to states (less or equal to steps).
Maintain a set of parameters.
Definition: GenericParam.h:216
void printState(const State *state, std::ostream &out=std::cout) const
Print a state to a stream.
A boost shared pointer wrapper for ompl::base::StateValidityChecker.
StateSamplerPtr allocStateSampler(void) const
Allocate a uniform state sampler for the state space.
bool satisfiesBounds(const State *state) const
Check if a state is inside the bounding box.
double getStateValidityCheckingResolution(void) const
Get the resolution at which state validity is verified. This call is only applicable if a ompl::base:...
StateSpacePtr stateSpace_
The state space planning is to be performed in.
void setValidStateSamplerAllocator(const ValidStateSamplerAllocator &vssa)
Set the allocator to use for a valid state sampler. This replaces the default uniform valid state sam...
void copyState(State *destination, const State *source) const
Copy a state to another.
virtual void printProperties(std::ostream &out=std::cout) const
Print properties of the current instance of the state space.
double getMaximumExtent(void) const
Get the maximum extent of the space we are planning in. This is the maximum distance that could be re...
bool checkMotion(const State *s1, const State *s2, std::pair< State *, double > &lastValid) const
Incrementally check if the path between two motions is valid. Also compute the last state that was va...
A boost shared pointer wrapper for ompl::base::MotionValidator.
bool setup_
Flag indicating whether setup() has been called on this instance.
void samplesPerSecond(double &uniform, double &near, double &gaussian, unsigned int attempts) const
Estimate the number of samples that can be drawn per second, using the sampler returned by allocState...
void freeStates(std::vector< State * > &states) const
Free the memory of an array of states.
virtual void setup(void)
Perform additional setup tasks (run once, before use). If state validity checking resolution has not ...
double probabilityOfValidState(unsigned int attempts) const
Estimate probability of sampling a valid state. setup() is assumed to have been called.
StateValidityCheckerPtr stateValidityChecker_
The instance of the state validity checker used for determining the validity of states in the plannin...
The base class for space information. This contains all the information about the space planning is d...
const StateValidityCheckerPtr & getStateValidityChecker(void) const
Return the instance of the used state validity checker.
Definition of an abstract state.
Definition: State.h:50
bool checkMotion(const State *s1, const State *s2) const
Check if the path between two states (from s1 to s2) is valid, using subdivision. This function assum...
bool equalStates(const State *state1, const State *state2) const
Check if two states are the same.
unsigned int getMotionStates(const State *s1, const State *s2, std::vector< State * > &states, unsigned int count, bool endpoints, bool alloc) const
Get count states that make up a motion between s1 and s2. Returns the number of states that were adde...
SpaceInformation(const StateSpacePtr &space)
Constructor. Sets the instance of the state space to plan with.
void freeState(State *state) const
Free the memory of a state.
ParamSet & params(void)
Get the combined parameters for the classes that the space information manages.
void enforceBounds(State *state) const
Bring the state within the bounds of the state space.
ParamSet params_
Combined parameters for the contained classes.
ValidStateSamplerAllocator vssa_
The optional valid state sampler allocator.
void setMotionValidator(const MotionValidatorPtr &mv)
Set the instance of the motion validity checker to use. Parallel implementations of planners assume t...
const ParamSet & params(void) const
Get the combined parameters for the classes that the space information manages.
void clearValidStateSamplerAllocator(void)
Clear the allocator used for the valid state sampler. This will revert to using the uniform valid sta...
const MotionValidatorPtr & getMotionValidator(void) const
Return the instance of the used state validity checker.
void setDefaultMotionValidator(void)
Set default motion validator for the state space.
State * allocState(void) const
Allocate memory for a state.