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 <functional>
51 #include <utility>
52 #include <cstdlib>
53 #include <vector>
54 #include <iostream>
55 
57 namespace ompl
58 {
63  namespace base
64  {
66 
67  OMPL_CLASS_FORWARD(SpaceInformation);
69 
76  typedef std::function<bool(const State *)> StateValidityCheckerFn;
77 
82  {
83  public:
84  // non-copyable
85  SpaceInformation(const SpaceInformation &) = delete;
86  SpaceInformation &operator=(const SpaceInformation &) = delete;
87 
90 
91  virtual ~SpaceInformation() = default;
92 
94  bool isValid(const State *state) const
95  {
96  return stateValidityChecker_->isValid(state);
97  }
98 
101  {
102  return stateSpace_;
103  }
104 
109  bool equalStates(const State *state1, const State *state2) const
110  {
111  return stateSpace_->equalStates(state1, state2);
112  }
113 
115  bool satisfiesBounds(const State *state) const
116  {
117  return stateSpace_->satisfiesBounds(state);
118  }
119 
121  double distance(const State *state1, const State *state2) const
122  {
123  return stateSpace_->distance(state1, state2);
124  }
125 
127  void enforceBounds(State *state) const
128  {
129  stateSpace_->enforceBounds(state);
130  }
131 
133  void printState(const State *state, std::ostream &out = std::cout) const
134  {
135  stateSpace_->printState(state, out);
136  }
137 
147  {
148  stateValidityChecker_ = svc;
149  setup_ = false;
150  }
151 
157  void setStateValidityChecker(const StateValidityCheckerFn &svc);
158 
161  {
162  return stateValidityChecker_;
163  }
164 
169  {
170  motionValidator_ = mv;
171  setup_ = false;
172  }
173 
176  {
177  return motionValidator_;
178  }
179 
182  {
183  return motionValidator_;
184  }
185 
192  void setStateValidityCheckingResolution(double resolution)
193  {
194  stateSpace_->setLongestValidSegmentFraction(resolution);
195  setup_ = false;
196  }
197 
203  {
204  return stateSpace_->getLongestValidSegmentFraction();
205  }
206 
210  unsigned int getStateDimension() const
211  {
212  return stateSpace_->getDimension();
213  }
214 
216  double getSpaceMeasure() const
217  {
218  return stateSpace_->getMeasure();
219  }
220 
225  State *allocState() const
226  {
227  return stateSpace_->allocState();
228  }
229 
231  void allocStates(std::vector<State *> &states) const
232  {
233  for (auto &state : states)
234  state = stateSpace_->allocState();
235  }
236 
238  void freeState(State *state) const
239  {
240  stateSpace_->freeState(state);
241  }
242 
244  void freeStates(std::vector<State *> &states) const
245  {
246  for (auto &state : states)
247  stateSpace_->freeState(state);
248  }
249 
251  void copyState(State *destination, const State *source) const
252  {
253  stateSpace_->copyState(destination, source);
254  }
255 
257  State *cloneState(const State *source) const
258  {
259  return stateSpace_->cloneState(source);
260  }
261 
269  {
270  return stateSpace_->allocStateSampler();
271  }
272 
279 
284 
288 
297  double getMaximumExtent() const
298  {
299  return stateSpace_->getMaximumExtent();
300  }
301 
311  bool searchValidNearby(State *state, const State *near, double distance, unsigned int attempts) const;
312 
321  bool searchValidNearby(const ValidStateSamplerPtr &sampler, State *state, const State *near,
322  double distance) const;
323 
332  unsigned int randomBounceMotion(const StateSamplerPtr &sss, const State *start, unsigned int steps,
333  std::vector<State *> &states, bool alloc) const;
334 
345  bool checkMotion(const State *s1, const State *s2, std::pair<State *, double> &lastValid) const
346  {
347  return motionValidator_->checkMotion(s1, s2, lastValid);
348  }
349 
352  bool checkMotion(const State *s1, const State *s2) const
353  {
354  return motionValidator_->checkMotion(s1, s2);
355  }
356 
363  bool checkMotion(const std::vector<State *> &states, unsigned int count,
364  unsigned int &firstInvalidStateIndex) const;
365 
367  bool checkMotion(const std::vector<State *> &states, unsigned int count) const;
368 
381  unsigned int getMotionStates(const State *s1, const State *s2, std::vector<State *> &states,
382  unsigned int count, bool endpoints, bool alloc) const;
383 
385  unsigned int getCheckedMotionCount() const
386  {
387  return motionValidator_->getCheckedMotionCount();
388  }
389 
396  double probabilityOfValidState(unsigned int attempts) const;
397 
399  double averageValidMotionLength(unsigned int attempts) const;
400 
403  void samplesPerSecond(double &uniform, double &near, double &gaussian, unsigned int attempts) const;
404 
406  virtual void printSettings(std::ostream &out = std::cout) const;
407 
409  virtual void printProperties(std::ostream &out = std::cout) const;
410 
413  {
414  return params_;
415  }
416 
418  const ParamSet &params() const
419  {
420  return params_;
421  }
422 
427  virtual void setup();
428 
430  bool isSetup() const;
431 
432  protected:
435 
438 
442 
446 
448  bool setup_;
449 
452 
455  };
456  }
457 }
458 
459 #endif
unsigned int getStateDimension() const
Return the dimension of the state space.
const StateSpacePtr & getStateSpace() 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 shared pointer wrapper for ompl::base::ValidStateSampler.
double probabilityOfValidState(unsigned int attempts) const
Estimate probability of sampling a valid state. setup() is assumed to have been called.
double averageValidMotionLength(unsigned int attempts) const
Estimate the length of a valid motion. setup() is assumed to have been called.
A 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 shared pointer wrapper for ompl::base::StateSampler.
void freeState(State *state) const
Free the memory of a state.
void setStateValidityChecker(const StateValidityCheckerPtr &svc)
Set the instance of the state validity checker to use. Parallel implementations of planners assume th...
MotionValidatorPtr & getMotionValidator()
Return the non-const instance of the used state validity checker.
StateSamplerPtr allocStateSampler() const
Allocate a uniform state sampler for the state space.
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...
bool isValid(const State *state) const
Check if a given state is valid or not.
State * allocState() const
Allocate memory for a state.
const MotionValidatorPtr & getMotionValidator() const
Return the instance of the used state validity checker.
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...
ValidStateSamplerPtr allocValidStateSampler() const
Allocate an instance of a valid state sampler for this space. If setValidStateSamplerAllocator() was ...
double getStateValidityCheckingResolution() const
Get the resolution at which state validity is verified. This call is only applicable if a ompl::base:...
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...
Maintain a set of parameters.
Definition: GenericParam.h:226
void setDefaultMotionValidator()
Set default motion validator for the state space.
bool equalStates(const State *state1, const State *state2) const
Check if two states are the same.
ParamSet & params()
Get the combined parameters for the classes that the space information manages.
A shared pointer wrapper for ompl::base::StateValidityChecker.
const ParamSet & params() const
Get the combined parameters for the classes that the space information manages.
StateSpacePtr stateSpace_
The state space planning is to be performed in.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
void setValidStateSamplerAllocator(const ValidStateSamplerAllocator &vssa)
Set the allocator to use for a valid state sampler. This replaces the default uniform valid state sam...
void printState(const State *state, std::ostream &out=std::cout) const
Print a state to a stream.
State * cloneState(const State *source) const
Clone a state.
bool satisfiesBounds(const State *state) const
Check if a state is inside the bounding box.
virtual void printSettings(std::ostream &out=std::cout) const
Print information about the current instance of the state space.
A shared pointer wrapper for ompl::base::MotionValidator.
void copyState(State *destination, const State *source) const
Copy a state to another.
const StateValidityCheckerPtr & getStateValidityChecker() const
Return the instance of the used state validity checker.
bool setup_
Flag indicating whether setup() has been called on this instance.
void enforceBounds(State *state) const
Bring the state within the bounds of the state space.
unsigned int getCheckedMotionCount() const
Get the total number of motion segments checked by the MotionValidator so far.
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 ...
std::function< ValidStateSamplerPtr(const SpaceInformation *)> ValidStateSamplerAllocator
Definition of a function that can allocate a valid state sampler.
Definition of an abstract state.
Definition: State.h:49
virtual void printProperties(std::ostream &out=std::cout) const
Print properties of the current instance of the state space.
void freeStates(std::vector< State *> &states) const
Free the memory of an array of states.
bool checkMotion(const State *s1, const State *s2) const
Check if the path between two states (from s1 to s2) is valid, using the MotionValidator. This function assumes s1 is valid.
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...
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.
double getMaximumExtent() const
Get the maximum extent of the space we are planning in. This is the maximum distance that could be re...
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).
void allocStates(std::vector< State *> &states) const
Allocate memory for each element of the array states.
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...
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...
double distance(const State *state1, const State *state2) const
Compute the distance between two states.
double getSpaceMeasure() const
Get a measure of the space (this can be thought of as a generalization of volume) ...