All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends
PathGeometric.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
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 Willow Garage 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/geometric/PathGeometric.h"
38 #include "ompl/base/samplers/UniformValidStateSampler.h"
39 #include "ompl/base/ScopedState.h"
40 #include <algorithm>
41 #include <cmath>
42 #include <limits>
43 #include <boost/math/constants/constants.hpp>
44 
46 {
47  copyFrom(path);
48 }
49 
51 {
52  states_.resize(1);
53  states_[0] = si_->cloneState(state);
54 }
55 
57 {
58  states_.resize(2);
59  states_[0] = si_->cloneState(state1);
60  states_[1] = si_->cloneState(state2);
61 }
62 
64 {
65  if (this != &other)
66  {
67  freeMemory();
68  si_ = other.si_;
69  copyFrom(other);
70  }
71  return *this;
72 }
73 
75 {
76  states_.resize(other.states_.size());
77  for (unsigned int i = 0 ; i < states_.size() ; ++i)
78  states_[i] = si_->cloneState(other.states_[i]);
79 }
80 
82 {
83  for (unsigned int i = 0 ; i < states_.size() ; ++i)
84  si_->freeState(states_[i]);
85 }
86 
88 {
89  double L = 0.0;
90  for (unsigned int i = 1 ; i < states_.size() ; ++i)
91  L += si_->distance(states_[i-1], states_[i]);
92  return L;
93 }
94 
96 {
97  double c = 0.0;
98  for (unsigned int i = 0 ; i < states_.size() ; ++i)
99  c += si_->getStateValidityChecker()->clearance(states_[i]);
100  if (states_.empty())
101  c = std::numeric_limits<double>::infinity();
102  else
103  c /= (double)states_.size();
104  return c;
105 }
106 
108 {
109  double s = 0.0;
110  if (states_.size() > 2)
111  {
112  double a = si_->distance(states_[0], states_[1]);
113  for (unsigned int i = 2 ; i < states_.size() ; ++i)
114  {
115  // view the path as a sequence of segments, and look at the triangles it forms:
116  // s1
117  // /\ s4
118  // a / \ b |
119  // / \ |
120  // /......\_______|
121  // s0 c s2 s3
122  //
123  // use Pythagoras generalized theorem to find the cos of the angle between segments a and b
124  double b = si_->distance(states_[i-1], states_[i]);
125  double c = si_->distance(states_[i-2], states_[i]);
126  double acosValue = (a*a + b*b - c*c) / (2.0*a*b);
127 
128  if (acosValue > -1.0 && acosValue < 1.0)
129  {
130  // the smoothness is actually the outside angle of the one we compute
131  double angle = (boost::math::constants::pi<double>() - acos(acosValue));
132 
133  // and we normalize by the length of the segments
134  double k = 2.0 * angle / (a + b);
135  s += k * k;
136  }
137  a = b;
138  }
139  }
140  return s;
141 }
142 
144 {
145  bool result = true;
146  if (states_.size() > 0)
147  {
148  if (si_->isValid(states_[0]))
149  {
150  int last = states_.size() - 1;
151  for (int j = 0 ; result && j < last ; ++j)
152  if (!si_->checkMotion(states_[j], states_[j + 1]))
153  result = false;
154  }
155  else
156  result = false;
157  }
158 
159  return result;
160 }
161 
162 void ompl::geometric::PathGeometric::print(std::ostream &out) const
163 {
164  out << "Geometric path with " << states_.size() << " states" << std::endl;
165  for (unsigned int i = 0 ; i < states_.size() ; ++i)
166  si_->printState(states_[i], out);
167  out << std::endl;
168 }
170 {
171  const base::StateSpace* space(si_->getStateSpace().get());
172  std::vector<double> reals;
173  for (unsigned int i = 0 ; i < states_.size() ; ++i)
174  {
175  space->copyToReals(reals, states_[i]);
176  std::copy(reals.begin(), reals.end(), std::ostream_iterator<double>(out, " "));
177  out << std::endl;
178  }
179  out << std::endl;
180 }
181 
182 std::pair<bool, bool> ompl::geometric::PathGeometric::checkAndRepair(unsigned int attempts)
183 {
184  if (states_.empty())
185  return std::make_pair(true, true);
186  if (states_.size() == 1)
187  {
188  bool result = si_->isValid(states_[0]);
189  return std::make_pair(result, result);
190  }
191 
192  // a path with invalid endpoints cannot be fixed; planners should not return such paths anyway
193  const int n1 = states_.size() - 1;
194  if (!si_->isValid(states_[0]) || !si_->isValid(states_[n1]))
195  return std::make_pair(false, false);
196 
197  base::State *temp = NULL;
198  base::UniformValidStateSampler *uvss = NULL;
199  bool result = true;
200 
201  for (int i = 1 ; i < n1 ; ++i)
202  if (!si_->checkMotion(states_[i-1], states_[i]))
203  {
204  // we now compute a state around which to sample
205  if (!temp)
206  temp = si_->allocState();
207  if (!uvss)
208  {
209  uvss = new base::UniformValidStateSampler(si_.get());
210  uvss->setNrAttempts(attempts);
211  }
212 
213  // and a radius of sampling around that state
214  double radius = 0.0;
215 
216  if (si_->isValid(states_[i]))
217  {
218  si_->copyState(temp, states_[i]);
219  radius = si_->distance(states_[i-1], states_[i]);
220  }
221  else
222  {
223  unsigned int nextValid = n1;
224  for (int j = i + 1 ; j < n1 ; ++j)
225  if (si_->isValid(states_[j]))
226  {
227  nextValid = j;
228  break;
229  }
230  // we know nextValid will be initialised because n1 is certainly valid.
231  si_->getStateSpace()->interpolate(states_[i - 1], states_[nextValid], 0.5, temp);
232  radius = std::max(si_->distance(states_[i-1], temp), si_->distance(states_[i-1], states_[i]));
233  }
234 
235  bool success = false;
236 
237  for (unsigned int a = 0 ; a < attempts ; ++a)
238  if (uvss->sampleNear(states_[i], temp, radius))
239  {
240  if (si_->checkMotion(states_[i-1], states_[i]))
241  {
242  success = true;
243  break;
244  }
245  }
246  else
247  break;
248  if (!success)
249  {
250  result = false;
251  break;
252  }
253  }
254 
255  // free potentially allocated memory
256  if (temp)
257  si_->freeState(temp);
258  bool originalValid = uvss == NULL;
259  if (uvss)
260  delete uvss;
261 
262  return std::make_pair(originalValid, result);
263 }
264 
266 {
267  if (states_.size() < 2)
268  return;
269  std::vector<base::State*> newStates(1, states_[0]);
270  for (unsigned int i = 1 ; i < states_.size() ; ++i)
271  {
272  base::State *temp = si_->allocState();
273  si_->getStateSpace()->interpolate(newStates.back(), states_[i], 0.5, temp);
274  newStates.push_back(temp);
275  newStates.push_back(states_[i]);
276  }
277  states_.swap(newStates);
278 }
279 
281 {
282  unsigned int n = 0;
283  const int n1 = states_.size() - 1;
284  for (int i = 0 ; i < n1 ; ++i)
285  n += si_->getStateSpace()->validSegmentCount(states_[i], states_[i + 1]);
286  interpolate(n);
287 }
288 
289 void ompl::geometric::PathGeometric::interpolate(unsigned int requestCount)
290 {
291  if (requestCount < states_.size() || states_.size() < 2)
292  return;
293 
294  unsigned int count = requestCount;
295 
296  // the remaining length of the path we need to add states along
297  double remainingLength = length();
298 
299  // the new array of states this path will have
300  std::vector<base::State*> newStates;
301  const int n1 = states_.size() - 1;
302 
303  for (int i = 0 ; i < n1 ; ++i)
304  {
305  base::State *s1 = states_[i];
306  base::State *s2 = states_[i + 1];
307 
308  newStates.push_back(s1);
309 
310  // the maximum number of states that can be added on the current motion (without its endpoints)
311  // such that we can at least fit the remaining states
312  int maxNStates = count + i - states_.size();
313 
314  if (maxNStates > 0)
315  {
316  // compute an approximate number of states the following segment needs to contain; this includes endpoints
317  double segmentLength = si_->distance(s1, s2);
318  int ns = i + 1 == n1 ? maxNStates + 2 : (int)floor(0.5 + (double)count * segmentLength / remainingLength) + 1;
319 
320  // if more than endpoints are needed
321  if (ns > 2)
322  {
323  ns -= 2; // subtract endpoints
324 
325  // make sure we don't add too many states
326  if (ns > maxNStates)
327  ns = maxNStates;
328 
329  // compute intermediate states
330  std::vector<base::State*> block;
331  unsigned int ans = si_->getMotionStates(s1, s2, block, ns, false, true);
332  // sanity checks
333  if ((int)ans != ns || block.size() != ans)
334  throw Exception("Internal error in path interpolation. Incorrect number of intermediate states. Please contact the developers.");
335 
336  newStates.insert(newStates.end(), block.begin(), block.end());
337  }
338  else
339  ns = 0;
340 
341  // update what remains to be done
342  count -= (ns + 1);
343  remainingLength -= segmentLength;
344  }
345  else
346  count--;
347  }
348 
349  // add the last state
350  newStates.push_back(states_[n1]);
351  states_.swap(newStates);
352  if (requestCount != states_.size())
353  throw Exception("Internal error in path interpolation. This should never happen. Please contact the developers.");
354 }
355 
357 {
358  std::reverse(states_.begin(), states_.end());
359 }
360 
362 {
363  freeMemory();
364  states_.resize(2);
365  states_[0] = si_->allocState();
366  states_[1] = si_->allocState();
367  base::StateSamplerPtr ss = si_->allocStateSampler();
368  ss->sampleUniform(states_[0]);
369  ss->sampleUniform(states_[1]);
370 }
371 
373 {
374  freeMemory();
375  states_.resize(2);
376  states_[0] = si_->allocState();
377  states_[1] = si_->allocState();
379  uvss->setNrAttempts(attempts);
380  bool ok = false;
381  for (unsigned int i = 0 ; i < attempts ; ++i)
382  {
383  if (uvss->sample(states_[0]) && uvss->sample(states_[1]))
384  if (si_->checkMotion(states_[0], states_[1]))
385  {
386  ok = true;
387  break;
388  }
389  }
390  delete uvss;
391  if (!ok)
392  {
393  freeMemory();
394  states_.clear();
395  }
396  return ok;
397 }
398 
399 void ompl::geometric::PathGeometric::overlay(const PathGeometric &over, unsigned int startIndex)
400 {
401  if (startIndex > states_.size())
402  throw Exception("Index on path is out of bounds");
403  const base::StateSpacePtr &sm = over.si_->getStateSpace();
404  const base::StateSpacePtr &dm = si_->getStateSpace();
405  bool copy = !states_.empty();
406  for (unsigned int i = 0, j = startIndex ; i < over.states_.size() ; ++i, ++j)
407  {
408  if (j == states_.size())
409  {
410  base::State *s = si_->allocState();
411  if (copy)
412  si_->copyState(s, states_.back());
413  states_.push_back(s);
414  }
415 
416  copyStateData(dm, states_[j], sm, over.states_[i]);
417  }
418 }
419 
421 {
422  states_.push_back(si_->cloneState(state));
423 }
424 
426 {
427  if (path.si_->getStateSpace()->getName() == si_->getStateSpace()->getName())
428  {
429  PathGeometric copy(path);
430  states_.insert(states_.end(), copy.states_.begin(), copy.states_.end());
431  copy.states_.clear();
432  }
433  else
434  overlay(path, states_.size());
435 }
436 
438 {
439  int index = getClosestIndex(state);
440  if (index > 0)
441  {
442  if ((std::size_t)(index + 1) < states_.size())
443  {
444  double b = si_->distance(state, states_[index-1]);
445  double a = si_->distance(state, states_[index+1]);
446  if (b > a)
447  ++index;
448  }
449  for (int i = 0 ; i < index ; ++i)
450  si_->freeState(states_[i]);
451  states_.erase(states_.begin(), states_.begin() + index);
452  }
453 }
454 
456 {
457  int index = getClosestIndex(state);
458  if (index >= 0)
459  {
460  if (index > 0 && (std::size_t)(index + 1) < states_.size())
461  {
462  double b = si_->distance(state, states_[index-1]);
463  double a = si_->distance(state, states_[index+1]);
464  if (b < a)
465  --index;
466  }
467  if ((std::size_t)(index + 1) < states_.size())
468  {
469  for (std::size_t i = index + 1 ; i < states_.size() ; ++i)
470  si_->freeState(states_[i]);
471  states_.resize(index + 1);
472  }
473  }
474 }
475 
477 {
478  if (states_.empty())
479  return -1;
480 
481  int index = 0;
482  double min_d = si_->distance(states_[0], state);
483  for (std::size_t i = 1 ; i < states_.size() ; ++i)
484  {
485  double d = si_->distance(states_[i], state);
486  if (d < min_d)
487  {
488  min_d = d;
489  index = i;
490  }
491  }
492  return index;
493 }
virtual bool sample(State *state)
Sample a state. Return false in case of failure.
void interpolate(void)
Insert a number of states in a path so that the path is made up of (approximately) the states checked...
void keepAfter(const base::State *state)
Keep the part of the path that is after state (getClosestIndex() is used to find out which way-point ...
A boost shared pointer wrapper for ompl::base::StateSpace.
A boost shared pointer wrapper for ompl::base::StateSampler.
void copyFrom(const PathGeometric &other)
Copy data to this path from another path instance.
void overlay(const PathGeometric &over, unsigned int startIndex=0)
Overlay the path over on top of the current path. States are added to the current path if needed (by ...
AdvancedStateCopyOperation copyStateData(const StateSpacePtr &destS, State *dest, const StateSpacePtr &sourceS, const State *source)
Copy data from source (state from space sourceS) to dest (state from space destS) on a component by c...
virtual void printAsMatrix(std::ostream &out) const
Print the path as a real-valued matrix where the i-th row represents the i-th state along the path...
void append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
PathGeometric(const base::SpaceInformationPtr &si)
Construct a path instance for a given space information.
Definition: PathGeometric.h:60
A state sampler that only samples valid states, uniformly.
std::pair< bool, bool > checkAndRepair(unsigned int attempts)
Check if the path is valid. If it is not, attempts are made to fix the path by sampling around invali...
void subdivide(void)
Add a state at the middle of each segment.
SpaceInformationPtr si_
The space information this path is part of.
Definition: Path.h:103
bool randomValid(unsigned int attempts)
Set this path to a random valid segment. Sample attempts times for valid segments. Returns true on success.
virtual bool check(void) const
Check if the path is valid.
int getClosestIndex(const base::State *state) const
Get the index of the way-point along the path that is closest to state. Returns -1 for an empty path...
void reverse(void)
Reverse the path.
A boost shared pointer wrapper for ompl::base::SpaceInformation.
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
Definition: StateSpace.h:73
Definition of an abstract state.
Definition: State.h:50
PathGeometric & operator=(const PathGeometric &other)
Assignment operator.
The exception type for ompl.
Definition: Exception.h:47
void keepBefore(const base::State *state)
Keep the part of the path that is before state (getClosestIndex() is used to find out which way-point...
std::vector< base::State * > states_
The list of states that make up the path.
Definition of a geometric path.
Definition: PathGeometric.h:55
void random(void)
Set this path to a random segment.
void copyToReals(std::vector< double > &reals, const State *source) const
Copy all the real values from a state source to the array reals using getValueAddressAtLocation() ...
Definition: StateSpace.cpp:308
virtual bool sampleNear(State *state, const State *near, const double distance)
Sample a state near another, within specified distance. Return false, in case of failure.
virtual double length(void) const
Compute the length of a geometric path (sum of lengths of segments that make up the path) ...
void freeMemory(void)
Free the memory corresponding to the states on this path.
virtual void print(std::ostream &out) const
Print the path to a stream.
double smoothness(void) const
Compute a notion of smoothness for this path. The closer the value is to 0, the smoother the path...
double clearance(void) const
Compute the clearance of the way-points along the path (no interpolation is performed). Detailed formula follows.
void setNrAttempts(unsigned int attempts)
Finding a valid sample usually requires performing multiple attempts. This call allows setting the nu...