SpaceInformation.cpp
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 #include "ompl/base/SpaceInformation.h"
38 #include "ompl/base/samplers/UniformValidStateSampler.h"
39 #include "ompl/base/DiscreteMotionValidator.h"
40 #include "ompl/base/spaces/ReedsSheppStateSpace.h"
41 #include "ompl/base/spaces/DubinsStateSpace.h"
42 #include "ompl/util/Exception.h"
43 #include "ompl/util/Time.h"
44 #include "ompl/tools/config/MagicConstants.h"
45 #include <queue>
46 #include <cassert>
47 
49  stateSpace_(space), setup_(false)
50 {
51  if (!stateSpace_)
52  throw Exception("Invalid space definition");
54  params_.include(stateSpace_->params());
55 }
56 
58 {
60  {
62  OMPL_WARN("State validity checker not set! No collision checking is performed");
63  }
64 
65  if (!motionValidator_)
67 
68  stateSpace_->setup();
69  if (stateSpace_->getDimension() <= 0)
70  throw Exception("The dimension of the state space we plan in must be > 0");
71 
72  params_.clear();
73  params_.include(stateSpace_->params());
74 
75  setup_ = true;
76 }
77 
79 {
80  return setup_;
81 }
82 
84 {
85  class BoostFnStateValidityChecker : public StateValidityChecker
86  {
87  public:
88 
89  BoostFnStateValidityChecker(SpaceInformation *si,
90  const StateValidityCheckerFn &fn) : StateValidityChecker(si), fn_(fn)
91  {
92  }
93 
94  virtual bool isValid(const State *state) const
95  {
96  return fn_(state);
97  }
98 
99  protected:
100 
102  };
103 
104  if (!svc)
105  throw Exception("Invalid function definition for state validity checking");
106 
107  setStateValidityChecker(StateValidityCheckerPtr(dynamic_cast<StateValidityChecker*>(new BoostFnStateValidityChecker(this, svc))));
108 }
109 
111 {
112  if (dynamic_cast<ReedsSheppStateSpace*>(stateSpace_.get()))
114  else if (dynamic_cast<DubinsStateSpace*>(stateSpace_.get()))
115  motionValidator_.reset(new DubinsMotionValidator(this));
116  else
117  motionValidator_.reset(new DiscreteMotionValidator(this));
118 }
119 
120 
122 {
123  vssa_ = vssa;
124  setup_ = false;
125 }
126 
128 {
130  setup_ = false;
131 }
132 
133 unsigned int ompl::base::SpaceInformation::randomBounceMotion(const StateSamplerPtr &sss, const State *start, unsigned int steps, std::vector<State*> &states, bool alloc) const
134 {
135  if (alloc)
136  {
137  states.resize(steps);
138  for (unsigned int i = 0 ; i < steps ; ++i)
139  states[i] = allocState();
140  }
141  else
142  if (states.size() < steps)
143  steps = states.size();
144 
145  const State *prev = start;
146  std::pair<State*, double> lastValid;
147  unsigned int j = 0;
148  for (unsigned int i = 0 ; i < steps ; ++i)
149  {
150  sss->sampleUniform(states[j]);
151  lastValid.first = states[j];
152  if (checkMotion(prev, states[j], lastValid) || lastValid.second > std::numeric_limits<double>::epsilon())
153  prev = states[j++];
154  }
155 
156  return j;
157 }
158 
159 bool ompl::base::SpaceInformation::searchValidNearby(const ValidStateSamplerPtr &sampler, State *state, const State *near, double distance) const
160 {
161  if (state != near)
162  copyState(state, near);
163 
164  // fix bounds, if needed
165  if (!satisfiesBounds(state))
166  enforceBounds(state);
167 
168  bool result = isValid(state);
169 
170  if (!result)
171  {
172  // try to find a valid state nearby
173  State *temp = cloneState(state);
174  result = sampler->sampleNear(state, temp, distance);
175  freeState(temp);
176  }
177 
178  return result;
179 }
180 
181 bool ompl::base::SpaceInformation::searchValidNearby(State *state, const State *near, double distance, unsigned int attempts) const
182 {
183  if (satisfiesBounds(near) && isValid(near))
184  {
185  if (state != near)
186  copyState(state, near);
187  return true;
188  }
189  else
190  {
191  // try to find a valid state nearby
193  uvss->setNrAttempts(attempts);
194  return searchValidNearby(ValidStateSamplerPtr(uvss), state, near, distance);
195  }
196 }
197 
198 unsigned int ompl::base::SpaceInformation::getMotionStates(const State *s1, const State *s2, std::vector<State*> &states, unsigned int count, bool endpoints, bool alloc) const
199 {
200  // add 1 to the number of states we want to add between s1 & s2. This gives us the number of segments to split the motion into
201  count++;
202 
203  if (count < 2)
204  {
205  unsigned int added = 0;
206 
207  // if they want endpoints, then at most endpoints are included
208  if (endpoints)
209  {
210  if (alloc)
211  {
212  states.resize(2);
213  states[0] = allocState();
214  states[1] = allocState();
215  }
216  if (states.size() > 0)
217  {
218  copyState(states[0], s1);
219  added++;
220  }
221 
222  if (states.size() > 1)
223  {
224  copyState(states[1], s2);
225  added++;
226  }
227  }
228  else
229  if (alloc)
230  states.resize(0);
231  return added;
232  }
233 
234  if (alloc)
235  {
236  states.resize(count + (endpoints ? 1 : -1));
237  if (endpoints)
238  states[0] = allocState();
239  }
240 
241  unsigned int added = 0;
242 
243  if (endpoints && states.size() > 0)
244  {
245  copyState(states[0], s1);
246  added++;
247  }
248 
249  /* find the states in between */
250  for (unsigned int j = 1 ; j < count && added < states.size() ; ++j)
251  {
252  if (alloc)
253  states[added] = allocState();
254  stateSpace_->interpolate(s1, s2, (double)j / (double)count, states[added]);
255  added++;
256  }
257 
258  if (added < states.size() && endpoints)
259  {
260  if (alloc)
261  states[added] = allocState();
262  copyState(states[added], s2);
263  added++;
264  }
265 
266  return added;
267 }
268 
269 
270 bool ompl::base::SpaceInformation::checkMotion(const std::vector<State*> &states, unsigned int count, unsigned int &firstInvalidStateIndex) const
271 {
272  assert(states.size() >= count);
273  for (unsigned int i = 0 ; i < count ; ++i)
274  if (!isValid(states[i]))
275  {
276  firstInvalidStateIndex = i;
277  return false;
278  }
279  return true;
280 }
281 
282 bool ompl::base::SpaceInformation::checkMotion(const std::vector<State*> &states, unsigned int count) const
283 {
284  assert(states.size() >= count);
285  if (count > 0)
286  {
287  if (count > 1)
288  {
289  if (!isValid(states.front()))
290  return false;
291  if (!isValid(states[count - 1]))
292  return false;
293 
294  // we have 2 or more states, and the first and last states are valid
295 
296  if (count > 2)
297  {
298  std::queue< std::pair<int, int> > pos;
299  pos.push(std::make_pair(0, count - 1));
300 
301  while (!pos.empty())
302  {
303  std::pair<int, int> x = pos.front();
304 
305  int mid = (x.first + x.second) / 2;
306  if (!isValid(states[mid]))
307  return false;
308 
309  if (x.first < mid - 1)
310  pos.push(std::make_pair(x.first, mid));
311  if (x.second > mid + 1)
312  pos.push(std::make_pair(mid, x.second));
313  }
314  }
315  }
316  else
317  return isValid(states.front());
318  }
319  return true;
320 }
321 
323 {
324  if (vssa_)
325  return vssa_(this);
326  else
328 }
329 
330 double ompl::base::SpaceInformation::probabilityOfValidState(unsigned int attempts) const
331 {
332  if (attempts == 0)
333  return 0.0;
334 
335  unsigned int valid = 0;
336  unsigned int invalid = 0;
337 
339  State *s = allocState();
340 
341  for (unsigned int i = 0 ; i < attempts ; ++i)
342  {
343  ss->sampleUniform(s);
344  if (isValid(s))
345  ++valid;
346  else
347  ++invalid;
348  }
349 
350  freeState(s);
351 
352  return (double)valid / (double)(valid + invalid);
353 }
354 
356 {
357  // take the square root here because we in fact have a nested for loop
358  // where each loop executes #attempts steps (the sample() function of the UniformValidStateSampler if a for loop too)
359  attempts = std::max((unsigned int)floor(sqrt((double)attempts) + 0.5), 2u);
360 
363  uvss->setNrAttempts(attempts);
364 
365  State *s1 = allocState();
366  State *s2 = allocState();
367 
368  std::pair<State*, double> lastValid;
369  lastValid.first = NULL;
370 
371  double d = 0.0;
372  unsigned int count = 0;
373  for (unsigned int i = 0 ; i < attempts ; ++i)
374  if (uvss->sample(s1))
375  {
376  ++count;
377  ss->sampleUniform(s2);
378  if (checkMotion(s1, s2, lastValid))
379  d += distance(s1, s2);
380  else
381  d += distance(s1, s2) * lastValid.second;
382  }
383 
384  freeState(s2);
385  freeState(s1);
386  delete uvss;
387 
388  if (count > 0)
389  return d / (double)count;
390  else
391  return 0.0;
392 }
393 
394 void ompl::base::SpaceInformation::samplesPerSecond(double &uniform, double &near, double &gaussian, unsigned int attempts) const
395 {
397  std::vector<State*> states(attempts + 1);
398  allocStates(states);
399 
400  time::point start = time::now();
401  for (unsigned int i = 0 ; i < attempts ; ++i)
402  ss->sampleUniform(states[i]);
403  uniform = (double)attempts / time::seconds(time::now() - start);
404 
405  double d = getMaximumExtent() / 10.0;
406  ss->sampleUniform(states[attempts]);
407 
408  start = time::now();
409  for (unsigned int i = 1 ; i <= attempts ; ++i)
410  ss->sampleUniformNear(states[i - 1], states[i], d);
411  near = (double)attempts / time::seconds(time::now() - start);
412 
413  start = time::now();
414  for (unsigned int i = 1 ; i <= attempts ; ++i)
415  ss->sampleGaussian(states[i - 1], states[i], d);
416  gaussian = (double)attempts / time::seconds(time::now() - start);
417 
418  freeStates(states);
419 }
420 
421 void ompl::base::SpaceInformation::printSettings(std::ostream &out) const
422 {
423  out << "Settings for the state space '" << stateSpace_->getName() << "'" << std::endl;
424  out << " - state validity check resolution: " << (getStateValidityCheckingResolution() * 100.0) << '%' << std::endl;
425  out << " - valid segment count factor: " << stateSpace_->getValidSegmentCountFactor() << std::endl;
426  out << " - state space:" << std::endl;
427  stateSpace_->printSettings(out);
428  out << std::endl << "Declared parameters:" << std::endl;
429  params_.print(out);
431  out << "Valid state sampler named " << vss->getName() << " with parameters:" << std::endl;
432  vss->params().print(out);
433 }
434 
436 {
437  out << "Properties of the state space '" << stateSpace_->getName() << "'" << std::endl;
438  out << " - signature: ";
439  std::vector<int> sig;
440  stateSpace_->computeSignature(sig);
441  for (std::size_t i = 0 ; i < sig.size() ; ++i)
442  out << sig[i] << " ";
443  out << std::endl;
444  out << " - dimension: " << stateSpace_->getDimension() << std::endl;
445  out << " - extent: " << stateSpace_->getMaximumExtent() << std::endl;
446  if (isSetup())
447  {
448  bool result = true;
449  try
450  {
451  stateSpace_->sanityChecks();
452  }
453  catch(Exception &e)
454  {
455  result = false;
456  out << std::endl << " - SANITY CHECKS FOR STATE SPACE ***DID NOT PASS*** (" << e.what() << ")" << std::endl << std::endl;
457  OMPL_ERROR(e.what());
458  }
459  if (result)
460  out << " - sanity checks for state space passed" << std::endl;
461  out << " - probability of valid states: " << probabilityOfValidState(magic::TEST_STATE_COUNT) << std::endl;
462  out << " - average length of a valid motion: " << averageValidMotionLength(magic::TEST_STATE_COUNT) << std::endl;
463  double uniform, near, gaussian;
464  samplesPerSecond(uniform, near, gaussian, magic::TEST_STATE_COUNT);
465  out << " - average number of samples drawn per second: sampleUniform()=" << uniform << " sampleUniformNear()=" << near << " sampleGaussian()=" << gaussian << std::endl;
466  }
467  else
468  out << "Call setup() before to get more information" << std::endl;
469 }
double distance(const State *state1, const State *state2) const
Compute the distance between two states.
void print(std::ostream &out) const
Print the parameters to a stream.
boost::function< ValidStateSamplerPtr(const SpaceInformation *)> ValidStateSamplerAllocator
Definition of a function that can allocate a valid state sampler.
virtual bool sample(State *state)
Sample a state. Return false in case of failure.
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 include(const ParamSet &other, const std::string &prefix="")
Include the params of a different ParamSet into this one. Optionally include a prefix for each of the...
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...
A motion validator that only uses the state validity checker. Motions are checked for validity at a s...
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).
A state sampler that only samples valid states, uniformly.
void setDefaultMotionValidator()
Set default motion validator for the state space.
A boost shared pointer wrapper for ompl::base::StateValidityChecker.
The simplest state validity checker: all states are valid.
bool satisfiesBounds(const State *state) const
Check if a state is inside the bounding box.
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:62
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.
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...
bool setup_
Flag indicating whether setup() has been called on this instance.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
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.
Abstract definition for a class checking the validity of states. The implementation of this class mus...
boost::posix_time::ptime point
Representation of a point in time.
Definition: Time.h:50
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.
A Reeds-Shepp motion validator that only uses the state validity checker. Motions are checked for val...
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
A Dubins motion validator that only uses the state validity checker. Motions are checked for validity...
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...
The exception type for ompl.
Definition: Exception.h:47
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.
point now()
Get the current time point.
Definition: Time.h:56
static const unsigned int TEST_STATE_COUNT
When multiple states need to be generated as part of the computation of various information (usually ...
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.
State * allocState() const
Allocate memory for a state.
void clear()
Clear all the set parameters.
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
void setNrAttempts(unsigned int attempts)
Finding a valid sample usually requires performing multiple attempts. This call allows setting the nu...