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);
173 return std::make_pair(
true,
true);
174 if (states_.size() == 1)
176 bool result = si_->isValid(states_[0]);
177 return std::make_pair(result, result);
181 const int n1 = states_.size() - 1;
182 if (!si_->isValid(states_[0]) || !si_->isValid(states_[n1]))
183 return std::make_pair(
false,
false);
189 for (
int i = 1 ; i < n1 ; ++i)
190 if (!si_->checkMotion(states_[i-1], states_[i]))
194 temp = si_->allocState();
204 if (si_->isValid(states_[i]))
206 si_->copyState(temp, states_[i]);
207 radius = si_->distance(states_[i-1], states_[i]);
211 unsigned int nextValid = n1;
212 for (
int j = i + 1 ; j < n1 ; ++j)
213 if (si_->isValid(states_[j]))
219 si_->getStateSpace()->interpolate(states_[i - 1], states_[nextValid], 0.5, temp);
220 radius = std::max(si_->distance(states_[i-1], temp), si_->distance(states_[i-1], states_[i]));
223 bool success =
false;
225 for (
unsigned int a = 0 ; a < attempts ; ++a)
226 if (uvss->
sampleNear(states_[i], temp, radius))
228 if (si_->checkMotion(states_[i-1], states_[i]))
245 si_->freeState(temp);
246 bool originalValid = uvss == NULL;
250 return std::make_pair(originalValid, result);
255 if (states_.size() < 2)
257 std::vector<base::State*> newStates(1, states_[0]);
258 for (
unsigned int i = 1 ; i < states_.size() ; ++i)
261 si_->getStateSpace()->interpolate(newStates.back(), states_[i], 0.5, temp);
262 newStates.push_back(temp);
263 newStates.push_back(states_[i]);
265 states_.swap(newStates);
271 const int n1 = states_.size() - 1;
272 for (
int i = 0 ; i < n1 ; ++i)
273 n += si_->getStateSpace()->validSegmentCount(states_[i], states_[i + 1]);
279 if (requestCount < states_.size() || states_.size() < 2)
282 unsigned int count = requestCount;
285 double remainingLength = length();
288 std::vector<base::State*> newStates;
289 const int n1 = states_.size() - 1;
291 for (
int i = 0 ; i < n1 ; ++i)
296 newStates.push_back(s1);
300 int maxNStates = count + i - states_.size();
305 double segmentLength = si_->distance(s1, s2);
306 int ns = i + 1 == n1 ? maxNStates + 2 : (int)floor(0.5 + (
double)count * segmentLength / remainingLength) + 1;
318 std::vector<base::State*> block;
319 unsigned int ans = si_->getMotionStates(s1, s2, block, ns,
false,
true);
321 if ((
int)ans != ns || block.size() != ans)
322 throw Exception(
"Internal error in path interpolation. Incorrect number of intermediate states. Please contact the developers.");
324 newStates.insert(newStates.end(), block.begin(), block.end());
331 remainingLength -= segmentLength;
338 newStates.push_back(states_[n1]);
339 states_.swap(newStates);
340 if (requestCount != states_.size())
341 throw Exception(
"Internal error in path interpolation. This should never happen. Please contact the developers.");
346 std::reverse(states_.begin(), states_.end());
353 states_[0] = si_->allocState();
354 states_[1] = si_->allocState();
356 ss->sampleUniform(states_[0]);
357 ss->sampleUniform(states_[1]);
364 states_[0] = si_->allocState();
365 states_[1] = si_->allocState();
369 for (
unsigned int i = 0 ; i < attempts ; ++i)
371 if (uvss->
sample(states_[0]) && uvss->
sample(states_[1]))
372 if (si_->checkMotion(states_[0], states_[1]))
389 if (startIndex > states_.size())
390 throw Exception(
"Index on path is out of bounds");
393 bool copy = !states_.empty();
394 for (
unsigned int i = 0, j = startIndex ; i < over.
states_.size() ; ++i, ++j)
396 if (j == states_.size())
400 si_->copyState(s, states_.back());
401 states_.push_back(s);
410 states_.push_back(si_->cloneState(state));
415 if (path.
si_->getStateSpace()->getName() == si_->getStateSpace()->getName())
418 states_.insert(states_.end(), copy.
states_.begin(), copy.
states_.end());
422 overlay(path, states_.size());
427 int index = getClosestIndex(state);
430 if ((std::size_t)(index + 1) < states_.size())
432 double b = si_->distance(state, states_[index-1]);
433 double a = si_->distance(state, states_[index+1]);
437 for (
int i = 0 ; i < index ; ++i)
438 si_->freeState(states_[i]);
439 states_.erase(states_.begin(), states_.begin() + index);
445 int index = getClosestIndex(state);
448 if (index > 0 && (std::size_t)(index + 1) < states_.size())
450 double b = si_->distance(state, states_[index-1]);
451 double a = si_->distance(state, states_[index+1]);
455 if ((std::size_t)(index + 1) < states_.size())
457 for (std::size_t i = index + 1 ; i < states_.size() ; ++i)
458 si_->freeState(states_[i]);
459 states_.resize(index + 1);
470 double min_d = si_->distance(states_[0], state);
471 for (std::size_t i = 1 ; i < states_.size() ; ++i)
473 double d = si_->distance(states_[i], state);
502 if (states_.size() == 1)
508 if (states_.size() == 2)
510 double d = si_->distance(states_[0], states_[1]);
513 times[1] = std::max(2.0 * d / maxVel, sqrt(2.0 * d / maxAcc));
518 std::vector<double> L(states_.size() - 1);
519 for (std::size_t i = 0 ; i < L.size() ; ++i)
520 L[i] = si_->distance(states_[i], states_[i + 1]);
523 times.resize(states_.size());
527 std::vector<double> vel(states_.size(), maxVel);
528 vel.front() = vel.back() = 0.0;
534 for (std::size_t i = 1 ; i < L.size() ; ++i)
538 double c = si_->distance(states_[i-1], states_[i+1]);
539 double acosValue = (a*a + b*b - c*c) / (2.0*a*b);
540 vel[i] *= std::min(1.0, fabs(acosValue));
544 unsigned int steps = 0;
545 while (change && steps <= maxSteps)
551 for (std::size_t i = 1 ; i < times.size() ; ++i)
552 times[i] = times[i - 1] + (2.0 * L[i-1]) / (vel[i-1] + vel[i]);
554 for (std::size_t i = 0 ; i < L.size() ; ++i)
556 double acc = (vel[i + 1] - vel[i]) / (times[i + 1] - times[i]);
571 for (
int i = L.size() - 1 ; i >= 0 ; --i)
573 double acc = (vel[i + 1] - vel[i]) / (times[i + 1] - times[i]);