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 #include <utility>
48 
49 ompl::base::SpaceInformation::SpaceInformation(StateSpacePtr space) : stateSpace_(std::move(space)), setup_(false)
50 {
51  if (!stateSpace_)
52  throw Exception("Invalid space definition");
54  params_.include(stateSpace_->params());
55 }
56 
58 {
60  {
61  stateValidityChecker_ = std::make_shared<AllValidStateValidityChecker>(this);
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 FnStateValidityChecker : public StateValidityChecker
86  {
87  public:
88  FnStateValidityChecker(SpaceInformation *si, StateValidityCheckerFn fn)
89  : StateValidityChecker(si), fn_(std::move(fn))
90  {
91  }
92 
93  bool isValid(const State *state) const override
94  {
95  return fn_(state);
96  }
97 
98  protected:
100  };
101 
102  if (!svc)
103  throw Exception("Invalid function definition for state validity checking");
104 
105  setStateValidityChecker(std::make_shared<FnStateValidityChecker>(this, svc));
106 }
107 
109 {
110  if (dynamic_cast<ReedsSheppStateSpace *>(stateSpace_.get()))
111  motionValidator_ = std::make_shared<ReedsSheppMotionValidator>(this);
112  else if (dynamic_cast<DubinsStateSpace *>(stateSpace_.get()))
113  motionValidator_ = std::make_shared<DubinsMotionValidator>(this);
114  else
115  motionValidator_ = std::make_shared<DiscreteMotionValidator>(this);
116 }
117 
119 {
120  vssa_ = vssa;
121  setup_ = false;
122 }
123 
125 {
127  setup_ = false;
128 }
129 
131  unsigned int steps, std::vector<State *> &states,
132  bool alloc) const
133 {
134  if (alloc)
135  {
136  states.resize(steps);
137  for (unsigned int i = 0; i < steps; ++i)
138  states[i] = allocState();
139  }
140  else if (states.size() < steps)
141  steps = states.size();
142 
143  const State *prev = start;
144  std::pair<State *, double> lastValid;
145  unsigned int j = 0;
146  for (unsigned int i = 0; i < steps; ++i)
147  {
148  sss->sampleUniform(states[j]);
149  lastValid.first = states[j];
150  if (checkMotion(prev, states[j], lastValid) || lastValid.second > std::numeric_limits<double>::epsilon())
151  prev = states[j++];
152  }
153 
154  return j;
155 }
156 
158  const State *near, double distance) const
159 {
160  if (state != near)
161  copyState(state, near);
162 
163  // fix bounds, if needed
164  if (!satisfiesBounds(state))
165  enforceBounds(state);
166 
167  bool result = isValid(state);
168 
169  if (!result)
170  {
171  // try to find a valid state nearby
172  State *temp = cloneState(state);
173  result = sampler->sampleNear(state, temp, distance);
174  freeState(temp);
175  }
176 
177  return result;
178 }
179 
181  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
192  auto uvss = std::make_shared<UniformValidStateSampler>(this);
193  uvss->setNrAttempts(attempts);
194  return searchValidNearby(uvss, state, near, distance);
195  }
196 }
197 
199  std::vector<State *> &states, unsigned int count,
200  bool endpoints, bool alloc) const
201 {
202  // add 1 to the number of states we want to add between s1 & s2. This gives us the number of segments to split the
203  // motion into
204  count++;
205 
206  if (count < 2)
207  {
208  unsigned int added = 0;
209 
210  // if they want endpoints, then at most endpoints are included
211  if (endpoints)
212  {
213  if (alloc)
214  {
215  states.resize(2);
216  states[0] = allocState();
217  states[1] = allocState();
218  }
219  if (states.size() > 0)
220  {
221  copyState(states[0], s1);
222  added++;
223  }
224 
225  if (states.size() > 1)
226  {
227  copyState(states[1], s2);
228  added++;
229  }
230  }
231  else if (alloc)
232  states.resize(0);
233  return added;
234  }
235 
236  if (alloc)
237  {
238  states.resize(count + (endpoints ? 1 : -1));
239  if (endpoints)
240  states[0] = allocState();
241  }
242 
243  unsigned int added = 0;
244 
245  if (endpoints && states.size() > 0)
246  {
247  copyState(states[0], s1);
248  added++;
249  }
250 
251  /* find the states in between */
252  for (unsigned int j = 1; j < count && added < states.size(); ++j)
253  {
254  if (alloc)
255  states[added] = allocState();
256  stateSpace_->interpolate(s1, s2, (double)j / (double)count, states[added]);
257  added++;
258  }
259 
260  if (added < states.size() && endpoints)
261  {
262  if (alloc)
263  states[added] = allocState();
264  copyState(states[added], s2);
265  added++;
266  }
267 
268  return added;
269 }
270 
271 bool ompl::base::SpaceInformation::checkMotion(const std::vector<State *> &states, unsigned int count,
272  unsigned int &firstInvalidStateIndex) const
273 {
274  assert(states.size() >= count);
275  for (unsigned int i = 0; i < count; ++i)
276  if (!isValid(states[i]))
277  {
278  firstInvalidStateIndex = i;
279  return false;
280  }
281  return true;
282 }
283 
284 bool ompl::base::SpaceInformation::checkMotion(const std::vector<State *> &states, unsigned int count) const
285 {
286  assert(states.size() >= count);
287  if (count > 0)
288  {
289  if (count > 1)
290  {
291  if (!isValid(states.front()))
292  return false;
293  if (!isValid(states[count - 1]))
294  return false;
295 
296  // we have 2 or more states, and the first and last states are valid
297 
298  if (count > 2)
299  {
300  std::queue<std::pair<int, int>> pos;
301  pos.push(std::make_pair(0, count - 1));
302 
303  while (!pos.empty())
304  {
305  std::pair<int, int> x = pos.front();
306 
307  int mid = (x.first + x.second) / 2;
308  if (!isValid(states[mid]))
309  return false;
310 
311  pos.pop();
312 
313  if (x.first < mid - 1)
314  pos.push(std::make_pair(x.first, mid));
315  if (x.second > mid + 1)
316  pos.push(std::make_pair(mid, x.second));
317  }
318  }
319  }
320  else
321  return isValid(states.front());
322  }
323  return true;
324 }
325 
327 {
328  if (vssa_)
329  return vssa_(this);
330  else
331  return std::make_shared<UniformValidStateSampler>(this);
332 }
333 
334 double ompl::base::SpaceInformation::probabilityOfValidState(unsigned int attempts) const
335 {
336  if (attempts == 0)
337  return 0.0;
338 
339  unsigned int valid = 0;
340  unsigned int invalid = 0;
341 
343  State *s = allocState();
344 
345  for (unsigned int i = 0; i < attempts; ++i)
346  {
347  ss->sampleUniform(s);
348  if (isValid(s))
349  ++valid;
350  else
351  ++invalid;
352  }
353 
354  freeState(s);
355 
356  return (double)valid / (double)(valid + invalid);
357 }
358 
360 {
361  // take the square root here because we in fact have a nested for loop
362  // where each loop executes #attempts steps (the sample() function of the UniformValidStateSampler if a for loop
363  // too)
364  attempts = std::max((unsigned int)floor(sqrt((double)attempts) + 0.5), 2u);
365 
367  auto uvss(std::make_shared<UniformValidStateSampler>(this));
368  uvss->setNrAttempts(attempts);
369 
370  State *s1 = allocState();
371  State *s2 = allocState();
372 
373  std::pair<State *, double> lastValid;
374  lastValid.first = nullptr;
375 
376  double d = 0.0;
377  unsigned int count = 0;
378  for (unsigned int i = 0; i < attempts; ++i)
379  if (uvss->sample(s1))
380  {
381  ++count;
382  ss->sampleUniform(s2);
383  if (checkMotion(s1, s2, lastValid))
384  d += distance(s1, s2);
385  else
386  d += distance(s1, s2) * lastValid.second;
387  }
388 
389  freeState(s2);
390  freeState(s1);
391 
392  if (count > 0)
393  return d / (double)count;
394  else
395  return 0.0;
396 }
397 
398 void ompl::base::SpaceInformation::samplesPerSecond(double &uniform, double &near, double &gaussian,
399  unsigned int attempts) const
400 {
402  std::vector<State *> states(attempts + 1);
403  allocStates(states);
404 
405  time::point start = time::now();
406  for (unsigned int i = 0; i < attempts; ++i)
407  ss->sampleUniform(states[i]);
408  uniform = (double)attempts / time::seconds(time::now() - start);
409 
410  double d = getMaximumExtent() / 10.0;
411  ss->sampleUniform(states[attempts]);
412 
413  start = time::now();
414  for (unsigned int i = 1; i <= attempts; ++i)
415  ss->sampleUniformNear(states[i - 1], states[i], d);
416  near = (double)attempts / time::seconds(time::now() - start);
417 
418  start = time::now();
419  for (unsigned int i = 1; i <= attempts; ++i)
420  ss->sampleGaussian(states[i - 1], states[i], d);
421  gaussian = (double)attempts / time::seconds(time::now() - start);
422 
423  freeStates(states);
424 }
425 
426 void ompl::base::SpaceInformation::printSettings(std::ostream &out) const
427 {
428  out << "Settings for the state space '" << stateSpace_->getName() << "'" << std::endl;
429  out << " - state validity check resolution: " << (getStateValidityCheckingResolution() * 100.0) << '%'
430  << std::endl;
431  out << " - valid segment count factor: " << stateSpace_->getValidSegmentCountFactor() << std::endl;
432  out << " - state space:" << std::endl;
433  stateSpace_->printSettings(out);
434  out << std::endl
435  << "Declared parameters:" << std::endl;
436  params_.print(out);
438  out << "Valid state sampler named " << vss->getName() << " with parameters:" << std::endl;
439  vss->params().print(out);
440 }
441 
443 {
444  out << "Properties of the state space '" << stateSpace_->getName() << "'" << std::endl;
445  out << " - signature: ";
446  std::vector<int> sig;
447  stateSpace_->computeSignature(sig);
448  for (int i : sig)
449  out << i << " ";
450  out << std::endl;
451  out << " - dimension: " << stateSpace_->getDimension() << std::endl;
452  out << " - extent: " << stateSpace_->getMaximumExtent() << std::endl;
453  if (isSetup())
454  {
455  bool result = true;
456  try
457  {
458  stateSpace_->sanityChecks();
459  }
460  catch (Exception &e)
461  {
462  result = false;
463  out << std::endl
464  << " - SANITY CHECKS FOR STATE SPACE ***DID NOT PASS*** (" << e.what() << ")" << std::endl
465  << std::endl;
466  OMPL_ERROR(e.what());
467  }
468  if (result)
469  out << " - sanity checks for state space passed" << std::endl;
470  out << " - probability of valid states: " << probabilityOfValidState(magic::TEST_STATE_COUNT) << std::endl;
471  out << " - average length of a valid motion: " << averageValidMotionLength(magic::TEST_STATE_COUNT)
472  << std::endl;
473  double uniform, near, gaussian;
474  samplesPerSecond(uniform, near, gaussian, magic::TEST_STATE_COUNT);
475  out << " - average number of samples drawn per second: sampleUniform()=" << uniform
476  << " sampleUniformNear()=" << near << " sampleGaussian()=" << gaussian << std::endl;
477  }
478  else
479  out << "Call setup() before to get more information" << std::endl;
480 }
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 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...
StateSamplerPtr allocStateSampler() const
Allocate a uniform state sampler for the state space.
STL namespace.
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.
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...
void setDefaultMotionValidator()
Set default motion validator for the state space.
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:76
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...
State * cloneState(const State *source) const
Clone a state.
bool satisfiesBounds(const State *state) const
Check if a state is inside the bounding box.
void print(std::ostream &out) const
Print the parameters to a stream.
virtual void printSettings(std::ostream &out=std::cout) const
Print information about the current instance of the state space.
void copyState(State *destination, const State *source) const
Copy a state to another.
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 enforceBounds(State *state) const
Bring the state within the bounds of the state space.
Abstract definition for a class checking the validity of states. The implementation of this class mus...
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
#define OMPL_WARN(fmt,...)
Log a formatted warning string.
Definition: Console.h:66
The exception type for ompl.
Definition: Exception.h:46
point now()
Get the current time point.
Definition: Time.h:70
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, std::pair< State *, double > &lastValid) const
Incrementally check if the path between two motions is valid. Also compute the last state that was va...
static const unsigned int TEST_STATE_COUNT
When multiple states need to be generated as part of the computation of various information (usually ...
void clearValidStateSamplerAllocator()
Clear the allocator used for the valid state sampler. This will revert to using the uniform valid sta...
std::chrono::system_clock::time_point point
Representation of a point in time.
Definition: Time.h:64
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 clear()
Clear all the set parameters.
double distance(const State *state1, const State *state2) const
Compute the distance between two states.