TangentBundleStateSpace.cpp
62 constrainedSanityChecks(CONSTRAINED_STATESPACE_GEODESIC_CONTINUITY | CONSTRAINED_STATESPACE_SAMPLERS);
72 bool ompl::base::TangentBundleStateSpace::discreteGeodesic(const State *from, const State *to, bool interpolate,
180 ompl::base::State *ompl::base::TangentBundleStateSpace::geodesicInterpolate(const std::vector<State *> &geodesic,
Tangent space and bounding polytope approximating some patch of the manifold.
Definition: AtlasChart.h:53
bool psi(const Eigen::Ref< const Eigen::VectorXd > &u, Eigen::Ref< Eigen::VectorXd > out) const
Exponential mapping. Project chart point u onto the manifold and store the result in out,...
Definition: AtlasChart.cpp:190
void psiInverse(const Eigen::Ref< const Eigen::VectorXd > &x, Eigen::Ref< Eigen::VectorXd > out) const
Logarithmic mapping. Project ambient point x onto the chart and store the result in out,...
Definition: AtlasChart.cpp:232
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 constrainedSanityChecks(unsigned int flags) const
Do some sanity checks relating to discrete geodesic computation and constraint satisfaction....
Definition: ConstrainedStateSpace.cpp:111
@ 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
A shared pointer wrapper for ompl::base::Constraint.
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
void sanityChecks() const override
Do sanity checks, minus geodesic constraint satisfiability (as this is a lazy method).
Definition: TangentBundleStateSpace.cpp:60
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: TangentBundleStateSpace.cpp:72
State * geodesicInterpolate(const std::vector< State * > &geodesic, double t) const override
Like interpolate(...), but interpolates between intermediate states already supplied in stateList fro...
Definition: TangentBundleStateSpace.cpp:180
bool project(State *state) const
Reproject a state onto the surface of the manifold. Returns true if projection was successful,...
Definition: TangentBundleStateSpace.cpp:190