ConstrainedStateSpace.h
86 bool checkMotion(const State *s1, const State *s2, std::pair<State *, double> &lastValid) const override;
182 {
186 }
340 };
The base class for space information. This contains all the information about the space planning is d...
Definition: SpaceInformation.h:145
@ CONSTRAINED_STATESPACE_GEODESIC_INTERPOLATE
Check whether geodesicInterpolate(...) returns constraint satisfying states.
Definition: ConstrainedStateSpace.h:186
const ConstraintPtr getConstraint() const
Returns the constraint that defines the underlying manifold.
Definition: ConstrainedStateSpace.h:340
SanityChecks
Flags used in a bit mask for constrained state space sanity checks, constraintSanityChecks().
Definition: ConstrainedStateSpace.h:170
A shared pointer wrapper for ompl::base::SpaceInformation.
A shared pointer wrapper for ompl::base::Constraint.
ConstrainedMotionValidator(SpaceInformation *si)
Constructor.
Definition: ConstrainedStateSpace.cpp:45
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
ConstrainedStateSpace(const StateSpacePtr &ambientSpace, const ConstraintPtr &constraint)
Construct a constrained space from an ambientSpace and a constraint.
Definition: ConstrainedStateSpace.cpp:102
SpaceInformation * si_
SpaceInformation associated with this space. Required for early collision checking in manifold traver...
Definition: ConstrainedStateSpace.h:350
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:338
virtual void clear()
Clear any allocated memory from the state space.
Definition: ConstrainedStateSpace.cpp:261
double * getValueAddressAtIndex(State *state, unsigned int index) const override
Many states contain a number of double values. This function provides a means to get the memory addre...
Definition: WrapperStateSpace.h:386
double lambda_
Manifold traversal from x to y is stopped if accumulated distance is greater than d(x,...
Definition: ConstrainedStateSpace.h:368
void copy(const Eigen::Ref< const Eigen::VectorXd > &other)
Copy the contents from a vector into this state. Uses the underlying copy operator used by Eigen for ...
Definition: ConstrainedStateSpace.h:213
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 getDelta() const
Get delta, the step size across the manifold.
Definition: ConstrainedStateSpace.h:316
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:142
void setDelta(double delta)
Set delta, the step size for traversing the manifold and collision checking. Default defined by ompl:...
Definition: ConstrainedStateSpace.cpp:211
void constrainedSanityChecks(unsigned int flags) const
Do some sanity checks relating to discrete geodesic computation and constraint satisfaction....
Definition: ConstrainedStateSpace.cpp:111
double delta_
Step size when traversing the manifold and collision checking.
Definition: ConstrainedStateSpace.h:362
bool isMetricSpace() const override
Returns false as the implicit constrained configuration space defined by the constraint is not metric...
Definition: ConstrainedStateSpace.h:231
@ CONSTRAINED_STATESPACE_SAMPLERS
Check whether state samplers return constraint satisfying samples.
Definition: ConstrainedStateSpace.h:174
A StateSpace that has a Constraint imposed upon it. Underlying space functions are passed to the ambi...
Definition: ConstrainedStateSpace.h:165
@ CONSTRAINED_STATESPACE_GEODESIC_CONTINUITY
Check whether discrete geodesics keep lambda_ * delta_ continuity.
Definition: ConstrainedStateSpace.h:182
unsigned int getDimension() const override
Get the dimension of the space (not the dimension of the surrounding ambient space)
Definition: WrapperStateSpace.h:308
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
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
A State in a ConstrainedStateSpace, represented as a dense real vector of values. For convenience and...
Definition: ConstrainedStateSpace.h:200
A shared pointer wrapper for ompl::base::StateSpace.
const ConstrainedStateSpace & ss_
Space in which we check motion.
Definition: ConstrainedStateSpace.h:122
State * allocState() const override
Allocate a new state in this space.
Definition: ConstrainedStateSpace.cpp:265
unsigned int getAmbientDimension() const
Returns the dimension of the ambient space.
Definition: ConstrainedStateSpace.h:328
void setLambda(double lambda)
Set lambda, where lambda * distance(x, y) is the maximum length of the geodesic x to y....
Definition: ConstrainedStateSpace.h:307
unsigned int validSegmentCount(const State *s1, const State *s2) const override
Return the valid segment count on the manifold, as valid segment count is determined by delta_ and la...
Definition: ConstrainedStateSpace.h:258
State space wrapper that transparently passes state space operations through to the underlying space....
Definition: WrapperStateSpace.h:163
unsigned int getManifoldDimension() const
Returns the dimension of the manifold.
Definition: ConstrainedStateSpace.h:334
void sanityChecks() const override
Perform both constrained and regular sanity checks.
Definition: ConstrainedStateSpace.cpp:186
@ CONSTRAINED_STATESPACE_GEODESIC_SATISFY
Check whether discrete geodesics satisfy the constraint at all points.
Definition: ConstrainedStateSpace.h:178
void setSpaceInformation(SpaceInformation *si)
Sets the space information for this state space. Required for collision checking in manifold traversa...
Definition: ConstrainedStateSpace.cpp:198
StateType(const ConstrainedStateSpace *space)
Constructor. Requires space to setup information about underlying state.
Definition: ConstrainedStateSpace.h:204
const ConstraintPtr constraint_
Constraint function that defines the manifold.
Definition: ConstrainedStateSpace.h:353
@ CONSTRAINED_STATESPACE_JACOBIAN
Check if the constraint's numerical Jacobian approximates its provided Jacobian.
Definition: ConstrainedStateSpace.h:190