Loading...
Searching...
No Matches
ConstrainedStateSpace.cpp
102ompl::base::ConstrainedStateSpace::ConstrainedStateSpace(const StateSpacePtr &space, const ConstraintPtr &constraint)
142 if (flags & CONSTRAINED_STATESPACE_SAMPLERS && (!constraint_->isSatisfied(s1) || !constraint_->isSatisfied(s2)))
270void ompl::base::ConstrainedStateSpace::interpolate(const State *from, const State *to, const double t,
287ompl::base::State *ompl::base::ConstrainedStateSpace::geodesicInterpolate(const std::vector<State *> &geodesic,
315 assert((t1 < t2 || std::abs(t1 - t2) < std::numeric_limits<double>::epsilon()) ? (i < geodesic.size()) : (i + 1 < geodesic.size()));
bool checkMotion(const State *s1, const State *s2) const override
Return whether we can step from s1 to s2 along the manifold without collision.
Definition ConstrainedStateSpace.cpp:55
const ConstrainedStateSpace & ss_
Space in which we check motion.
Definition ConstrainedStateSpace.h:90
A State in a ConstrainedStateSpace, represented as a dense real vector of values. For convenience and...
Definition ConstrainedStateSpace.h:169
A StateSpace that has a Constraint imposed upon it. Underlying space functions are passed to the ambi...
Definition ConstrainedStateSpace.h:134
virtual State * geodesicInterpolate(const std::vector< State * > &geodesic, double t) const
Like interpolate(...), but interpolates between intermediate states already supplied in stateList fro...
Definition ConstrainedStateSpace.cpp:287
void setSpaceInformation(SpaceInformation *si)
Sets the space information for this state space. Required for collision checking in manifold traversa...
Definition ConstrainedStateSpace.cpp:198
virtual void clear()
Clear any allocated memory from the state space.
Definition ConstrainedStateSpace.cpp:261
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
virtual bool discreteGeodesic(const State *from, const State *to, bool interpolate=false, std::vector< State * > *geodesic=nullptr) const =0
Traverse the manifold from from toward to. Returns true if we reached to, and false if we stopped ear...
double delta_
Step size when traversing the manifold and collision checking.
Definition ConstrainedStateSpace.h:330
void constrainedSanityChecks(unsigned int flags) const
Do some sanity checks relating to discrete geodesic computation and constraint satisfaction....
Definition ConstrainedStateSpace.cpp:111
ConstrainedStateSpace(const StateSpacePtr &ambientSpace, const ConstraintPtr &constraint)
Construct a constrained space from an ambientSpace and a constraint.
Definition ConstrainedStateSpace.cpp:102
@ CONSTRAINED_STATESPACE_GEODESIC_SATISFY
Check whether discrete geodesics satisfy the constraint at all points.
Definition ConstrainedStateSpace.h:146
@ CONSTRAINED_STATESPACE_SAMPLERS
Check whether state samplers return constraint satisfying samples.
Definition ConstrainedStateSpace.h:142
@ CONSTRAINED_STATESPACE_GEODESIC_CONTINUITY
Check whether discrete geodesics keep lambda_ * delta_ continuity.
Definition ConstrainedStateSpace.h:150
@ CONSTRAINED_STATESPACE_JACOBIAN
Check if the constraint's numerical Jacobian approximates its provided Jacobian.
Definition ConstrainedStateSpace.h:158
@ CONSTRAINED_STATESPACE_GEODESIC_INTERPOLATE
Check whether geodesicInterpolate(...) returns constraint satisfying states.
Definition ConstrainedStateSpace.h:154
void sanityChecks() const override
Perform both constrained and regular sanity checks.
Definition ConstrainedStateSpace.cpp:186
void setDelta(double delta)
Set delta, the step size for traversing the manifold and collision checking. Default defined by ompl:...
Definition ConstrainedStateSpace.cpp:211
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
unsigned int getManifoldDimension() const
Returns the dimension of the manifold.
Definition ConstrainedStateSpace.h:302
A shared pointer wrapper for ompl::base::Constraint.
A shared pointer wrapper for ompl::base::SpaceInformation.
The base class for space information. This contains all the information about the space planning is d...
Definition SpaceInformation.h:82
A shared pointer wrapper for ompl::base::StateSampler.
A shared pointer wrapper for ompl::base::StateSpace.
@ STATESPACE_DISTANCE_BOUND
Check whether the StateSpace::distance() is bounded by StateSpace::getExtent()
Definition StateSpace.h:152
@ STATESPACE_DISTANCE_DIFFERENT_STATES
Check whether the distances between non-equal states is strictly positive (StateSpace::distance())
Definition StateSpace.h:139
@ STATESPACE_RESPECT_BOUNDS
Check whether sampled states are always within bounds.
Definition StateSpace.h:155
@ STATESPACE_DISTANCE_SYMMETRIC
Check whether the distance function is symmetric (StateSpace::distance())
Definition StateSpace.h:142
@ STATESPACE_ENFORCE_BOUNDS_NO_OP
Check that enforceBounds() does not modify the contents of states that are within bounds.
Definition StateSpace.h:158
virtual void sanityChecks() const
Convenience function that allows derived state spaces to choose which checks should pass (see SanityC...
Definition StateSpace.cpp:603
virtual StateSamplerPtr allocStateSampler() const
Allocate an instance of the state sampler for this space. This sampler will be allocated with the sam...
Definition StateSpace.cpp:800
void setup() override
Perform final setup steps. This function is automatically called by the SpaceInformation....
Definition WrapperStateSpace.cpp:77
double getMaximumExtent() const override
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
Definition WrapperStateSpace.h:249
void setLongestValidSegmentFraction(double segmentFraction) override
When performing discrete validation of motions, the length of the longest segment that does not requi...
Definition WrapperStateSpace.h:213
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
unsigned int getDimension() const override
Get the dimension of the space (not the dimension of the surrounding ambient space)
Definition WrapperStateSpace.h:244
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
static const unsigned int TEST_STATE_COUNT
When multiple states need to be generated as part of the computation of various information (usually ...
Definition MagicConstants.h:101