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