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