51 ompl::geometric::PathGeometric::PathGeometric(const base::SpaceInformationPtr &si, const base::State *state)
58 ompl::geometric::PathGeometric::PathGeometric(const base::SpaceInformationPtr &si, const base::State *state1,
59 const base::State *state2)
67 ompl::geometric::PathGeometric &ompl::geometric::PathGeometric::operator=(const PathGeometric &other)
72 si_ = other.si_;
82 states_[i] = si_->cloneState(other.states_[i]);
91 ompl::base::Cost ompl::geometric::PathGeometric::cost(const base::OptimizationObjectivePtr &opt) const
96 base::Cost cost(opt->initialCost(states_.front()));
191 const base::StateSpace *space(si_->getStateSpace().get());
195 space->copyToReals(reals, state);
217 base::State *temp = nullptr;
218 base::UniformValidStateSampler *uvss = nullptr;
233 uvss = new base::UniformValidStateSampler(si_.get());
256 radius = std::max(si_->distance(states_[i - 1], temp), si_->distance(states_[i - 1], states_[i]));
262 if (uvss->sampleNear(states_[i], temp, radius))
299 base::State *temp = si_->allocState();
314 base::State *s1 = states_[i];
315 base::State *s2 = states_[i + 1];
344 base::State *s1 = states_[i];
345 base::State *s2 = states_[i + 1];
355 // compute an approximate number of states the following segment needs to contain; this includes endpoints
358 i + 1 == n1 ? maxNStates + 2 : (int)floor(0.5 + (double)count * segmentLength / remainingLength) + 1;
412 base::UniformValidStateSampler uvss(si_.get());
432 void ompl::geometric::PathGeometric::overlay(const PathGeometric &over, unsigned int startIndex)
435 throw Exception("Index on path is out of bounds");
436 const base::StateSpacePtr &sm = over.si_->getStateSpace();
439 for (unsigned int i = 0, j = startIndex; i < over.states_.size(); ++i, ++j)
443 base::State *s = si_->allocState();
462 PathGeometric copy(path);
Reverse 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.
Representation of a space in which planning can be performed. Topology specific sampling,...
Remove all states and clear memory.
void prepend(const base::State *state)
Prepend state to the start of this path. The memory for state is copied.
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...
double smoothness() const
Compute a notion of smoothness for this path. The closer the value is to 0, the smoother the path....
PathGeometric & operator=(const PathGeometric &other)
bool sampleNear(State *state, const State *near, double distance) override
Sample a state near another, within specified distance. Return false, in case of failure.
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()
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...
Definition of a cost value. Can represent the cost of a motion or the cost of a state.
bool sample(State *state) override
Sample a state. Return false in case of failure.
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...
A state sampler that only samples valid states, uniformly.
Add a state at the middle of each segment.
The space information this path is part of.
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....
Free the memory corresponding to the states on this path.
std::vector< base::State * > states_
The list of states that make up the path.
void setNrAttempts(unsigned int attempts)
Finding a valid sample usually requires performing multiple attempts. This call allows setting the nu...
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 append(const base::State *state)
Append state to the end of this path. The memory for state is copied.
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 ...
double length() const override
Compute the length of a geometric path (sum of lengths of segments that make up the path)
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 copyFrom(const PathGeometric &other)
Copy data to this path from another path instance.
Set this path to a random segment.
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 ...
void print(std::ostream &out) const override
Print the path to a stream.
double clearance() const
Compute the clearance of the way-points along the path (no interpolation is performed)....