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"
44 #include <boost/math/constants/constants.hpp>
77 states_.resize(other.
states_.size());
78 for (
unsigned int i = 0 ; i < states_.size() ; ++i)
79 states_[i] = si_->cloneState(other.
states_[i]);
84 for (
unsigned int i = 0 ; i < states_.size() ; ++i)
85 si_->freeState(states_[i]);
91 for (
unsigned int i = 1 ; i < states_.size() ; ++i)
92 L += si_->distance(states_[i-1], states_[i]);
99 for (
unsigned int i = 1 ; i < states_.size() ; ++i)
101 if (!states_.empty())
109 for (
unsigned int i = 0 ; i < states_.size() ; ++i)
110 c += si_->getStateValidityChecker()->clearance(states_[i]);
112 c = std::numeric_limits<double>::infinity();
114 c /= (double)states_.size();
121 if (states_.size() > 2)
123 double a = si_->distance(states_[0], states_[1]);
124 for (
unsigned int i = 2 ; i < states_.size() ; ++i)
135 double b = si_->distance(states_[i-1], states_[i]);
136 double c = si_->distance(states_[i-2], states_[i]);
137 double acosValue = (a*a + b*b - c*c) / (2.0*a*b);
139 if (acosValue > -1.0 && acosValue < 1.0)
142 double angle = (boost::math::constants::pi<double>() - acos(acosValue));
145 double k = 2.0 * angle / (a + b);
157 if (states_.size() > 0)
159 if (si_->isValid(states_[0]))
161 int last = states_.size() - 1;
162 for (
int j = 0 ; result && j < last ; ++j)
163 if (!si_->checkMotion(states_[j], states_[j + 1]))
175 out <<
"Geometric path with " << states_.size() <<
" states" << std::endl;
176 for (
unsigned int i = 0 ; i < states_.size() ; ++i)
177 si_->printState(states_[i], out);
184 return std::make_pair(
true,
true);
185 if (states_.size() == 1)
187 bool result = si_->isValid(states_[0]);
188 return std::make_pair(result, result);
192 const int n1 = states_.size() - 1;
193 if (!si_->isValid(states_[0]) || !si_->isValid(states_[n1]))
194 return std::make_pair(
false,
false);
200 for (
int i = 1 ; i < n1 ; ++i)
201 if (!si_->checkMotion(states_[i-1], states_[i]))
205 temp = si_->allocState();
215 if (si_->isValid(states_[i]))
217 si_->copyState(temp, states_[i]);
218 radius = si_->distance(states_[i-1], states_[i]);
222 unsigned int nextValid = n1;
223 for (
int j = i + 1 ; j < n1 ; ++j)
224 if (si_->isValid(states_[j]))
230 si_->getStateSpace()->interpolate(states_[i - 1], states_[nextValid], 0.5, temp);
231 radius = std::max(si_->distance(states_[i-1], temp), si_->distance(states_[i-1], states_[i]));
234 bool success =
false;
236 for (
unsigned int a = 0 ; a < attempts ; ++a)
237 if (uvss->
sampleNear(states_[i], temp, radius))
239 if (si_->checkMotion(states_[i-1], states_[i]))
256 si_->freeState(temp);
257 bool originalValid = uvss == NULL;
261 return std::make_pair(originalValid, result);
266 if (states_.size() < 2)
268 std::vector<base::State*> newStates(1, states_[0]);
269 for (
unsigned int i = 1 ; i < states_.size() ; ++i)
272 si_->getStateSpace()->interpolate(newStates.back(), states_[i], 0.5, temp);
273 newStates.push_back(temp);
274 newStates.push_back(states_[i]);
276 states_.swap(newStates);
282 const int n1 = states_.size() - 1;
283 for (
int i = 0 ; i < n1 ; ++i)
284 n += si_->getStateSpace()->validSegmentCount(states_[i], states_[i + 1]);
290 if (requestCount < states_.size() || states_.size() < 2)
293 unsigned int count = requestCount;
296 double remainingLength = length();
299 std::vector<base::State*> newStates;
300 const int n1 = states_.size() - 1;
302 for (
int i = 0 ; i < n1 ; ++i)
307 newStates.push_back(s1);
311 int maxNStates = count + i - states_.size();
316 double segmentLength = si_->distance(s1, s2);
317 int ns = i + 1 == n1 ? maxNStates + 2 : (int)floor(0.5 + (
double)count * segmentLength / remainingLength) + 1;
329 std::vector<base::State*> block;
330 unsigned int ans = si_->getMotionStates(s1, s2, block, ns,
false,
true);
332 if ((
int)ans != ns || block.size() != ans)
333 throw Exception(
"Internal error in path interpolation. Incorrect number of intermediate states. Please contact the developers.");
335 newStates.insert(newStates.end(), block.begin(), block.end());
342 remainingLength -= segmentLength;
349 newStates.push_back(states_[n1]);
350 states_.swap(newStates);
351 if (requestCount != states_.size())
352 throw Exception(
"Internal error in path interpolation. This should never happen. Please contact the developers.");
357 std::reverse(states_.begin(), states_.end());
364 states_[0] = si_->allocState();
365 states_[1] = si_->allocState();
367 ss->sampleUniform(states_[0]);
368 ss->sampleUniform(states_[1]);
375 states_[0] = si_->allocState();
376 states_[1] = si_->allocState();
380 for (
unsigned int i = 0 ; i < attempts ; ++i)
382 if (uvss->
sample(states_[0]) && uvss->
sample(states_[1]))
383 if (si_->checkMotion(states_[0], states_[1]))
400 if (startIndex > states_.size())
401 throw Exception(
"Index on path is out of bounds");
404 bool copy = !states_.empty();
405 for (
unsigned int i = 0, j = startIndex ; i < over.
states_.size() ; ++i, ++j)
407 if (j == states_.size())
411 si_->copyState(s, states_.back());
412 states_.push_back(s);
421 states_.push_back(si_->cloneState(state));
426 if (path.
si_->getStateSpace()->getName() == si_->getStateSpace()->getName())
429 states_.insert(states_.end(), copy.
states_.begin(), copy.
states_.end());
433 overlay(path, states_.size());
438 int index = getClosestIndex(state);
441 if ((std::size_t)(index + 1) < states_.size())
443 double b = si_->distance(state, states_[index-1]);
444 double a = si_->distance(state, states_[index+1]);
448 for (
int i = 0 ; i < index ; ++i)
449 si_->freeState(states_[i]);
450 states_.erase(states_.begin(), states_.begin() + index);
456 int index = getClosestIndex(state);
459 if (index > 0 && (std::size_t)(index + 1) < states_.size())
461 double b = si_->distance(state, states_[index-1]);
462 double a = si_->distance(state, states_[index+1]);
466 if ((std::size_t)(index + 1) < states_.size())
468 for (std::size_t i = index + 1 ; i < states_.size() ; ++i)
469 si_->freeState(states_[i]);
470 states_.resize(index + 1);
481 double min_d = si_->distance(states_[0], state);
482 for (std::size_t i = 1 ; i < states_.size() ; ++i)
484 double d = si_->distance(states_[i], state);