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/OptimizationObjective.h"
40 #include "ompl/base/ScopedState.h"
41 #include <algorithm>
42 #include <cmath>
43 #include <limits>
44 #include <boost/math/constants/constants.hpp>
45 
47 {
48  copyFrom(path);
49 }
50 
52 {
53  states_.resize(1);
54  states_[0] = si_->cloneState(state);
55 }
56 
58 {
59  states_.resize(2);
60  states_[0] = si_->cloneState(state1);
61  states_[1] = si_->cloneState(state2);
62 }
63 
65 {
66  if (this != &other)
67  {
68  freeMemory();
69  si_ = other.si_;
70  copyFrom(other);
71  }
72  return *this;
73 }
74 
76 {
77  states_.resize(other.states_.size());
78  for (unsigned int i = 0 ; i < states_.size() ; ++i)
79  states_[i] = si_->cloneState(other.states_[i]);
80 }
81 
83 {
84  for (unsigned int i = 0 ; i < states_.size() ; ++i)
85  si_->freeState(states_[i]);
86 }
87 
89 {
90  double L = 0.0;
91  for (unsigned int i = 1 ; i < states_.size() ; ++i)
92  L += si_->distance(states_[i-1], states_[i]);
93  return L;
94 }
95 
97 {
98  double L = 0.0;
99  for (unsigned int i = 1 ; i < states_.size() ; ++i)
100  L = objective.combineObjectiveCosts(L, objective.getIncrementalCost(states_[i-1], states_[i]));
101  if (!states_.empty())
102  L += objective.getTerminalCost(states_.back());
103  return L;
104 }
105 
107 {
108  double c = 0.0;
109  for (unsigned int i = 0 ; i < states_.size() ; ++i)
110  c += si_->getStateValidityChecker()->clearance(states_[i]);
111  if (states_.empty())
112  c = std::numeric_limits<double>::infinity();
113  else
114  c /= (double)states_.size();
115  return c;
116 }
117 
119 {
120  double s = 0.0;
121  if (states_.size() > 2)
122  {
123  double a = si_->distance(states_[0], states_[1]);
124  for (unsigned int i = 2 ; i < states_.size() ; ++i)
125  {
126  // view the path as a sequence of segments, and look at the triangles it forms:
127  // s1
128  // /\ s4
129  // a / \ b |
130  // / \ |
131  // /......\_______|
132  // s0 c s2 s3
133  //
134  // use Pythagoras generalized theorem to find the cos of the angle between segments a and b
135  double b = si_->distance(states_[i-1], states_[i]);
136  double c = si_->distance(states_[i-2], states_[i]);
137  double acosValue = (a*a + b*b - c*c) / (2.0*a*b);
138 
139  if (acosValue > -1.0 && acosValue < 1.0)
140  {
141  // the smoothness is actually the outside angle of the one we compute
142  double angle = (boost::math::constants::pi<double>() - acos(acosValue));
143 
144  // and we normalize by the length of the segments
145  double k = 2.0 * angle / (a + b);
146  s += k * k;
147  }
148  a = b;
149  }
150  }
151  return s;
152 }
153 
155 {
156  bool result = true;
157  if (states_.size() > 0)
158  {
159  if (si_->isValid(states_[0]))
160  {
161  int last = states_.size() - 1;
162  for (int j = 0 ; result && j < last ; ++j)
163  if (!si_->checkMotion(states_[j], states_[j + 1]))
164  result = false;
165  }
166  else
167  result = false;
168  }
169 
170  return result;
171 }
172 
173 void ompl::geometric::PathGeometric::print(std::ostream &out) const
174 {
175  out << "Geometric path with " << states_.size() << " states" << std::endl;
176  for (unsigned int i = 0 ; i < states_.size() ; ++i)
177  si_->printState(states_[i], out);
178  out << std::endl;
179 }
180 
181 std::pair<bool, bool> ompl::geometric::PathGeometric::checkAndRepair(unsigned int attempts)
182 {
183  if (states_.empty())
184  return std::make_pair(true, true);
185  if (states_.size() == 1)
186  {
187  bool result = si_->isValid(states_[0]);
188  return std::make_pair(result, result);
189  }
190 
191  // a path with invalid endpoints cannot be fixed; planners should not return such paths anyway
192  const int n1 = states_.size() - 1;
193  if (!si_->isValid(states_[0]) || !si_->isValid(states_[n1]))
194  return std::make_pair(false, false);
195 
196  base::State *temp = NULL;
197  base::UniformValidStateSampler *uvss = NULL;
198  bool result = true;
199 
200  for (int i = 1 ; i < n1 ; ++i)
201  if (!si_->checkMotion(states_[i-1], states_[i]))
202  {
203  // we now compute a state around which to sample
204  if (!temp)
205  temp = si_->allocState();
206  if (!uvss)
207  {
208  uvss = new base::UniformValidStateSampler(si_.get());
209  uvss->setNrAttempts(attempts);
210  }
211 
212  // and a radius of sampling around that state
213  double radius = 0.0;
214 
215  if (si_->isValid(states_[i]))
216  {
217  si_->copyState(temp, states_[i]);
218  radius = si_->distance(states_[i-1], states_[i]);
219  }
220  else
221  {
222  unsigned int nextValid = n1;
223  for (int j = i + 1 ; j < n1 ; ++j)
224  if (si_->isValid(states_[j]))
225  {
226  nextValid = j;
227  break;
228  }
229  // we know nextValid will be initialised because n1 is certainly valid.
230  si_->getStateSpace()->interpolate(states_[i - 1], states_[nextValid], 0.5, temp);
231  radius = std::max(si_->distance(states_[i-1], temp), si_->distance(states_[i-1], states_[i]));
232  }
233 
234  bool success = false;
235 
236  for (unsigned int a = 0 ; a < attempts ; ++a)
237  if (uvss->sampleNear(states_[i], temp, radius))
238  {
239  if (si_->checkMotion(states_[i-1], states_[i]))
240  {
241  success = true;
242  break;
243  }
244  }
245  else
246  break;
247  if (!success)
248  {
249  result = false;
250  break;
251  }
252  }
253 
254  // free potentially allocated memory
255  if (temp)
256  si_->freeState(temp);
257  bool originalValid = uvss == NULL;
258  if (uvss)
259  delete uvss;
260 
261  return std::make_pair(originalValid, result);
262 }
263 
265 {
266  if (states_.size() < 2)
267  return;
268  std::vector<base::State*> newStates(1, states_[0]);
269  for (unsigned int i = 1 ; i < states_.size() ; ++i)
270  {
271  base::State *temp = si_->allocState();
272  si_->getStateSpace()->interpolate(newStates.back(), states_[i], 0.5, temp);
273  newStates.push_back(temp);
274  newStates.push_back(states_[i]);
275  }
276  states_.swap(newStates);
277 }
278 
280 {
281  unsigned int n = 0;
282  const int n1 = states_.size() - 1;
283  for (int i = 0 ; i < n1 ; ++i)
284  n += si_->getStateSpace()->validSegmentCount(states_[i], states_[i + 1]);
285  interpolate(n);
286 }
287 
288 void ompl::geometric::PathGeometric::interpolate(unsigned int requestCount)
289 {
290  if (requestCount < states_.size() || states_.size() < 2)
291  return;
292 
293  unsigned int count = requestCount;
294 
295  // the remaining length of the path we need to add states along
296  double remainingLength = length();
297 
298  // the new array of states this path will have
299  std::vector<base::State*> newStates;
300  const int n1 = states_.size() - 1;
301 
302  for (int i = 0 ; i < n1 ; ++i)
303  {
304  base::State *s1 = states_[i];
305  base::State *s2 = states_[i + 1];
306 
307  newStates.push_back(s1);
308 
309  // the maximum number of states that can be added on the current motion (without its endpoints)
310  // such that we can at least fit the remaining states
311  int maxNStates = count + i - states_.size();
312 
313  if (maxNStates > 0)
314  {
315  // compute an approximate number of states the following segment needs to contain; this includes endpoints
316  double segmentLength = si_->distance(s1, s2);
317  int ns = i + 1 == n1 ? maxNStates + 2 : (int)floor(0.5 + (double)count * segmentLength / remainingLength) + 1;
318 
319  // if more than endpoints are needed
320  if (ns > 2)
321  {
322  ns -= 2; // subtract endpoints
323 
324  // make sure we don't add too many states
325  if (ns > maxNStates)
326  ns = maxNStates;
327 
328  // compute intermediate states
329  std::vector<base::State*> block;
330  unsigned int ans = si_->getMotionStates(s1, s2, block, ns, false, true);
331  // sanity checks
332  if ((int)ans != ns || block.size() != ans)
333  throw Exception("Internal error in path interpolation. Incorrect number of intermediate states. Please contact the developers.");
334 
335  newStates.insert(newStates.end(), block.begin(), block.end());
336  }
337  else
338  ns = 0;
339 
340  // update what remains to be done
341  count -= (ns + 1);
342  remainingLength -= segmentLength;
343  }
344  else
345  count--;
346  }
347 
348  // add the last state
349  newStates.push_back(states_[n1]);
350  states_.swap(newStates);
351  if (requestCount != states_.size())
352  throw Exception("Internal error in path interpolation. This should never happen. Please contact the developers.");
353 }
354 
356 {
357  std::reverse(states_.begin(), states_.end());
358 }
359 
361 {
362  freeMemory();
363  states_.resize(2);
364  states_[0] = si_->allocState();
365  states_[1] = si_->allocState();
366  base::StateSamplerPtr ss = si_->allocStateSampler();
367  ss->sampleUniform(states_[0]);
368  ss->sampleUniform(states_[1]);
369 }
370 
372 {
373  freeMemory();
374  states_.resize(2);
375  states_[0] = si_->allocState();
376  states_[1] = si_->allocState();
378  uvss->setNrAttempts(attempts);
379  bool ok = false;
380  for (unsigned int i = 0 ; i < attempts ; ++i)
381  {
382  if (uvss->sample(states_[0]) && uvss->sample(states_[1]))
383  if (si_->checkMotion(states_[0], states_[1]))
384  {
385  ok = true;
386  break;
387  }
388  }
389  delete uvss;
390  if (!ok)
391  {
392  freeMemory();
393  states_.clear();
394  }
395  return ok;
396 }
397 
398 void ompl::geometric::PathGeometric::overlay(const PathGeometric &over, unsigned int startIndex)
399 {
400  if (startIndex > states_.size())
401  throw Exception("Index on path is out of bounds");
402  const base::StateSpacePtr &sm = over.si_->getStateSpace();
403  const base::StateSpacePtr &dm = si_->getStateSpace();
404  bool copy = !states_.empty();
405  for (unsigned int i = 0, j = startIndex ; i < over.states_.size() ; ++i, ++j)
406  {
407  if (j == states_.size())
408  {
409  base::State *s = si_->allocState();
410  if (copy)
411  si_->copyState(s, states_.back());
412  states_.push_back(s);
413  }
414 
415  copyStateData(dm, states_[j], sm, over.states_[i]);
416  }
417 }
418 
420 {
421  states_.push_back(si_->cloneState(state));
422 }
423 
425 {
426  if (path.si_->getStateSpace()->getName() == si_->getStateSpace()->getName())
427  {
428  PathGeometric copy(path);
429  states_.insert(states_.end(), copy.states_.begin(), copy.states_.end());
430  copy.states_.clear();
431  }
432  else
433  overlay(path, states_.size());
434 }
435 
437 {
438  int index = getClosestIndex(state);
439  if (index > 0)
440  {
441  if ((std::size_t)(index + 1) < states_.size())
442  {
443  double b = si_->distance(state, states_[index-1]);
444  double a = si_->distance(state, states_[index+1]);
445  if (b > a)
446  ++index;
447  }
448  for (int i = 0 ; i < index ; ++i)
449  si_->freeState(states_[i]);
450  states_.erase(states_.begin(), states_.begin() + index);
451  }
452 }
453 
455 {
456  int index = getClosestIndex(state);
457  if (index >= 0)
458  {
459  if (index > 0 && (std::size_t)(index + 1) < states_.size())
460  {
461  double b = si_->distance(state, states_[index-1]);
462  double a = si_->distance(state, states_[index+1]);
463  if (b < a)
464  --index;
465  }
466  if ((std::size_t)(index + 1) < states_.size())
467  {
468  for (std::size_t i = index + 1 ; i < states_.size() ; ++i)
469  si_->freeState(states_[i]);
470  states_.resize(index + 1);
471  }
472  }
473 }
474 
476 {
477  if (states_.empty())
478  return -1;
479 
480  int index = 0;
481  double min_d = si_->distance(states_[0], state);
482  for (std::size_t i = 1 ; i < states_.size() ; ++i)
483  {
484  double d = si_->distance(states_[i], state);
485  if (d < min_d)
486  {
487  min_d = d;
488  index = i;
489  }
490  }
491  return index;
492 }