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  ClassForward(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 
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(void) const
388  {
389  return params_;
390  }
391 
396  virtual void setup(void);
397 
399  bool isSetup(void) const;
400 
401  protected:
403  void setDefaultMotionValidator(void);
404 
407 
410 
413 
415  bool setup_;
416 
419 
422  };
423 
424  }
425 
426 }
427 
428 #endif