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 
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 
91 ompl::base::Cost ompl::geometric::PathGeometric::cost(const base::OptimizationObjectivePtr &opt) const
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 }
The exception type for ompl.
Definition: Exception.h:47
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
Definition: Cost.h:48
SpaceInformationPtr si_
The space information this path is part of.
Definition: Path.h:123
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition: StateSpace.h:71
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:329
Definition of an abstract state.
Definition: State.h:50
A state sampler that only samples valid states, uniformly.
bool sample(State *state) override
Sample a state. Return false in case of failure.
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 setNrAttempts(unsigned int attempts)
Finding a valid sample usually requires performing multiple attempts. This call allows setting the nu...
Definition of a geometric path.
Definition: PathGeometric.h:66
double smoothness() const
Compute a notion of smoothness for this path. The closer the value is to 0, the smoother 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.
double clearance() const
Compute the clearance of the way-points along the path (no interpolation is performed)....
void print(std::ostream &out) const override
Print the path to a stream.
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 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.
PathGeometric & operator=(const PathGeometric &other)
Assignment operator.
void prepend(const base::State *state)
Prepend state to the start of this path. The memory for state is copied.
void clear()
Remove all states and clear memory.
void subdivide()
Add a state at the middle of each segment.
void random()
Set this path to a random segment.
void reverse()
Reverse 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 copyFrom(const PathGeometric &other)
Copy data to this path from another path instance.
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)
void append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
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...
PathGeometric(const base::SpaceInformationPtr &si)
Construct a path instance for a given space information.
Definition: PathGeometric.h:69
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 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...