37 #ifndef OMPL_BASE_SPACES_SO3_STATE_SPACE_ 38 #define OMPL_BASE_SPACES_SO3_STATE_SPACE_ 40 #include "ompl/base/StateSpace.h" 99 void setAxisAngle(
double ax,
double ay,
double az,
double angle);
119 setName(
"SO3" + getName());
128 double norm(
const StateType *state)
const;
130 virtual unsigned int getDimension()
const;
132 virtual double getMaximumExtent()
const;
134 virtual double getMeasure()
const;
136 virtual void enforceBounds(
State *state)
const;
138 virtual bool satisfiesBounds(
const State *state)
const;
140 virtual void copyState(
State *destination,
const State *source)
const;
142 virtual unsigned int getSerializationLength()
const;
144 virtual void serialize(
void *serialization,
const State *state)
const;
146 virtual void deserialize(
State *state,
const void *serialization)
const;
148 virtual double distance(
const State *state1,
const State *state2)
const;
150 virtual bool equalStates(
const State *state1,
const State *state2)
const;
152 virtual void interpolate(
const State *from,
const State *to,
const double t,
State *state)
const;
156 virtual State* allocState()
const;
158 virtual void freeState(
State *state)
const;
160 virtual double* getValueAddressAtIndex(
State *state,
const unsigned int index)
const;
162 virtual void printState(
const State *state, std::ostream &out)
const;
164 virtual void printSettings(std::ostream &out)
const;
166 virtual void registerProjections();
ompl::base::SO3StateSpace
A boost shared pointer wrapper for ompl::base::StateSampler.
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)
Constructor.
double w
scalar component of quaternion
Main namespace. Contains everything in this library.
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.
Definition of an abstract state.
virtual void sampleUniform(State *state)
Sample a state.
double z
Z component of quaternion vector.
virtual void sampleUniformNear(State *state, const State *near, const double distance)
To sample unit quaternions uniformly within some given distance, we sample a 3-vector from the R^3 ta...
Abstract definition of a state space sampler.
double y
Y component of quaternion vector.
virtual void sampleGaussian(State *state, const State *mean, const double stdDev)
Sample a state such that the expected distance between mean and state is stdDev.
double x
X component of quaternion vector.