Loading...
Searching...
No Matches
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
57namespace ompl
58{
63 namespace base
64 {
66
67 OMPL_CLASS_FORWARD(SpaceInformation);
69
76 using StateValidityCheckerFn = std::function<bool(const State *)>;
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 {
149 setup_ = false;
150 }
151
158
164
169 {
170 motionValidator_ = mv;
171 setup_ = false;
172 }
173
176 {
177 return motionValidator_;
178 }
179
185
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
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 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
352 virtual 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 virtual 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
A shared pointer wrapper for ompl::base::MotionValidator.
Maintain a set of parameters.
The base class for space information. This contains all the information about the space planning is d...
virtual 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....
StateValidityCheckerPtr stateValidityChecker_
The instance of the state validity checker used for determining the validity of states in the plannin...
void setStateValidityChecker(const StateValidityCheckerPtr &svc)
Set the instance of the state validity checker to use. Parallel implementations of planners assume th...
unsigned int getCheckedMotionCount() const
Get the total number of motion segments checked by the MotionValidator so far.
void freeState(State *state) const
Free the memory of a state.
void setDefaultMotionValidator()
Set default motion validator for the 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...
bool equalStates(const State *state1, const State *state2) const
Check if two states are the same.
virtual 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...
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 printState(const State *state, std::ostream &out=std::cout) const
Print a state to a stream.
bool satisfiesBounds(const State *state) const
Check if a state is inside the bounding box.
double distance(const State *state1, const State *state2) const
Compute the distance between two states.
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....
double probabilityOfValidState(unsigned int attempts) const
Estimate probability of sampling a valid state. setup() is assumed to have been called.
void setMotionValidator(const MotionValidatorPtr &mv)
Set the instance of the motion validity checker to use. Parallel implementations of planners assume t...
virtual void setup()
Perform additional setup tasks (run once, before use). If state validity checking resolution has not ...
const ParamSet & params() const
Get the combined parameters for the classes that the space information manages.
void freeStates(std::vector< State * > &states) const
Free the memory of an array of states.
void enforceBounds(State *state) const
Bring the state within the bounds of the state space.
StateSamplerPtr allocStateSampler() const
Allocate a uniform state sampler for the state space.
double averageValidMotionLength(unsigned int attempts) const
Estimate the length of a valid motion. setup() is assumed to have been called.
double getStateValidityCheckingResolution() const
Get the resolution at which state validity is verified. This call is only applicable if a ompl::base:...
const MotionValidatorPtr & getMotionValidator() const
Return the instance of the used state validity checker.
ParamSet params_
Combined parameters for the contained classes.
virtual void printSettings(std::ostream &out=std::cout) const
Print information about the current instance of the state space.
bool isSetup() const
Return true if setup was called.
virtual void printProperties(std::ostream &out=std::cout) const
Print properties of the current instance of the state space.
StateSpacePtr stateSpace_
The state space planning is to be performed in.
const StateValidityCheckerPtr & getStateValidityChecker() const
Return the instance of the used state validity checker.
double getSpaceMeasure() const
Get a measure of the space (this can be thought of as a generalization of volume)
unsigned int getStateDimension() const
Return the dimension of the state space.
ParamSet & params()
Get the combined parameters for the classes that the space information manages.
void allocStates(std::vector< State * > &states) const
Allocate memory for each element of the array states.
void copyState(State *destination, const State *source) const
Copy a state to another.
MotionValidatorPtr motionValidator_
The instance of the motion validator to use when determining the validity of motions in the planning ...
ValidStateSamplerAllocator vssa_
The optional valid state sampler allocator.
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....
double getMaximumExtent() const
Get the maximum extent of the space we are planning in. This is the maximum distance that could be re...
ValidStateSamplerPtr allocValidStateSampler() const
Allocate an instance of a valid state sampler for this space. If setValidStateSamplerAllocator() was ...
State * cloneState(const State *source) const
Clone a state.
const StateSpacePtr & getStateSpace() const
Return the instance of the used state space.
MotionValidatorPtr & getMotionValidator()
Return the non-const instance of the used state validity checker.
State * allocState() const
Allocate memory for a state.
void setValidStateSamplerAllocator(const ValidStateSamplerAllocator &vssa)
Set the allocator to use for a valid state sampler. This replaces the default uniform valid state sam...
void clearValidStateSamplerAllocator()
Clear the allocator used for the valid state sampler. This will revert to using the uniform valid sta...
bool setup_
Flag indicating whether setup() has been called on this instance.
bool isValid(const State *state) const
Check if a given state is valid or not.
virtual 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 shared pointer wrapper for ompl::base::StateSampler.
A shared pointer wrapper for ompl::base::StateSpace.
A shared pointer wrapper for ompl::base::StateValidityChecker.
Definition of an abstract state.
Definition State.h:50
A shared pointer wrapper for ompl::base::ValidStateSampler.
std::function< bool(const State *)> StateValidityCheckerFn
If no state validity checking class is specified (StateValidityChecker), a std::function can be speci...
std::function< ValidStateSamplerPtr(const SpaceInformation *)> ValidStateSamplerAllocator
Definition of a function that can allocate a valid state sampler.
Main namespace. Contains everything in this library.