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