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>
70 for(
unsigned int k = 0; k < states.size(); k++)
72 this->
append(states.at(k));
89 states_.resize(other.
states_.size());
90 for (
unsigned int i = 0; i < states_.size(); ++i)
91 states_[i] = si_->cloneState(other.
states_[i]);
96 for (
auto &state : states_)
97 si_->freeState(state);
103 return opt->identityCost();
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()));
115 for (
unsigned int i = 1; i < states_.size(); ++i)
116 L += si_->distance(states_[i - 1], states_[i]);
123 for (
auto state : states_)
124 c += si_->getStateValidityChecker()->clearance(state);
126 c = std::numeric_limits<double>::infinity();
128 c /= (double)states_.size();
135 if (states_.size() > 2)
137 double a = si_->distance(states_[0], states_[1]);
138 for (
unsigned int i = 2; i < states_.size(); ++i)
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);
153 if (acosValue > -1.0 && acosValue < 1.0)
156 double angle = (boost::math::constants::pi<double>() - acos(acosValue));
159 double k = 2.0 * angle / (a + b);
175 if (states_.size() > 0)
177 if (si_->isValid(states_[0]))
179 int last = states_.size() - 1;
180 for (
int j = 0; result && j < last; ++j)
181 if (!si_->checkMotion(states_[j], states_[j + 1]))
193 out <<
"Geometric path with " << states_.size() <<
" states" << std::endl;
194 for (
auto state : states_)
195 si_->printState(state, out);
201 std::vector<double> reals;
202 for (
auto state : states_)
205 std::copy(reals.begin(), reals.end(), std::ostream_iterator<double>(out,
" "));
214 return std::make_pair(
true,
true);
215 if (states_.size() == 1)
217 bool result = si_->isValid(states_[0]);
218 return std::make_pair(result, result);
222 const int n1 = states_.size() - 1;
223 if (!si_->isValid(states_[0]) || !si_->isValid(states_[n1]))
224 return std::make_pair(
false,
false);
230 for (
int i = 1; i < n1; ++i)
231 if (!si_->checkMotion(states_[i - 1], states_[i]) ||
235 (i == n1 - 1 && !si_->checkMotion(states_[i], states_[i + 1])))
239 temp = si_->allocState();
249 if (si_->isValid(states_[i]))
251 si_->copyState(temp, states_[i]);
252 radius = si_->distance(states_[i - 1], states_[i]);
256 unsigned int nextValid = n1 - 1;
257 for (
int j = i + 1; j < n1; ++j)
258 if (si_->isValid(states_[j]))
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]));
268 bool success =
false;
270 for (
unsigned int a = 0; a < attempts; ++a)
271 if (uvss->
sampleNear(states_[i], temp, radius))
273 if (si_->checkMotion(states_[i - 1], states_[i]) &&
276 (i < n1 - 1 || si_->checkMotion(states_[i], states_[i + 1])))
293 si_->freeState(temp);
294 bool originalValid = uvss ==
nullptr;
298 return std::make_pair(originalValid, result);
303 if (states_.size() < 2)
305 std::vector<base::State *> newStates(1, states_[0]);
306 for (
unsigned int i = 1; i < states_.size(); ++i)
309 si_->getStateSpace()->interpolate(newStates.back(), states_[i], 0.5, temp);
310 newStates.push_back(temp);
311 newStates.push_back(states_[i]);
313 states_.swap(newStates);
318 std::vector<base::State *> newStates;
319 const int segments = states_.size() - 1;
321 for (
int i = 0; i < segments; ++i)
326 newStates.push_back(s1);
327 unsigned int n = si_->getStateSpace()->validSegmentCount(s1, s2);
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());
333 newStates.push_back(states_[segments]);
334 states_.swap(newStates);
339 if (requestCount < states_.size() || states_.size() < 2)
342 unsigned int count = requestCount;
345 double remainingLength = length();
348 std::vector<base::State *> newStates;
349 const int n1 = states_.size() - 1;
351 for (
int i = 0; i < n1; ++i)
356 newStates.push_back(s1);
360 int maxNStates = count + i - states_.size();
365 double segmentLength = si_->distance(s1, s2);
367 i + 1 == n1 ? maxNStates + 2 : (int)floor(0.5 + (
double)count * segmentLength / remainingLength) + 1;
379 std::vector<base::State *> block;
380 si_->getMotionStates(s1, s2, block, ns,
false,
true);
381 newStates.insert(newStates.end(), block.begin(), block.end());
388 remainingLength -= segmentLength;
395 newStates.push_back(states_[n1]);
396 states_.swap(newStates);
401 std::reverse(states_.begin(), states_.end());
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]);
419 states_[0] = si_->allocState();
420 states_[1] = si_->allocState();
424 for (
unsigned int i = 0; i < attempts; ++i)
427 if (si_->checkMotion(states_[0], states_[1]))
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)
450 if (j == states_.size())
454 si_->copyState(s, states_.back());
455 states_.push_back(s);
458 copyStateData(dm, states_[j], sm, over.
states_[i]);
464 states_.push_back(si_->cloneState(state));
469 if (path.si_->getStateSpace()->getName() == si_->getStateSpace()->getName())
472 states_.insert(states_.end(), copy.
states_.begin(), copy.
states_.end());
476 overlay(path, states_.size());
481 states_.insert(states_.begin(), si_->cloneState(state));
486 int index = getClosestIndex(state);
489 if ((std::size_t)(index + 1) < states_.size())
491 double b = si_->distance(state, states_[index - 1]);
492 double a = si_->distance(state, states_[index + 1]);
496 for (
int i = 0; i < index; ++i)
497 si_->freeState(states_[i]);
498 states_.erase(states_.begin(), states_.begin() + index);
504 int index = getClosestIndex(state);
507 if (index > 0 && (std::size_t)(index + 1) < states_.size())
509 double b = si_->distance(state, states_[index - 1]);
510 double a = si_->distance(state, states_[index + 1]);
514 if ((std::size_t)(index + 1) < states_.size())
516 for (std::size_t i = index + 1; i < states_.size(); ++i)
517 si_->freeState(states_[i]);
518 states_.resize(index + 1);
529 double min_d = si_->distance(states_[0], state);
530 for (std::size_t i = 1; i < states_.size(); ++i)
532 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 ...