Loading...
Searching...
No Matches
ProjectedStateSpace.cpp
46ompl::base::ProjectedStateSampler::ProjectedStateSampler(const ProjectedStateSpace *space, StateSamplerPtr sampler)
58void ompl::base::ProjectedStateSampler::sampleUniformNear(State *state, const State *near, const double distance)
65void ompl::base::ProjectedStateSampler::sampleGaussian(State *state, const State *mean, const double stdDev)
76bool ompl::base::ProjectedStateSpace::discreteGeodesic(const State *from, const State *to, bool interpolate,
double lambda_
Manifold traversal from x to y is stopped if accumulated distance is greater than d(x,...
Definition ConstrainedStateSpace.h:336
State * allocState() const override
Allocate a new state in this space.
Definition ConstrainedStateSpace.cpp:265
void interpolate(const State *from, const State *to, double t, State *state) const override
Find a state between from and to around time t, where t = 0 is from, and t = 1 is the final state rea...
Definition ConstrainedStateSpace.cpp:270
double delta_
Step size when traversing the manifold and collision checking.
Definition ConstrainedStateSpace.h:330
SpaceInformation * si_
SpaceInformation associated with this space. Required for early collision checking in manifold traver...
Definition ConstrainedStateSpace.h:318
const ConstraintPtr constraint_
Constraint function that defines the manifold.
Definition ConstrainedStateSpace.h:321
ProjectedStateSampler(const ProjectedStateSpace *space, StateSamplerPtr sampler)
Constructor.
Definition ProjectedStateSpace.cpp:46
void sampleUniform(State *state) override
Sample a state uniformly in ambient space and project to the manifold. Return sample in state.
Definition ProjectedStateSpace.cpp:51
void sampleGaussian(State *state, const State *mean, double stdDev) override
Sample a state uniformly from a normal distribution with given mean and stdDev in ambient space and p...
Definition ProjectedStateSpace.cpp:65
void sampleUniformNear(State *state, const State *near, double distance) override
Sample a state uniformly from the ball with center near and radius distance in ambient space and proj...
Definition ProjectedStateSpace.cpp:58
ConstrainedStateSpace encapsulating a projection-based methodology for planning with constraints.
Definition ProjectedStateSpace.h:103
bool discreteGeodesic(const State *from, const State *to, bool interpolate=false, std::vector< State * > *geodesic=nullptr) const override
Traverse the manifold from from toward to. Returns true if we reached to, and false if we stopped ear...
Definition ProjectedStateSpace.cpp:76
A shared pointer wrapper for ompl::base::StateSampler.
void sampleUniformNear(State *state, const State *near, double distance) override
Sample a nearby state using underlying sampler.
Definition WrapperStateSpace.cpp:44
void sampleUniform(State *state) override
Sample a state using underlying sampler.
Definition WrapperStateSpace.cpp:39
void sampleGaussian(State *state, const State *mean, double stdDev) override
Sample a state within a Gaussian distribution using underlying sampler.
Definition WrapperStateSpace.cpp:50
WrapperStateSampler(const StateSpace *space, StateSamplerPtr sampler)
Constructor. Requires the wrapper state space space and the underlying sampler sampler.
Definition WrapperStateSpace.h:59
void interpolate(const State *from, const State *to, double t, State *state) const override
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state....
Definition WrapperStateSpace.h:299
void copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap.
Definition WrapperStateSpace.h:269
void freeState(State *state) const override
Free the memory of the allocated state.
Definition WrapperStateSpace.h:315
double distance(const State *state1, const State *state2) const override
Computes distance between two states. This function satisfies the properties of a metric if isMetricS...
Definition WrapperStateSpace.h:274
STL namespace.