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>
80 states_.resize(other.
states_.size());
81 for (
unsigned int i = 0; i < states_.size(); ++i)
82 states_[i] = si_->cloneState(other.
states_[i]);
87 for (
auto &state : states_)
88 si_->freeState(state);
94 return opt->identityCost();
96 base::Cost cost(opt->initialCost(states_.front()));
97 for (std::size_t i = 1; i < states_.size(); ++i)
98 cost = opt->combineCosts(cost, opt->motionCost(states_[i - 1], states_[i]));
99 cost = opt->combineCosts(cost, opt->terminalCost(states_.back()));
106 for (
unsigned int i = 1; i < states_.size(); ++i)
107 L += si_->distance(states_[i - 1], states_[i]);
114 for (
auto state : states_)
115 c += si_->getStateValidityChecker()->clearance(state);
117 c = std::numeric_limits<double>::infinity();
119 c /= (double)states_.size();
126 if (states_.size() > 2)
128 double a = si_->distance(states_[0], states_[1]);
129 for (
unsigned int i = 2; i < states_.size(); ++i)
140 double b = si_->distance(states_[i - 1], states_[i]);
141 double c = si_->distance(states_[i - 2], states_[i]);
142 double acosValue = (a * a + b * b - c * c) / (2.0 * a * b);
144 if (acosValue > -1.0 && acosValue < 1.0)
147 double angle = (boost::math::constants::pi<double>() - acos(acosValue));
150 double k = 2.0 * angle / (a + b);
166 if (states_.size() > 0)
168 if (si_->isValid(states_[0]))
170 int last = states_.size() - 1;
171 for (
int j = 0; result && j < last; ++j)
172 if (!si_->checkMotion(states_[j], states_[j + 1]))
184 out <<
"Geometric path with " << states_.size() <<
" states" << std::endl;
185 for (
auto state : states_)
186 si_->printState(state, out);
192 std::vector<double> reals;
193 for (
auto state : states_)
196 std::copy(reals.begin(), reals.end(), std::ostream_iterator<double>(out,
" "));
205 return std::make_pair(
true,
true);
206 if (states_.size() == 1)
208 bool result = si_->isValid(states_[0]);
209 return std::make_pair(result, result);
213 const int n1 = states_.size() - 1;
214 if (!si_->isValid(states_[0]) || !si_->isValid(states_[n1]))
215 return std::make_pair(
false,
false);
221 for (
int i = 1; i < n1; ++i)
222 if (!si_->checkMotion(states_[i - 1], states_[i]) ||
226 (i == n1 - 1 && !si_->checkMotion(states_[i], states_[i + 1])))
230 temp = si_->allocState();
240 if (si_->isValid(states_[i]))
242 si_->copyState(temp, states_[i]);
243 radius = si_->distance(states_[i - 1], states_[i]);
247 unsigned int nextValid = n1 - 1;
248 for (
int j = i + 1; j < n1; ++j)
249 if (si_->isValid(states_[j]))
255 si_->getStateSpace()->interpolate(states_[i - 1], states_[nextValid], 0.5, temp);
256 radius = std::max(si_->distance(states_[i - 1], temp), si_->distance(states_[i - 1], states_[i]));
259 bool success =
false;
261 for (
unsigned int a = 0; a < attempts; ++a)
262 if (uvss->
sampleNear(states_[i], temp, radius))
264 if (si_->checkMotion(states_[i - 1], states_[i]) &&
267 (i < n1 - 1 || si_->checkMotion(states_[i], states_[i + 1])))
284 si_->freeState(temp);
285 bool originalValid = uvss ==
nullptr;
289 return std::make_pair(originalValid, result);
294 if (states_.size() < 2)
296 std::vector<base::State *> newStates(1, states_[0]);
297 for (
unsigned int i = 1; i < states_.size(); ++i)
300 si_->getStateSpace()->interpolate(newStates.back(), states_[i], 0.5, temp);
301 newStates.push_back(temp);
302 newStates.push_back(states_[i]);
304 states_.swap(newStates);
309 std::vector<base::State *> newStates;
310 const int segments = states_.size() - 1;
312 for (
int i = 0; i < segments; ++i)
317 newStates.push_back(s1);
318 unsigned int n = si_->getStateSpace()->validSegmentCount(s1, s2);
320 std::vector<base::State *> block;
321 si_->getMotionStates(s1, s2, block, n - 1,
false,
true);
322 newStates.insert(newStates.end(), block.begin(), block.end());
324 newStates.push_back(states_[segments]);
325 states_.swap(newStates);
330 if (requestCount < states_.size() || states_.size() < 2)
333 unsigned int count = requestCount;
336 double remainingLength = length();
339 std::vector<base::State *> newStates;
340 const int n1 = states_.size() - 1;
342 for (
int i = 0; i < n1; ++i)
347 newStates.push_back(s1);
351 int maxNStates = count + i - states_.size();
356 double segmentLength = si_->distance(s1, s2);
358 i + 1 == n1 ? maxNStates + 2 : (int)floor(0.5 + (
double)count * segmentLength / remainingLength) + 1;
370 std::vector<base::State *> block;
371 si_->getMotionStates(s1, s2, block, ns,
false,
true);
372 newStates.insert(newStates.end(), block.begin(), block.end());
379 remainingLength -= segmentLength;
386 newStates.push_back(states_[n1]);
387 states_.swap(newStates);
392 std::reverse(states_.begin(), states_.end());
399 states_[0] = si_->allocState();
400 states_[1] = si_->allocState();
401 base::StateSamplerPtr ss = si_->allocStateSampler();
402 ss->sampleUniform(states_[0]);
403 ss->sampleUniform(states_[1]);
410 states_[0] = si_->allocState();
411 states_[1] = si_->allocState();
415 for (
unsigned int i = 0; i < attempts; ++i)
418 if (si_->checkMotion(states_[0], states_[1]))
434 if (startIndex > states_.size())
435 throw Exception(
"Index on path is out of bounds");
436 const base::StateSpacePtr &sm = over.
si_->getStateSpace();
437 const base::StateSpacePtr &dm = si_->getStateSpace();
438 bool copy = !states_.empty();
439 for (
unsigned int i = 0, j = startIndex; i < over.
states_.size(); ++i, ++j)
441 if (j == states_.size())
445 si_->copyState(s, states_.back());
446 states_.push_back(s);
449 copyStateData(dm, states_[j], sm, over.
states_[i]);
455 states_.push_back(si_->cloneState(state));
460 if (path.si_->getStateSpace()->getName() == si_->getStateSpace()->getName())
463 states_.insert(states_.end(), copy.
states_.begin(), copy.
states_.end());
467 overlay(path, states_.size());
472 states_.insert(states_.begin(), si_->cloneState(state));
477 int index = getClosestIndex(state);
480 if ((std::size_t)(index + 1) < states_.size())
482 double b = si_->distance(state, states_[index - 1]);
483 double a = si_->distance(state, states_[index + 1]);
487 for (
int i = 0; i < index; ++i)
488 si_->freeState(states_[i]);
489 states_.erase(states_.begin(), states_.begin() + index);
495 int index = getClosestIndex(state);
498 if (index > 0 && (std::size_t)(index + 1) < states_.size())
500 double b = si_->distance(state, states_[index - 1]);
501 double a = si_->distance(state, states_[index + 1]);
505 if ((std::size_t)(index + 1) < states_.size())
507 for (std::size_t i = index + 1; i < states_.size(); ++i)
508 si_->freeState(states_[i]);
509 states_.resize(index + 1);
520 double min_d = si_->distance(states_[0], state);
521 for (std::size_t i = 1; i < states_.size(); ++i)
523 double d = si_->distance(states_[i], state);
The exception type for ompl.
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
SpaceInformationPtr si_
The space information this path is part of.
Representation of a space in which planning can be performed. Topology specific sampling,...
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.
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 ...