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
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();
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 ...
This namespace contains sampling based planning routines shared by both planning under geometric cons...