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  std::vector<base::State *> newStates;
310  const int segments = states_.size() - 1;
311 
312  for (int i = 0; i < segments; ++i)
313  {
314  base::State *s1 = states_[i];
315  base::State *s2 = states_[i + 1];
316 
317  newStates.push_back(s1);
318  unsigned int n = si_->getStateSpace()->validSegmentCount(s1, s2);
319 
320  std::vector<base::State *> block;
321  si_->getMotionStates(s1, s2, block, n - 1, false, true);
322  newStates.insert(newStates.end(), block.begin(), block.end());
323  }
324  newStates.push_back(states_[segments]);
325  states_.swap(newStates);
326 }
327 
328 void ompl::geometric::PathGeometric::interpolate(unsigned int requestCount)
329 {
330  if (requestCount < states_.size() || states_.size() < 2)
331  return;
332 
333  unsigned int count = requestCount;
334 
335  // the remaining length of the path we need to add states along
336  double remainingLength = length();
337 
338  // the new array of states this path will have
339  std::vector<base::State *> newStates;
340  const int n1 = states_.size() - 1;
341 
342  for (int i = 0; i < n1; ++i)
343  {
344  base::State *s1 = states_[i];
345  base::State *s2 = states_[i + 1];
346 
347  newStates.push_back(s1);
348 
349  // the maximum number of states that can be added on the current motion (without its endpoints)
350  // such that we can at least fit the remaining states
351  int maxNStates = count + i - states_.size();
352 
353  if (maxNStates > 0)
354  {
355  // compute an approximate number of states the following segment needs to contain; this includes endpoints
356  double segmentLength = si_->distance(s1, s2);
357  int ns =
358  i + 1 == n1 ? maxNStates + 2 : (int)floor(0.5 + (double)count * segmentLength / remainingLength) + 1;
359 
360  // if more than endpoints are needed
361  if (ns > 2)
362  {
363  ns -= 2; // subtract endpoints
364 
365  // make sure we don't add too many states
366  if (ns > maxNStates)
367  ns = maxNStates;
368 
369  // compute intermediate states
370  std::vector<base::State *> block;
371  si_->getMotionStates(s1, s2, block, ns, false, true);
372  newStates.insert(newStates.end(), block.begin(), block.end());
373  }
374  else
375  ns = 0;
376 
377  // update what remains to be done
378  count -= (ns + 1);
379  remainingLength -= segmentLength;
380  }
381  else
382  count--;
383  }
384 
385  // add the last state
386  newStates.push_back(states_[n1]);
387  states_.swap(newStates);
388 }
389 
391 {
392  std::reverse(states_.begin(), states_.end());
393 }
394 
396 {
397  freeMemory();
398  states_.resize(2);
399  states_[0] = si_->allocState();
400  states_[1] = si_->allocState();
401  base::StateSamplerPtr ss = si_->allocStateSampler();
402  ss->sampleUniform(states_[0]);
403  ss->sampleUniform(states_[1]);
404 }
405 
407 {
408  freeMemory();
409  states_.resize(2);
410  states_[0] = si_->allocState();
411  states_[1] = si_->allocState();
412  base::UniformValidStateSampler uvss(si_.get());
413  uvss.setNrAttempts(attempts);
414  bool ok = false;
415  for (unsigned int i = 0; i < attempts; ++i)
416  {
417  if (uvss.sample(states_[0]) && uvss.sample(states_[1]))
418  if (si_->checkMotion(states_[0], states_[1]))
419  {
420  ok = true;
421  break;
422  }
423  }
424  if (!ok)
425  {
426  freeMemory();
427  states_.clear();
428  }
429  return ok;
430 }
431 
432 void ompl::geometric::PathGeometric::overlay(const PathGeometric &over, unsigned int startIndex)
433 {
434  if (startIndex > states_.size())
435  throw Exception("Index on path is out of bounds");
436  const base::StateSpacePtr &sm = over.si_->getStateSpace();
437  const base::StateSpacePtr &dm = si_->getStateSpace();
438  bool copy = !states_.empty();
439  for (unsigned int i = 0, j = startIndex; i < over.states_.size(); ++i, ++j)
440  {
441  if (j == states_.size())
442  {
443  base::State *s = si_->allocState();
444  if (copy)
445  si_->copyState(s, states_.back());
446  states_.push_back(s);
447  }
448 
449  copyStateData(dm, states_[j], sm, over.states_[i]);
450  }
451 }
452 
454 {
455  states_.push_back(si_->cloneState(state));
456 }
457 
459 {
460  if (path.si_->getStateSpace()->getName() == si_->getStateSpace()->getName())
461  {
462  PathGeometric copy(path);
463  states_.insert(states_.end(), copy.states_.begin(), copy.states_.end());
464  copy.states_.clear();
465  }
466  else
467  overlay(path, states_.size());
468 }
469 
471 {
472  states_.insert(states_.begin(), si_->cloneState(state));
473 }
474 
476 {
477  int index = getClosestIndex(state);
478  if (index > 0)
479  {
480  if ((std::size_t)(index + 1) < states_.size())
481  {
482  double b = si_->distance(state, states_[index - 1]);
483  double a = si_->distance(state, states_[index + 1]);
484  if (b > a)
485  ++index;
486  }
487  for (int i = 0; i < index; ++i)
488  si_->freeState(states_[i]);
489  states_.erase(states_.begin(), states_.begin() + index);
490  }
491 }
492 
494 {
495  int index = getClosestIndex(state);
496  if (index >= 0)
497  {
498  if (index > 0 && (std::size_t)(index + 1) < states_.size())
499  {
500  double b = si_->distance(state, states_[index - 1]);
501  double a = si_->distance(state, states_[index + 1]);
502  if (b < a)
503  --index;
504  }
505  if ((std::size_t)(index + 1) < states_.size())
506  {
507  for (std::size_t i = index + 1; i < states_.size(); ++i)
508  si_->freeState(states_[i]);
509  states_.resize(index + 1);
510  }
511  }
512 }
513 
515 {
516  if (states_.empty())
517  return -1;
518 
519  int index = 0;
520  double min_d = si_->distance(states_[0], state);
521  for (std::size_t i = 1; i < states_.size(); ++i)
522  {
523  double d = si_->distance(states_[i], state);
524  if (d < min_d)
525  {
526  min_d = d;
527  index = i;
528  }
529  }
530  return index;
531 }
532 
534 {
535  freeMemory();
536  states_.clear();
537 }
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...
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
virtual 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:328
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.
bool sampleNear(State *state, const State *near, double distance) override
Sample a state near another, within specified distance. Return false, in case of failure.
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...