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()
94  {
95  }
96 
98  bool isValid(const State *state) const
99  {
100  return stateValidityChecker_->isValid(state);
101  }
102 
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 
161  void setStateValidityChecker(const StateValidityCheckerFn &svc);
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() const
210  {
211  return stateSpace_->getDimension();
212  }
213 
218  State* allocState() 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() 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 
358  unsigned int getMotionStates(const State *s1, const State *s2, std::vector<State*> &states, unsigned int count, bool endpoints, bool alloc) const;
359 
366  double probabilityOfValidState(unsigned int attempts) const;
367 
369  double averageValidMotionLength(unsigned int attempts) const;
370 
372  void samplesPerSecond(double &uniform, double &near, double &gaussian, unsigned int attempts) const;
373 
375  virtual void printSettings(std::ostream &out = std::cout) const;
376 
378  virtual void printProperties(std::ostream &out = std::cout) const;
379 
382  {
383  return params_;
384  }
385 
387  const ParamSet& params() const
388  {
389  return params_;
390  }
391 
396  virtual void setup();
397 
399  bool isSetup() const;
400 
401  protected:
404 
407 
410 
413 
415  bool setup_;
416 
419 
422  };
423 
424  }
425 
426 }
427 
428 #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.
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...
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.
double getStateValidityCheckingResolution() const
Get the resolution at which state validity is verified. This call is only applicable if a ompl::base:...
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.
double getMaximumExtent() const
Get the maximum extent of the space we are planning in. This is the maximum distance that could be re...
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:234
void setDefaultMotionValidator()
Set default motion validator for the state space.
void printState(const State *state, std::ostream &out=std::cout) const
Print a state to a stream.
ParamSet & params()
Get the combined parameters for the classes that the space information manages.
A boost shared pointer wrapper for ompl::base::StateValidityChecker.
bool satisfiesBounds(const State *state) const
Check if a state is inside the bounding box.
ValidStateSamplerPtr allocValidStateSampler() const
Allocate an instance of a valid state sampler for this space. If setValidStateSamplerAllocator() was ...
StateSpacePtr stateSpace_
The state space planning is to be performed in.
Main namespace. Contains everything in this library.
Definition: Cost.h:42
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.
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.
const StateValidityCheckerPtr & getStateValidityChecker() const
Return the instance of the used state validity checker.
const ParamSet & params() const
Get the combined parameters for the classes that the space information manages.
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.
StateSamplerPtr allocStateSampler() const
Allocate a uniform state sampler for the state space.
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...
virtual void setup()
Perform additional setup tasks (run once, before use). If state validity checking resolution has not ...
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.
const StateSpacePtr & getStateSpace() const
Return the instance of the used state space.
void freeState(State *state) const
Free the memory of a state.
unsigned int getStateDimension() const
Return the dimension of the state space.
const MotionValidatorPtr & getMotionValidator() const
Return the instance of the used state validity checker.
void enforceBounds(State *state) const
Bring the state within the bounds of the state space.
void clearValidStateSamplerAllocator()
Clear the allocator used for the valid state sampler. This will revert to using the uniform valid sta...
ParamSet params_
Combined parameters for the contained classes.
ValidStateSamplerAllocator vssa_
The optional valid state sampler allocator.
bool isSetup() const
Return true if setup was called.
void setMotionValidator(const MotionValidatorPtr &mv)
Set the instance of the motion validity checker to use. Parallel implementations of planners assume t...
State * allocState() const
Allocate memory for a state.