42 namespace ompl
115 type_ = STATE_SPACE_SO3;
118 ~SO3StateSpace() override = default;
121 double norm(const StateType *state) const;
129 void enforceBounds(State *state) const override;
131 bool satisfiesBounds(const State *state) const override;
137 void serialize(void *serialization, const State *state) const override;
139 void deserialize(State *state, const void *serialization) const override;
145 void interpolate(const State *from, const State *to, const double t, State *state) const override;
147 StateSamplerPtr allocDefaultStateSampler() const override;
149 State *allocState() const override;
151 void freeState(State *state) const override;
153 double *getValueAddressAtIndex(State *state, const unsigned int index) const override;
155 void printState(const State *state, std::ostream &out) const override;
A shared pointer wrapper for ompl::base::StateSampler.
void sampleUniformNear(State *state, const State *near, const double distance) override
To sample unit quaternions uniformly within some given distance, we sample a 3-vector from the R^3 ta...
A state space representing SO(3). The internal representation is done with quaternions. The distance between states is the angle between quaternions and interpolation is done with slerp.
SO3StateSampler(const StateSpace *space)
void sampleUniform(State *state) override
Sample a state.
scalar component of quaternion
The definition of a state in SO(3) represented as a unit quaternion.
State space sampler for SO(3), using quaternion representation.
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
void sampleGaussian(State *state, const State *mean, const double stdDev) override
Sample a state such that the expected distance between mean and state is stdDev.
Z component of quaternion vector.
Y component of quaternion vector.