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 }
169 
170 std::pair<bool, bool> ompl::geometric::PathGeometric::checkAndRepair(unsigned int attempts)
171 {
172  if (states_.empty())
173  return std::make_pair(true, true);
174  if (states_.size() == 1)
175  {
176  bool result = si_->isValid(states_[0]);
177  return std::make_pair(result, result);
178  }
179 
180  // a path with invalid endpoints cannot be fixed; planners should not return such paths anyway
181  const int n1 = states_.size() - 1;
182  if (!si_->isValid(states_[0]) || !si_->isValid(states_[n1]))
183  return std::make_pair(false, false);
184 
185  base::State *temp = NULL;
186  base::UniformValidStateSampler *uvss = NULL;
187  bool result = true;
188 
189  for (int i = 1 ; i < n1 ; ++i)
190  if (!si_->checkMotion(states_[i-1], states_[i]))
191  {
192  // we now compute a state around which to sample
193  if (!temp)
194  temp = si_->allocState();
195  if (!uvss)
196  {
197  uvss = new base::UniformValidStateSampler(si_.get());
198  uvss->setNrAttempts(attempts);
199  }
200 
201  // and a radius of sampling around that state
202  double radius = 0.0;
203 
204  if (si_->isValid(states_[i]))
205  {
206  si_->copyState(temp, states_[i]);
207  radius = si_->distance(states_[i-1], states_[i]);
208  }
209  else
210  {
211  unsigned int nextValid = n1;
212  for (int j = i + 1 ; j < n1 ; ++j)
213  if (si_->isValid(states_[j]))
214  {
215  nextValid = j;
216  break;
217  }
218  // we know nextValid will be initialised because n1 is certainly valid.
219  si_->getStateSpace()->interpolate(states_[i - 1], states_[nextValid], 0.5, temp);
220  radius = std::max(si_->distance(states_[i-1], temp), si_->distance(states_[i-1], states_[i]));
221  }
222 
223  bool success = false;
224 
225  for (unsigned int a = 0 ; a < attempts ; ++a)
226  if (uvss->sampleNear(states_[i], temp, radius))
227  {
228  if (si_->checkMotion(states_[i-1], states_[i]))
229  {
230  success = true;
231  break;
232  }
233  }
234  else
235  break;
236  if (!success)
237  {
238  result = false;
239  break;
240  }
241  }
242 
243  // free potentially allocated memory
244  if (temp)
245  si_->freeState(temp);
246  bool originalValid = uvss == NULL;
247  if (uvss)
248  delete uvss;
249 
250  return std::make_pair(originalValid, result);
251 }
252 
254 {
255  if (states_.size() < 2)
256  return;
257  std::vector<base::State*> newStates(1, states_[0]);
258  for (unsigned int i = 1 ; i < states_.size() ; ++i)
259  {
260  base::State *temp = si_->allocState();
261  si_->getStateSpace()->interpolate(newStates.back(), states_[i], 0.5, temp);
262  newStates.push_back(temp);
263  newStates.push_back(states_[i]);
264  }
265  states_.swap(newStates);
266 }
267 
269 {
270  unsigned int n = 0;
271  const int n1 = states_.size() - 1;
272  for (int i = 0 ; i < n1 ; ++i)
273  n += si_->getStateSpace()->validSegmentCount(states_[i], states_[i + 1]);
274  interpolate(n);
275 }
276 
277 void ompl::geometric::PathGeometric::interpolate(unsigned int requestCount)
278 {
279  if (requestCount < states_.size() || states_.size() < 2)
280  return;
281 
282  unsigned int count = requestCount;
283 
284  // the remaining length of the path we need to add states along
285  double remainingLength = length();
286 
287  // the new array of states this path will have
288  std::vector<base::State*> newStates;
289  const int n1 = states_.size() - 1;
290 
291  for (int i = 0 ; i < n1 ; ++i)
292  {
293  base::State *s1 = states_[i];
294  base::State *s2 = states_[i + 1];
295 
296  newStates.push_back(s1);
297 
298  // the maximum number of states that can be added on the current motion (without its endpoints)
299  // such that we can at least fit the remaining states
300  int maxNStates = count + i - states_.size();
301 
302  if (maxNStates > 0)
303  {
304  // compute an approximate number of states the following segment needs to contain; this includes endpoints
305  double segmentLength = si_->distance(s1, s2);
306  int ns = i + 1 == n1 ? maxNStates + 2 : (int)floor(0.5 + (double)count * segmentLength / remainingLength) + 1;
307 
308  // if more than endpoints are needed
309  if (ns > 2)
310  {
311  ns -= 2; // subtract endpoints
312 
313  // make sure we don't add too many states
314  if (ns > maxNStates)
315  ns = maxNStates;
316 
317  // compute intermediate states
318  std::vector<base::State*> block;
319  unsigned int ans = si_->getMotionStates(s1, s2, block, ns, false, true);
320  // sanity checks
321  if ((int)ans != ns || block.size() != ans)
322  throw Exception("Internal error in path interpolation. Incorrect number of intermediate states. Please contact the developers.");
323 
324  newStates.insert(newStates.end(), block.begin(), block.end());
325  }
326  else
327  ns = 0;
328 
329  // update what remains to be done
330  count -= (ns + 1);
331  remainingLength -= segmentLength;
332  }
333  else
334  count--;
335  }
336 
337  // add the last state
338  newStates.push_back(states_[n1]);
339  states_.swap(newStates);
340  if (requestCount != states_.size())
341  throw Exception("Internal error in path interpolation. This should never happen. Please contact the developers.");
342 }
343 
345 {
346  std::reverse(states_.begin(), states_.end());
347 }
348 
350 {
351  freeMemory();
352  states_.resize(2);
353  states_[0] = si_->allocState();
354  states_[1] = si_->allocState();
355  base::StateSamplerPtr ss = si_->allocStateSampler();
356  ss->sampleUniform(states_[0]);
357  ss->sampleUniform(states_[1]);
358 }
359 
361 {
362  freeMemory();
363  states_.resize(2);
364  states_[0] = si_->allocState();
365  states_[1] = si_->allocState();
367  uvss->setNrAttempts(attempts);
368  bool ok = false;
369  for (unsigned int i = 0 ; i < attempts ; ++i)
370  {
371  if (uvss->sample(states_[0]) && uvss->sample(states_[1]))
372  if (si_->checkMotion(states_[0], states_[1]))
373  {
374  ok = true;
375  break;
376  }
377  }
378  delete uvss;
379  if (!ok)
380  {
381  freeMemory();
382  states_.clear();
383  }
384  return ok;
385 }
386 
387 void ompl::geometric::PathGeometric::overlay(const PathGeometric &over, unsigned int startIndex)
388 {
389  if (startIndex > states_.size())
390  throw Exception("Index on path is out of bounds");
391  const base::StateSpacePtr &sm = over.si_->getStateSpace();
392  const base::StateSpacePtr &dm = si_->getStateSpace();
393  bool copy = !states_.empty();
394  for (unsigned int i = 0, j = startIndex ; i < over.states_.size() ; ++i, ++j)
395  {
396  if (j == states_.size())
397  {
398  base::State *s = si_->allocState();
399  if (copy)
400  si_->copyState(s, states_.back());
401  states_.push_back(s);
402  }
403 
404  copyStateData(dm, states_[j], sm, over.states_[i]);
405  }
406 }
407 
409 {
410  states_.push_back(si_->cloneState(state));
411 }
412 
414 {
415  if (path.si_->getStateSpace()->getName() == si_->getStateSpace()->getName())
416  {
417  PathGeometric copy(path);
418  states_.insert(states_.end(), copy.states_.begin(), copy.states_.end());
419  copy.states_.clear();
420  }
421  else
422  overlay(path, states_.size());
423 }
424 
426 {
427  int index = getClosestIndex(state);
428  if (index > 0)
429  {
430  if ((std::size_t)(index + 1) < states_.size())
431  {
432  double b = si_->distance(state, states_[index-1]);
433  double a = si_->distance(state, states_[index+1]);
434  if (b > a)
435  ++index;
436  }
437  for (int i = 0 ; i < index ; ++i)
438  si_->freeState(states_[i]);
439  states_.erase(states_.begin(), states_.begin() + index);
440  }
441 }
442 
444 {
445  int index = getClosestIndex(state);
446  if (index >= 0)
447  {
448  if (index > 0 && (std::size_t)(index + 1) < states_.size())
449  {
450  double b = si_->distance(state, states_[index-1]);
451  double a = si_->distance(state, states_[index+1]);
452  if (b < a)
453  --index;
454  }
455  if ((std::size_t)(index + 1) < states_.size())
456  {
457  for (std::size_t i = index + 1 ; i < states_.size() ; ++i)
458  si_->freeState(states_[i]);
459  states_.resize(index + 1);
460  }
461  }
462 }
463 
465 {
466  if (states_.empty())
467  return -1;
468 
469  int index = 0;
470  double min_d = si_->distance(states_[0], state);
471  for (std::size_t i = 1 ; i < states_.size() ; ++i)
472  {
473  double d = si_->distance(states_[i], state);
474  if (d < min_d)
475  {
476  min_d = d;
477  index = i;
478  }
479  }
480  return index;
481 }
482 
483 namespace ompl
484 {
485  namespace magic
486  {
489  }
490 }
491 
492 
493 void ompl::geometric::PathGeometric::computeFastTimeParametrization(double maxVel, double maxAcc, std::vector<double> &times, unsigned int maxSteps) const
494 {
495  // This implementation greatly benefited from discussions with Kenneth Anderson (http://sites.google.com/site/kennethaanderson/
496 
497  if (states_.empty())
498  {
499  times.clear();
500  return;
501  }
502  if (states_.size() == 1)
503  {
504  times.resize(1);
505  times[0] = 0.0;
506  return;
507  }
508  if (states_.size() == 2)
509  {
510  double d = si_->distance(states_[0], states_[1]);
511  times.resize(2);
512  times[0] = 0.0;
513  times[1] = std::max(2.0 * d / maxVel, sqrt(2.0 * d / maxAcc));
514  return;
515  }
516 
517  // compute the lengths of the segments along the path
518  std::vector<double> L(states_.size() - 1);
519  for (std::size_t i = 0 ; i < L.size() ; ++i)
520  L[i] = si_->distance(states_[i], states_[i + 1]);
521 
522  // the time for the first state is 0
523  times.resize(states_.size());
524  times[0] = 0.0;
525 
526  // the velocity is maximum everywhere, except at endpoints
527  std::vector<double> vel(states_.size(), maxVel);
528  vel.front() = vel.back() = 0.0;
529 
530  // compute the cosine of the angle between consecutive segments
531  // and scale the maximum desired velocity by that value
532  // this has the effect of stopping at very sharp turns
533  // and ignoring straight lines
534  for (std::size_t i = 1 ; i < L.size() ; ++i)
535  {
536  double a = L[i-1];
537  double b = L[i];
538  double c = si_->distance(states_[i-1], states_[i+1]);
539  double acosValue = (a*a + b*b - c*c) / (2.0*a*b);
540  vel[i] *= std::min(1.0, fabs(acosValue));
541  }
542 
543  bool change = true;
544  unsigned int steps = 0;
545  while (change && steps <= maxSteps)
546  {
547  ++steps;
548  change = false;
549 
550  // compute the time points for every state, using the currently considered velocity
551  for (std::size_t i = 1 ; i < times.size() ; ++i)
552  times[i] = times[i - 1] + (2.0 * L[i-1]) / (vel[i-1] + vel[i]);
553 
554  for (std::size_t i = 0 ; i < L.size() ; ++i)
555  {
556  double acc = (vel[i + 1] - vel[i]) / (times[i + 1] - times[i]);
557  if (acc > maxAcc)
558  {
560  change = true;
561  }
562  else
563  if (acc < -maxAcc)
564  {
566  change = true;
567  }
568  }
569 
570  if (change)
571  for (int i = L.size() - 1 ; i >= 0 ; --i)
572  {
573  double acc = (vel[i + 1] - vel[i]) / (times[i + 1] - times[i]);
574  if (acc > maxAcc)
575  {
577  change = true;
578  }
579  else
580  if (acc < -maxAcc)
581  {
583  change = true;
584  }
585  }
586  }
587 
588 }