Loading...
Searching...
No Matches
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
51ompl::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
58ompl::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
91ompl::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
182void 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
202std::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
328void 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
432void 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 ...