All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
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 {
59  if (!stateValidityChecker_)
60  {
61  stateValidityChecker_.reset(new AllValidStateValidityChecker(this));
62  OMPL_WARN("State validity checker not set! No collision checking is performed");
63  }
64 
65  if (!motionValidator_)
66  setDefaultMotionValidator();
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()))
113  motionValidator_.reset(new ReedsSheppMotionValidator(this));
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 {
129  vssa_ = ValidStateSamplerAllocator();
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 
338  StateSamplerPtr ss = allocStateSampler();
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 
361  StateSamplerPtr ss = allocStateSampler();
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 double ompl::base::SpaceInformation::averageStateCost(unsigned int attempts) const
395 {
396  StateSamplerPtr ss = allocStateSampler();
397  State *state = allocState();
398  double cost = 0.0;
399 
400  for (unsigned int i = 0 ; i < attempts ; ++i)
401  {
402  ss->sampleUniform(state);
403  cost += stateValidityChecker_->cost(state);
404  }
405 
406  freeState(state);
407 
408  return cost / (double)attempts;
409 }
410 
411 void ompl::base::SpaceInformation::samplesPerSecond(double &uniform, double &near, double &gaussian, unsigned int attempts) const
412 {
413  StateSamplerPtr ss = allocStateSampler();
414  std::vector<State*> states(attempts + 1);
415  allocStates(states);
416 
417  time::point start = time::now();
418  for (unsigned int i = 0 ; i < attempts ; ++i)
419  ss->sampleUniform(states[i]);
420  uniform = (double)attempts / time::seconds(time::now() - start);
421 
422  double d = getMaximumExtent() / 10.0;
423  ss->sampleUniform(states[attempts]);
424 
425  start = time::now();
426  for (unsigned int i = 1 ; i <= attempts ; ++i)
427  ss->sampleUniformNear(states[i - 1], states[i], d);
428  near = (double)attempts / time::seconds(time::now() - start);
429 
430  start = time::now();
431  for (unsigned int i = 1 ; i <= attempts ; ++i)
432  ss->sampleGaussian(states[i - 1], states[i], d);
433  gaussian = (double)attempts / time::seconds(time::now() - start);
434 
435  freeStates(states);
436 }
437 
438 void ompl::base::SpaceInformation::printSettings(std::ostream &out) const
439 {
440  out << "Settings for the state space '" << stateSpace_->getName() << "'" << std::endl;
441  out << " - state validity check resolution: " << (getStateValidityCheckingResolution() * 100.0) << '%' << std::endl;
442  out << " - valid segment count factor: " << stateSpace_->getValidSegmentCountFactor() << std::endl;
443  out << " - state space:" << std::endl;
444  stateSpace_->printSettings(out);
445  out << std::endl << "Declared parameters:" << std::endl;
446  params_.print(out);
447  ValidStateSamplerPtr vss = allocValidStateSampler();
448  out << "Valid state sampler named " << vss->getName() << " with parameters:" << std::endl;
449  vss->params().print(out);
450 }
451 
453 {
454  out << "Properties of the state space '" << stateSpace_->getName() << "'" << std::endl;
455  out << " - signature: ";
456  std::vector<int> sig;
457  stateSpace_->computeSignature(sig);
458  for (std::size_t i = 0 ; i < sig.size() ; ++i)
459  out << sig[i] << " ";
460  out << std::endl;
461  out << " - dimension: " << stateSpace_->getDimension() << std::endl;
462  out << " - extent: " << stateSpace_->getMaximumExtent() << std::endl;
463  if (isSetup())
464  {
465  bool result = true;
466  try
467  {
468  stateSpace_->sanityChecks();
469  }
470  catch(Exception &e)
471  {
472  result = false;
473  out << std::endl << " - SANITY CHECKS FOR STATE SPACE ***DID NOT PASS*** (" << e.what() << ")" << std::endl << std::endl;
474  OMPL_ERROR(e.what());
475  }
476  if (result)
477  out << " - sanity checks for state space passed" << std::endl;
478  out << " - probability of valid states: " << probabilityOfValidState(magic::TEST_STATE_COUNT) << std::endl;
479  out << " - average state cost: " << averageStateCost(magic::TEST_STATE_COUNT) << std::endl;
480  out << " - average length of a valid motion: " << averageValidMotionLength(magic::TEST_STATE_COUNT) << std::endl;
481  double uniform, near, gaussian;
482  samplesPerSecond(uniform, near, gaussian, magic::TEST_STATE_COUNT);
483  out << " - average number of samples drawn per second: sampleUniform()=" << uniform << " sampleUniformNear()=" << near << " sampleGaussian()=" << gaussian << std::endl;
484  }
485  else
486  out << "Call setup() before to get more information" << std::endl;
487 }