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  if (states_.empty()) return opt->identityCost();
91  // Compute path cost by accumulating the cost along the path
92  base::Cost cost(opt->initialCost(states_.front()));
93  for (std::size_t i = 1; i < states_.size(); ++i)
94  cost = opt->combineCosts(cost, opt->motionCost(states_[i - 1], states_[i]));
95  cost = opt->combineCosts(cost, opt->terminalCost(states_.back()));
96  return cost;
97 }
98 
100 {
101  double L = 0.0;
102  for (unsigned int i = 1 ; i < states_.size() ; ++i)
103  L += si_->distance(states_[i-1], states_[i]);
104  return L;
105 }
106 
108 {
109  double c = 0.0;
110  for (unsigned int i = 0 ; i < states_.size() ; ++i)
111  c += si_->getStateValidityChecker()->clearance(states_[i]);
112  if (states_.empty())
113  c = std::numeric_limits<double>::infinity();
114  else
115  c /= (double)states_.size();
116  return c;
117 }
118 
120 {
121  double s = 0.0;
122  if (states_.size() > 2)
123  {
124  double a = si_->distance(states_[0], states_[1]);
125  for (unsigned int i = 2 ; i < states_.size() ; ++i)
126  {
127  // view the path as a sequence of segments, and look at the triangles it forms:
128  // s1
129  // /\ s4
130  // a / \ b |
131  // / \ |
132  // /......\_______|
133  // s0 c s2 s3
134  //
135  // use Pythagoras generalized theorem to find the cos of the angle between segments a and b
136  double b = si_->distance(states_[i-1], states_[i]);
137  double c = si_->distance(states_[i-2], states_[i]);
138  double acosValue = (a*a + b*b - c*c) / (2.0*a*b);
139 
140  if (acosValue > -1.0 && acosValue < 1.0)
141  {
142  // the smoothness is actually the outside angle of the one we compute
143  double angle = (boost::math::constants::pi<double>() - acos(acosValue));
144 
145  // and we normalize by the length of the segments
146  double k = 2.0 * angle / (a + b);
147  s += k * k;
148  }
149  a = b;
150  }
151  }
152  return s;
153 }
154 
156 {
157  bool result = true;
158  if (states_.size() > 0)
159  {
160  if (si_->isValid(states_[0]))
161  {
162  int last = states_.size() - 1;
163  for (int j = 0 ; result && j < last ; ++j)
164  if (!si_->checkMotion(states_[j], states_[j + 1]))
165  result = false;
166  }
167  else
168  result = false;
169  }
170 
171  return result;
172 }
173 
174 void ompl::geometric::PathGeometric::print(std::ostream &out) const
175 {
176  out << "Geometric path with " << states_.size() << " states" << std::endl;
177  for (unsigned int i = 0 ; i < states_.size() ; ++i)
178  si_->printState(states_[i], out);
179  out << std::endl;
180 }
182 {
183  const base::StateSpace* space(si_->getStateSpace().get());
184  std::vector<double> reals;
185  for (unsigned int i = 0 ; i < states_.size() ; ++i)
186  {
187  space->copyToReals(reals, states_[i]);
188  std::copy(reals.begin(), reals.end(), std::ostream_iterator<double>(out, " "));
189  out << std::endl;
190  }
191  out << std::endl;
192 }
193 
194 std::pair<bool, bool> ompl::geometric::PathGeometric::checkAndRepair(unsigned int attempts)
195 {
196  if (states_.empty())
197  return std::make_pair(true, true);
198  if (states_.size() == 1)
199  {
200  bool result = si_->isValid(states_[0]);
201  return std::make_pair(result, result);
202  }
203 
204  // a path with invalid endpoints cannot be fixed; planners should not return such paths anyway
205  const int n1 = states_.size() - 1;
206  if (!si_->isValid(states_[0]) || !si_->isValid(states_[n1]))
207  return std::make_pair(false, false);
208 
209  base::State *temp = NULL;
210  base::UniformValidStateSampler *uvss = NULL;
211  bool result = true;
212 
213  for (int i = 1 ; i < n1 ; ++i)
214  if (!si_->checkMotion(states_[i-1], states_[i]))
215  {
216  // we now compute a state around which to sample
217  if (!temp)
218  temp = si_->allocState();
219  if (!uvss)
220  {
221  uvss = new base::UniformValidStateSampler(si_.get());
222  uvss->setNrAttempts(attempts);
223  }
224 
225  // and a radius of sampling around that state
226  double radius = 0.0;
227 
228  if (si_->isValid(states_[i]))
229  {
230  si_->copyState(temp, states_[i]);
231  radius = si_->distance(states_[i-1], states_[i]);
232  }
233  else
234  {
235  unsigned int nextValid = n1;
236  for (int j = i + 1 ; j < n1 ; ++j)
237  if (si_->isValid(states_[j]))
238  {
239  nextValid = j;
240  break;
241  }
242  // we know nextValid will be initialised because n1 is certainly valid.
243  si_->getStateSpace()->interpolate(states_[i - 1], states_[nextValid], 0.5, temp);
244  radius = std::max(si_->distance(states_[i-1], temp), si_->distance(states_[i-1], states_[i]));
245  }
246 
247  bool success = false;
248 
249  for (unsigned int a = 0 ; a < attempts ; ++a)
250  if (uvss->sampleNear(states_[i], temp, radius))
251  {
252  if (si_->checkMotion(states_[i-1], states_[i]))
253  {
254  success = true;
255  break;
256  }
257  }
258  else
259  break;
260  if (!success)
261  {
262  result = false;
263  break;
264  }
265  }
266 
267  // free potentially allocated memory
268  if (temp)
269  si_->freeState(temp);
270  bool originalValid = uvss == NULL;
271  if (uvss)
272  delete uvss;
273 
274  return std::make_pair(originalValid, result);
275 }
276 
278 {
279  if (states_.size() < 2)
280  return;
281  std::vector<base::State*> newStates(1, states_[0]);
282  for (unsigned int i = 1 ; i < states_.size() ; ++i)
283  {
284  base::State *temp = si_->allocState();
285  si_->getStateSpace()->interpolate(newStates.back(), states_[i], 0.5, temp);
286  newStates.push_back(temp);
287  newStates.push_back(states_[i]);
288  }
289  states_.swap(newStates);
290 }
291 
293 {
294  unsigned int n = 0;
295  const int n1 = states_.size() - 1;
296  for (int i = 0 ; i < n1 ; ++i)
297  n += si_->getStateSpace()->validSegmentCount(states_[i], states_[i + 1]);
298  interpolate(n);
299 }
300 
301 void ompl::geometric::PathGeometric::interpolate(unsigned int requestCount)
302 {
303  if (requestCount < states_.size() || states_.size() < 2)
304  return;
305 
306  unsigned int count = requestCount;
307 
308  // the remaining length of the path we need to add states along
309  double remainingLength = length();
310 
311  // the new array of states this path will have
312  std::vector<base::State*> newStates;
313  const int n1 = states_.size() - 1;
314 
315  for (int i = 0 ; i < n1 ; ++i)
316  {
317  base::State *s1 = states_[i];
318  base::State *s2 = states_[i + 1];
319 
320  newStates.push_back(s1);
321 
322  // the maximum number of states that can be added on the current motion (without its endpoints)
323  // such that we can at least fit the remaining states
324  int maxNStates = count + i - states_.size();
325 
326  if (maxNStates > 0)
327  {
328  // compute an approximate number of states the following segment needs to contain; this includes endpoints
329  double segmentLength = si_->distance(s1, s2);
330  int ns = i + 1 == n1 ? maxNStates + 2 : (int)floor(0.5 + (double)count * segmentLength / remainingLength) + 1;
331 
332  // if more than endpoints are needed
333  if (ns > 2)
334  {
335  ns -= 2; // subtract endpoints
336 
337  // make sure we don't add too many states
338  if (ns > maxNStates)
339  ns = maxNStates;
340 
341  // compute intermediate states
342  std::vector<base::State*> block;
343  unsigned int ans = si_->getMotionStates(s1, s2, block, ns, false, true);
344  // sanity checks
345  if ((int)ans != ns || block.size() != ans)
346  throw Exception("Internal error in path interpolation. Incorrect number of intermediate states. Please contact the developers.");
347 
348  newStates.insert(newStates.end(), block.begin(), block.end());
349  }
350  else
351  ns = 0;
352 
353  // update what remains to be done
354  count -= (ns + 1);
355  remainingLength -= segmentLength;
356  }
357  else
358  count--;
359  }
360 
361  // add the last state
362  newStates.push_back(states_[n1]);
363  states_.swap(newStates);
364  if (requestCount != states_.size())
365  throw Exception("Internal error in path interpolation. This should never happen. Please contact the developers.");
366 }
367 
369 {
370  std::reverse(states_.begin(), states_.end());
371 }
372 
374 {
375  freeMemory();
376  states_.resize(2);
377  states_[0] = si_->allocState();
378  states_[1] = si_->allocState();
379  base::StateSamplerPtr ss = si_->allocStateSampler();
380  ss->sampleUniform(states_[0]);
381  ss->sampleUniform(states_[1]);
382 }
383 
385 {
386  freeMemory();
387  states_.resize(2);
388  states_[0] = si_->allocState();
389  states_[1] = si_->allocState();
391  uvss->setNrAttempts(attempts);
392  bool ok = false;
393  for (unsigned int i = 0 ; i < attempts ; ++i)
394  {
395  if (uvss->sample(states_[0]) && uvss->sample(states_[1]))
396  if (si_->checkMotion(states_[0], states_[1]))
397  {
398  ok = true;
399  break;
400  }
401  }
402  delete uvss;
403  if (!ok)
404  {
405  freeMemory();
406  states_.clear();
407  }
408  return ok;
409 }
410 
411 void ompl::geometric::PathGeometric::overlay(const PathGeometric &over, unsigned int startIndex)
412 {
413  if (startIndex > states_.size())
414  throw Exception("Index on path is out of bounds");
415  const base::StateSpacePtr &sm = over.si_->getStateSpace();
416  const base::StateSpacePtr &dm = si_->getStateSpace();
417  bool copy = !states_.empty();
418  for (unsigned int i = 0, j = startIndex ; i < over.states_.size() ; ++i, ++j)
419  {
420  if (j == states_.size())
421  {
422  base::State *s = si_->allocState();
423  if (copy)
424  si_->copyState(s, states_.back());
425  states_.push_back(s);
426  }
427 
428  copyStateData(dm, states_[j], sm, over.states_[i]);
429  }
430 }
431 
433 {
434  states_.push_back(si_->cloneState(state));
435 }
436 
438 {
439  if (path.si_->getStateSpace()->getName() == si_->getStateSpace()->getName())
440  {
441  PathGeometric copy(path);
442  states_.insert(states_.end(), copy.states_.begin(), copy.states_.end());
443  copy.states_.clear();
444  }
445  else
446  overlay(path, states_.size());
447 }
448 
450 {
451  states_.insert(states_.begin(), si_->cloneState(state));
452 }
453 
455 {
456  int index = getClosestIndex(state);
457  if (index > 0)
458  {
459  if ((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  for (int i = 0 ; i < index ; ++i)
467  si_->freeState(states_[i]);
468  states_.erase(states_.begin(), states_.begin() + index);
469  }
470 }
471 
473 {
474  int index = getClosestIndex(state);
475  if (index >= 0)
476  {
477  if (index > 0 && (std::size_t)(index + 1) < states_.size())
478  {
479  double b = si_->distance(state, states_[index-1]);
480  double a = si_->distance(state, states_[index+1]);
481  if (b < a)
482  --index;
483  }
484  if ((std::size_t)(index + 1) < states_.size())
485  {
486  for (std::size_t i = index + 1 ; i < states_.size() ; ++i)
487  si_->freeState(states_[i]);
488  states_.resize(index + 1);
489  }
490  }
491 }
492 
494 {
495  if (states_.empty())
496  return -1;
497 
498  int index = 0;
499  double min_d = si_->distance(states_[0], state);
500  for (std::size_t i = 1 ; i < states_.size() ; ++i)
501  {
502  double d = si_->distance(states_[i], state);
503  if (d < min_d)
504  {
505  min_d = d;
506  index = i;
507  }
508  }
509  return index;
510 }
virtual bool sample(State *state)
Sample a state. Return false in case of failure.
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 ...
void freeMemory()
Free the memory corresponding to the states on this path.
void interpolate()
Insert a number of states in a path so that the path is made up of (approximately) the states checked...
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...
void random()
Set this path to a random segment.
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:65
virtual double length() const
Compute the length of a geometric path (sum of lengths of segments that make up the path) ...
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...
SpaceInformationPtr si_
The space information this path is part of.
Definition: Path.h:103
virtual base::Cost cost(const base::OptimizationObjectivePtr &obj) const
The sum of the costs for the sequence of segments that make up the path, computed using OptimizationO...
double clearance() const
Compute the clearance of the way-points along the path (no interpolation is performed). Detailed formula follows.
bool randomValid(unsigned int attempts)
Set this path to a random valid segment. Sample attempts times for valid segments. Returns true on success.
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()
Reverse the path.
A boost shared pointer wrapper for ompl::base::SpaceInformation.
void prepend(const base::State *state)
Prepend state to the start of this path. The memory for state is copied.
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
A boost shared pointer wrapper for ompl::base::OptimizationObjective.
double smoothness() const
Compute a notion of smoothness for this path. The closer the value is to 0, the smoother the path...
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...
void subdivide()
Add a state at the middle of each segment.
std::vector< base::State * > states_
The list of states that make up the path.
Definition of a geometric path.
Definition: PathGeometric.h:60
virtual bool check() const
Check if the path is valid.
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:319
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.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:47
virtual void print(std::ostream &out) const
Print the path to a stream.
void setNrAttempts(unsigned int attempts)
Finding a valid sample usually requires performing multiple attempts. This call allows setting the nu...