37 #include "ompl/geometric/PathGeometric.h"
38 #include "ompl/base/samplers/UniformValidStateSampler.h"
39 #include "ompl/base/ScopedState.h"
43 #include <boost/math/constants/constants.hpp>
76 states_.resize(other.
states_.size());
77 for (
unsigned int i = 0 ; i < states_.size() ; ++i)
78 states_[i] = si_->cloneState(other.
states_[i]);
83 for (
unsigned int i = 0 ; i < states_.size() ; ++i)
84 si_->freeState(states_[i]);
90 for (
unsigned int i = 1 ; i < states_.size() ; ++i)
91 L += si_->distance(states_[i-1], states_[i]);
98 for (
unsigned int i = 0 ; i < states_.size() ; ++i)
99 c += si_->getStateValidityChecker()->clearance(states_[i]);
101 c = std::numeric_limits<double>::infinity();
103 c /= (double)states_.size();
110 if (states_.size() > 2)
112 double a = si_->distance(states_[0], states_[1]);
113 for (
unsigned int i = 2 ; i < states_.size() ; ++i)
124 double b = si_->distance(states_[i-1], states_[i]);
125 double c = si_->distance(states_[i-2], states_[i]);
126 double acosValue = (a*a + b*b - c*c) / (2.0*a*b);
128 if (acosValue > -1.0 && acosValue < 1.0)
131 double angle = (boost::math::constants::pi<double>() - acos(acosValue));
134 double k = 2.0 * angle / (a + b);
146 if (states_.size() > 0)
148 if (si_->isValid(states_[0]))
150 int last = states_.size() - 1;
151 for (
int j = 0 ; result && j < last ; ++j)
152 if (!si_->checkMotion(states_[j], states_[j + 1]))
164 out <<
"Geometric path with " << states_.size() <<
" states" << std::endl;
165 for (
unsigned int i = 0 ; i < states_.size() ; ++i)
166 si_->printState(states_[i], out);
172 std::vector<double> reals;
173 for (
unsigned int i = 0 ; i < states_.size() ; ++i)
176 std::copy(reals.begin(), reals.end(), std::ostream_iterator<double>(out,
" "));
185 return std::make_pair(
true,
true);
186 if (states_.size() == 1)
188 bool result = si_->isValid(states_[0]);
189 return std::make_pair(result, result);
193 const int n1 = states_.size() - 1;
194 if (!si_->isValid(states_[0]) || !si_->isValid(states_[n1]))
195 return std::make_pair(
false,
false);
201 for (
int i = 1 ; i < n1 ; ++i)
202 if (!si_->checkMotion(states_[i-1], states_[i]))
206 temp = si_->allocState();
216 if (si_->isValid(states_[i]))
218 si_->copyState(temp, states_[i]);
219 radius = si_->distance(states_[i-1], states_[i]);
223 unsigned int nextValid = n1;
224 for (
int j = i + 1 ; j < n1 ; ++j)
225 if (si_->isValid(states_[j]))
231 si_->getStateSpace()->interpolate(states_[i - 1], states_[nextValid], 0.5, temp);
232 radius = std::max(si_->distance(states_[i-1], temp), si_->distance(states_[i-1], states_[i]));
235 bool success =
false;
237 for (
unsigned int a = 0 ; a < attempts ; ++a)
238 if (uvss->
sampleNear(states_[i], temp, radius))
240 if (si_->checkMotion(states_[i-1], states_[i]))
257 si_->freeState(temp);
258 bool originalValid = uvss == NULL;
262 return std::make_pair(originalValid, result);
267 if (states_.size() < 2)
269 std::vector<base::State*> newStates(1, states_[0]);
270 for (
unsigned int i = 1 ; i < states_.size() ; ++i)
273 si_->getStateSpace()->interpolate(newStates.back(), states_[i], 0.5, temp);
274 newStates.push_back(temp);
275 newStates.push_back(states_[i]);
277 states_.swap(newStates);
283 const int n1 = states_.size() - 1;
284 for (
int i = 0 ; i < n1 ; ++i)
285 n += si_->getStateSpace()->validSegmentCount(states_[i], states_[i + 1]);
291 if (requestCount < states_.size() || states_.size() < 2)
294 unsigned int count = requestCount;
297 double remainingLength = length();
300 std::vector<base::State*> newStates;
301 const int n1 = states_.size() - 1;
303 for (
int i = 0 ; i < n1 ; ++i)
308 newStates.push_back(s1);
312 int maxNStates = count + i - states_.size();
317 double segmentLength = si_->distance(s1, s2);
318 int ns = i + 1 == n1 ? maxNStates + 2 : (int)floor(0.5 + (
double)count * segmentLength / remainingLength) + 1;
330 std::vector<base::State*> block;
331 unsigned int ans = si_->getMotionStates(s1, s2, block, ns,
false,
true);
333 if ((
int)ans != ns || block.size() != ans)
334 throw Exception(
"Internal error in path interpolation. Incorrect number of intermediate states. Please contact the developers.");
336 newStates.insert(newStates.end(), block.begin(), block.end());
343 remainingLength -= segmentLength;
350 newStates.push_back(states_[n1]);
351 states_.swap(newStates);
352 if (requestCount != states_.size())
353 throw Exception(
"Internal error in path interpolation. This should never happen. Please contact the developers.");
358 std::reverse(states_.begin(), states_.end());
365 states_[0] = si_->allocState();
366 states_[1] = si_->allocState();
368 ss->sampleUniform(states_[0]);
369 ss->sampleUniform(states_[1]);
376 states_[0] = si_->allocState();
377 states_[1] = si_->allocState();
381 for (
unsigned int i = 0 ; i < attempts ; ++i)
383 if (uvss->
sample(states_[0]) && uvss->
sample(states_[1]))
384 if (si_->checkMotion(states_[0], states_[1]))
401 if (startIndex > states_.size())
402 throw Exception(
"Index on path is out of bounds");
405 bool copy = !states_.empty();
406 for (
unsigned int i = 0, j = startIndex ; i < over.
states_.size() ; ++i, ++j)
408 if (j == states_.size())
412 si_->copyState(s, states_.back());
413 states_.push_back(s);
422 states_.push_back(si_->cloneState(state));
427 if (path.
si_->getStateSpace()->getName() == si_->getStateSpace()->getName())
430 states_.insert(states_.end(), copy.
states_.begin(), copy.
states_.end());
434 overlay(path, states_.size());
439 int index = getClosestIndex(state);
442 if ((std::size_t)(index + 1) < states_.size())
444 double b = si_->distance(state, states_[index-1]);
445 double a = si_->distance(state, states_[index+1]);
449 for (
int i = 0 ; i < index ; ++i)
450 si_->freeState(states_[i]);
451 states_.erase(states_.begin(), states_.begin() + index);
457 int index = getClosestIndex(state);
460 if (index > 0 && (std::size_t)(index + 1) < states_.size())
462 double b = si_->distance(state, states_[index-1]);
463 double a = si_->distance(state, states_[index+1]);
467 if ((std::size_t)(index + 1) < states_.size())
469 for (std::size_t i = index + 1 ; i < states_.size() ; ++i)
470 si_->freeState(states_[i]);
471 states_.resize(index + 1);
482 double min_d = si_->distance(states_[0], state);
483 for (std::size_t i = 1 ; i < states_.size() ; ++i)
485 double d = si_->distance(states_[i], state);
void interpolate(void)
Insert a number of states in a path so that the path is made up of (approximately) the states checked...
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 ...
A boost shared pointer wrapper for ompl::base::StateSpace.
A boost shared pointer wrapper for ompl::base::StateSampler.
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...
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 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.
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 subdivide(void)
Add a state at the middle of each segment.
SpaceInformationPtr si_
The space information this path is part of.
bool randomValid(unsigned int attempts)
Set this path to a random valid segment. Sample attempts times for valid segments. Returns true on success.
virtual bool check(void) const
Check if the path is valid.
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 reverse(void)
Reverse the path.
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
Definition of an abstract state.
PathGeometric & operator=(const PathGeometric &other)
Assignment operator.
The exception type for ompl.
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...
std::vector< base::State * > states_
The list of states that make up the path.
Definition of a geometric path.
void random(void)
Set this path to a random segment.
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() ...
virtual double length(void) const
Compute the length of a geometric path (sum of lengths of segments that make up the path) ...
void freeMemory(void)
Free the memory corresponding to the states on this path.
virtual void print(std::ostream &out) const
Print the path to a stream.
double smoothness(void) const
Compute a notion of smoothness for this path. The closer the value is to 0, the smoother the path...
double clearance(void) const
Compute the clearance of the way-points along the path (no interpolation is performed). Detailed formula follows.
void setNrAttempts(unsigned int attempts)
Finding a valid sample usually requires performing multiple attempts. This call allows setting the nu...