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 
51 ompl::geometric::PathGeometric::PathGeometric(const base::SpaceInformationPtr &si, const base::State *state)
52  : base::Path(si)
53 {
54  states_.resize(1);
55  states_[0] = si_->cloneState(state);
56 }
57 
58 ompl::geometric::PathGeometric::PathGeometric(const base::SpaceInformationPtr &si, const base::State *state1,
59  const base::State *state2)
60  : base::Path(si)
61 {
62  states_.resize(2);
63  states_[0] = si_->cloneState(state1);
64  states_[1] = si_->cloneState(state2);
65 }
66 
67 ompl::geometric::PathGeometric::PathGeometric(const base::SpaceInformationPtr &si,
68  std::vector<const base::State *> &states)
69  : base::Path(si)
70 {
71  for (unsigned int k = 0; k < states.size(); k++)
72  {
73  this->append(states.at(k));
74  }
75 }
76 
78 {
79  if (this != &other)
80  {
81  freeMemory();
82  si_ = other.si_;
83  copyFrom(other);
84  }
85  return *this;
86 }
87 
89 {
90  states_.resize(other.states_.size());
91  for (unsigned int i = 0; i < states_.size(); ++i)
92  states_[i] = si_->cloneState(other.states_[i]);
93 }
94 
96 {
97  for (auto &state : states_)
98  si_->freeState(state);
99 }
100 
101 ompl::base::Cost ompl::geometric::PathGeometric::cost(const base::OptimizationObjectivePtr &opt) const
102 {
103  if (states_.empty())
104  return opt->identityCost();
105  // Compute path cost by accumulating the cost along the path
106  base::Cost cost(opt->initialCost(states_.front()));
107  for (std::size_t i = 1; i < states_.size(); ++i)
108  cost = opt->combineCosts(cost, opt->motionCost(states_[i - 1], states_[i]));
109  cost = opt->combineCosts(cost, opt->terminalCost(states_.back()));
110  return cost;
111 }
112 
114 {
115  double L = 0.0;
116  for (unsigned int i = 1; i < states_.size(); ++i)
117  L += si_->distance(states_[i - 1], states_[i]);
118  return L;
119 }
120 
122 {
123  double c = 0.0;
124  for (auto state : states_)
125  c += si_->getStateValidityChecker()->clearance(state);
126  if (states_.empty())
127  c = std::numeric_limits<double>::infinity();
128  else
129  c /= (double)states_.size();
130  return c;
131 }
132 
134 {
135  double s = 0.0;
136  if (states_.size() > 2)
137  {
138  double a = si_->distance(states_[0], states_[1]);
139  for (unsigned int i = 2; i < states_.size(); ++i)
140  {
141  // view the path as a sequence of segments, and look at the triangles it forms:
142  // s1
143  // /\ s4
144  // a / \ b |
145  // / \ |
146  // /......\_______|
147  // s0 c s2 s3
148  //
149  // use Pythagoras generalized theorem to find the cos of the angle between segments a and b
150  double b = si_->distance(states_[i - 1], states_[i]);
151  double c = si_->distance(states_[i - 2], states_[i]);
152  double acosValue = (a * a + b * b - c * c) / (2.0 * a * b);
153 
154  if (acosValue > -1.0 && acosValue < 1.0)
155  {
156  // the smoothness is actually the outside angle of the one we compute
157  double angle = (boost::math::constants::pi<double>() - acos(acosValue));
158 
159  // and we normalize by the length of the segments
160  double k = 2.0 * angle / (a + b);
161  s += k * k;
162  }
163  a = b;
164  }
165  }
166  return s;
167 }
168 
170 {
171  // make sure state validity checker is set
172  if (!si_->isSetup())
173  si_->setup();
174 
175  bool result = true;
176  if (states_.size() > 0)
177  {
178  if (si_->isValid(states_[0]))
179  {
180  int last = states_.size() - 1;
181  for (int j = 0; result && j < last; ++j)
182  if (!si_->checkMotion(states_[j], states_[j + 1]))
183  result = false;
184  }
185  else
186  result = false;
187  }
188 
189  return result;
190 }
191 
192 void ompl::geometric::PathGeometric::print(std::ostream &out) const
193 {
194  out << "Geometric path with " << states_.size() << " states" << std::endl;
195  for (auto state : states_)
196  si_->printState(state, out);
197  out << std::endl;
198 }
200 {
201  const base::StateSpace *space(si_->getStateSpace().get());
202  std::vector<double> reals;
203  for (auto state : states_)
204  {
205  space->copyToReals(reals, state);
206  std::copy(reals.begin(), reals.end(), std::ostream_iterator<double>(out, " "));
207  out << std::endl;
208  }
209  out << std::endl;
210 }
211 
212 std::pair<bool, bool> ompl::geometric::PathGeometric::checkAndRepair(unsigned int attempts)
213 {
214  // make sure state validity checker is set
215  if (!si_->isSetup())
216  si_->setup();
217 
218  if (states_.empty())
219  return std::make_pair(true, true);
220 
221  if (states_.size() == 1)
222  {
223  bool result = si_->isValid(states_[0]);
224  return std::make_pair(result, result);
225  }
226  else if (states_.size() == 2)
227  {
228  bool result = si_->checkMotion(states_[0], states_[1]);
229  return std::make_pair(result, result);
230  }
231 
232  // a path with invalid endpoints cannot be fixed; planners should not return such paths anyway
233  const int n1 = states_.size() - 1;
234  if (!si_->isValid(states_[0]) || !si_->isValid(states_[n1]))
235  return std::make_pair(false, false);
236 
237  base::State *temp = nullptr;
238  base::UniformValidStateSampler *uvss = nullptr;
239  bool result = true;
240 
241  for (int i = 1; i < n1; ++i)
242  if (!si_->checkMotion(states_[i - 1], states_[i]) ||
243  // the penultimate state in the path needs an additional check:
244  // the motion between that state and the last state needs to be
245  // valid as well since we cannot change the last state.
246  (i == n1 - 1 && !si_->checkMotion(states_[i], states_[i + 1])))
247  {
248  // we now compute a state around which to sample
249  if (!temp)
250  temp = si_->allocState();
251  if (!uvss)
252  {
253  uvss = new base::UniformValidStateSampler(si_.get());
254  uvss->setNrAttempts(attempts);
255  }
256 
257  // and a radius of sampling around that state
258  double radius = 0.0;
259 
260  if (si_->isValid(states_[i]))
261  {
262  si_->copyState(temp, states_[i]);
263  radius = si_->distance(states_[i - 1], states_[i]);
264  }
265  else
266  {
267  unsigned int nextValid = n1 - 1;
268  for (int j = i + 1; j < n1; ++j)
269  if (si_->isValid(states_[j]))
270  {
271  nextValid = j;
272  break;
273  }
274  // we know nextValid will be initialised because n1 - 1 is certainly valid.
275  si_->getStateSpace()->interpolate(states_[i - 1], states_[nextValid], 0.5, temp);
276  radius = std::max(si_->distance(states_[i - 1], temp), si_->distance(states_[i - 1], states_[i]));
277  }
278 
279  bool success = false;
280 
281  for (unsigned int a = 0; a < attempts; ++a)
282  if (uvss->sampleNear(states_[i], temp, radius))
283  {
284  if (si_->checkMotion(states_[i - 1], states_[i]) &&
285  // the penultimate state needs an additional check
286  // (see comment at the top of outermost for-loop)
287  (i < n1 - 1 || si_->checkMotion(states_[i], states_[i + 1])))
288  {
289  success = true;
290  break;
291  }
292  }
293  else
294  break;
295  if (!success)
296  {
297  result = false;
298  break;
299  }
300  }
301 
302  // free potentially allocated memory
303  if (temp)
304  si_->freeState(temp);
305  bool originalValid = uvss == nullptr;
306  if (uvss)
307  delete uvss;
308 
309  return std::make_pair(originalValid, result);
310 }
311 
313 {
314  if (states_.size() < 2)
315  return;
316  std::vector<base::State *> newStates(1, states_[0]);
317  for (unsigned int i = 1; i < states_.size(); ++i)
318  {
319  base::State *temp = si_->allocState();
320  si_->getStateSpace()->interpolate(newStates.back(), states_[i], 0.5, temp);
321  newStates.push_back(temp);
322  newStates.push_back(states_[i]);
323  }
324  states_.swap(newStates);
325 }
326 
328 {
329  std::vector<base::State *> newStates;
330  const int segments = states_.size() - 1;
331 
332  for (int i = 0; i < segments; ++i)
333  {
334  base::State *s1 = states_[i];
335  base::State *s2 = states_[i + 1];
336 
337  newStates.push_back(s1);
338  unsigned int n = si_->getStateSpace()->validSegmentCount(s1, s2);
339 
340  std::vector<base::State *> block;
341  si_->getMotionStates(s1, s2, block, n - 1, false, true);
342  newStates.insert(newStates.end(), block.begin(), block.end());
343  }
344  newStates.push_back(states_[segments]);
345  states_.swap(newStates);
346 }
347 
348 void ompl::geometric::PathGeometric::interpolate(unsigned int requestCount)
349 {
350  if (requestCount < states_.size() || states_.size() < 2)
351  return;
352 
353  unsigned int count = requestCount;
354 
355  // the remaining length of the path we need to add states along
356  double remainingLength = length();
357 
358  // the new array of states this path will have
359  std::vector<base::State *> newStates;
360  const int n1 = states_.size() - 1;
361 
362  for (int i = 0; i < n1; ++i)
363  {
364  base::State *s1 = states_[i];
365  base::State *s2 = states_[i + 1];
366 
367  newStates.push_back(s1);
368 
369  // the maximum number of states that can be added on the current motion (without its endpoints)
370  // such that we can at least fit the remaining states
371  int maxNStates = count + i - states_.size();
372 
373  if (maxNStates > 0)
374  {
375  // compute an approximate number of states the following segment needs to contain; this includes endpoints
376  double segmentLength = si_->distance(s1, s2);
377  int ns =
378  i + 1 == n1 ? maxNStates + 2 : (int)floor(0.5 + (double)count * segmentLength / remainingLength) + 1;
379 
380  // if more than endpoints are needed
381  if (ns > 2)
382  {
383  ns -= 2; // subtract endpoints
384 
385  // make sure we don't add too many states
386  if (ns > maxNStates)
387  ns = maxNStates;
388 
389  // compute intermediate states
390  std::vector<base::State *> block;
391  si_->getMotionStates(s1, s2, block, ns, false, true);
392  newStates.insert(newStates.end(), block.begin(), block.end());
393  }
394  else
395  ns = 0;
396 
397  // update what remains to be done
398  count -= (ns + 1);
399  remainingLength -= segmentLength;
400  }
401  else
402  count--;
403  }
404 
405  // add the last state
406  newStates.push_back(states_[n1]);
407  states_.swap(newStates);
408 }
409 
411 {
412  std::reverse(states_.begin(), states_.end());
413 }
414 
416 {
417  freeMemory();
418  states_.resize(2);
419  states_[0] = si_->allocState();
420  states_[1] = si_->allocState();
421  base::StateSamplerPtr ss = si_->allocStateSampler();
422  ss->sampleUniform(states_[0]);
423  ss->sampleUniform(states_[1]);
424 }
425 
427 {
428  freeMemory();
429  states_.resize(2);
430  states_[0] = si_->allocState();
431  states_[1] = si_->allocState();
432  base::UniformValidStateSampler uvss(si_.get());
433  uvss.setNrAttempts(attempts);
434  bool ok = false;
435  for (unsigned int i = 0; i < attempts; ++i)
436  {
437  if (uvss.sample(states_[0]) && uvss.sample(states_[1]))
438  if (si_->checkMotion(states_[0], states_[1]))
439  {
440  ok = true;
441  break;
442  }
443  }
444  if (!ok)
445  {
446  freeMemory();
447  states_.clear();
448  }
449  return ok;
450 }
451 
452 void ompl::geometric::PathGeometric::overlay(const PathGeometric &over, unsigned int startIndex)
453 {
454  if (startIndex > states_.size())
455  throw Exception("Index on path is out of bounds");
456  const base::StateSpacePtr &sm = over.si_->getStateSpace();
457  const base::StateSpacePtr &dm = si_->getStateSpace();
458  bool copy = !states_.empty();
459  for (unsigned int i = 0, j = startIndex; i < over.states_.size(); ++i, ++j)
460  {
461  if (j == states_.size())
462  {
463  base::State *s = si_->allocState();
464  if (copy)
465  si_->copyState(s, states_.back());
466  states_.push_back(s);
467  }
468 
469  copyStateData(dm, states_[j], sm, over.states_[i]);
470  }
471 }
472 
474 {
475  states_.push_back(si_->cloneState(state));
476 }
477 
479 {
480  if (path.si_->getStateSpace()->getName() == si_->getStateSpace()->getName())
481  {
482  PathGeometric copy(path);
483  states_.insert(states_.end(), copy.states_.begin(), copy.states_.end());
484  copy.states_.clear();
485  }
486  else
487  overlay(path, states_.size());
488 }
489 
491 {
492  states_.insert(states_.begin(), si_->cloneState(state));
493 }
494 
496 {
497  int index = getClosestIndex(state);
498  if (index > 0)
499  {
500  if ((std::size_t)(index + 1) < states_.size())
501  {
502  double b = si_->distance(state, states_[index - 1]);
503  double a = si_->distance(state, states_[index + 1]);
504  if (b > a)
505  ++index;
506  }
507  for (int i = 0; i < index; ++i)
508  si_->freeState(states_[i]);
509  states_.erase(states_.begin(), states_.begin() + index);
510  }
511 }
512 
514 {
515  int index = getClosestIndex(state);
516  if (index >= 0)
517  {
518  if (index > 0 && (std::size_t)(index + 1) < states_.size())
519  {
520  double b = si_->distance(state, states_[index - 1]);
521  double a = si_->distance(state, states_[index + 1]);
522  if (b < a)
523  --index;
524  }
525  if ((std::size_t)(index + 1) < states_.size())
526  {
527  for (std::size_t i = index + 1; i < states_.size(); ++i)
528  si_->freeState(states_[i]);
529  states_.resize(index + 1);
530  }
531  }
532 }
533 
535 {
536  if (states_.empty())
537  return -1;
538 
539  int index = 0;
540  double min_d = si_->distance(states_[0], state);
541  for (std::size_t i = 1; i < states_.size(); ++i)
542  {
543  double d = si_->distance(states_[i], state);
544  if (d < min_d)
545  {
546  min_d = d;
547  index = i;
548  }
549  }
550  return index;
551 }
552 
554 {
555  freeMemory();
556  states_.clear();
557 }
void reverse()
Reverse the path.
bool randomValid(unsigned int attempts)
Set this path to a random valid segment. Sample attempts times for valid segments....
bool check() const override
Check if the path is valid.
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition: StateSpace.h:134
void clear()
Remove all states and clear memory.
void prepend(const base::State *state)
Prepend state to the start of this path. The memory for state is copied.
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...
Definition of an abstract state.
Definition: State.h:113
double smoothness() const
Compute a notion of smoothness for this path. The closer the value is to 0, the smoother the path....
PathGeometric & operator=(const PathGeometric &other)
Assignment operator.
bool sampleNear(State *state, const State *near, double distance) override
Sample a state near another, within specified distance. Return false, in case of failure.
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...
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:111
bool sample(State *state) override
Sample a state. Return false in case of failure.
base::Cost cost(const base::OptimizationObjectivePtr &obj) const override
The sum of the costs for the sequence of segments that make up the path, computed using OptimizationO...
Definition of a geometric path.
Definition: PathGeometric.h:97
A state sampler that only samples valid states, uniformly.
void subdivide()
Add a state at the middle of each segment.
SpaceInformationPtr si_
The space information this path is part of.
Definition: Path.h:187
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 freeMemory()
Free the memory corresponding to the states on this path.
std::vector< base::State * > states_
The list of states that make up the path.
void setNrAttempts(unsigned int attempts)
Finding a valid sample usually requires performing multiple attempts. This call allows setting the nu...
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 append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
void interpolate()
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 ...
double length() const override
Compute the length of a geometric path (sum of lengths of segments that make up the path)
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 copyFrom(const PathGeometric &other)
Copy data to this path from another path instance.
void random()
Set this path to a random segment.
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 ...
void print(std::ostream &out) const override
Print the path to a stream.
The exception type for ompl.
Definition: Exception.h:78
double clearance() const
Compute the clearance of the way-points along the path (no interpolation is performed)....
PathGeometric(const base::SpaceInformationPtr &si)
Construct a path instance for a given space information.