SO3StateSpace.cpp
52 BOOST_DEFINE_MATH_CONSTANT(root_three, 1.732050807568877293527446341505872366, 1.73205080756887729352744634150587236694280525381038062805580697945193301690880003708114618675724857567562614142, 0)
63 static inline void computeAxisAngle(SO3StateSpace::StateType &q, double ax, double ay, double az, double angle)
80 static inline void quaternionProduct(SO3StateSpace::StateType &q, const SO3StateSpace::StateType &q0, const SO3StateSpace::StateType &q1)
96 void ompl::base::SO3StateSpace::StateType::setAxisAngle(double ax, double ay, double az, double angle)
112 void ompl::base::SO3StateSampler::sampleUniformNear(State *state, const State *near, const double distance)
128 void ompl::base::SO3StateSampler::sampleGaussian(State *state, const State * mean, const double stdDev)
153 double x = rng_.gaussian(0, rotDev), y = rng_.gaussian(0, rotDev), z = rng_.gaussian(0, rotDev),
196 // see http://stackoverflow.com/questions/11667783/quaternion-and-normalization/12934750#12934750
259 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
296 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
298 void ompl::base::SO3StateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
378 registerDefaultProjection(ProjectionEvaluatorPtr(dynamic_cast<ProjectionEvaluator*>(new SO3DefaultProjection(this))));
381 double* ompl::base::SO3StateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
virtual void registerProjections()
Register the projections for this state space. Usually, this is at least the default projection...
Definition: SO3StateSpace.cpp:344
A boost shared pointer wrapper for ompl::base::StateSampler.
virtual State * allocState() const
Allocate a state that can store a point in the described space.
Definition: SO3StateSpace.cpp:334
virtual bool satisfiesBounds(const State *state) const
Check if a state is inside the bounding box. For unbounded spaces this function can always return tru...
Definition: SO3StateSpace.cpp:224
virtual void enforceBounds(State *state) const
Bring the state within the bounds of the state space. For unbounded spaces this function can be a no-...
Definition: SO3StateSpace.cpp:194
virtual void freeState(State *state) const
Free the memory of the allocated state.
Definition: SO3StateSpace.cpp:339
virtual double getMaximumExtent() const
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
Definition: SO3StateSpace.cpp:177
virtual void copyState(State *destination, const State *source) const
Copy a state to another. The memory of source and destination should NOT overlap. ...
Definition: SO3StateSpace.cpp:229
virtual void printState(const State *state, std::ostream &out) const
Print a state to a stream.
Definition: SO3StateSpace.cpp:386
virtual unsigned int getSerializationLength() const
Get the number of chars in the serialization of a state in this space.
Definition: SO3StateSpace.cpp:239
The definition of a state in SO(3) represented as a unit quaternion.
Definition: SO3StateSpace.h:94
virtual unsigned int getDimension() const
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
Definition: SO3StateSpace.cpp:172
double norm(const StateType *state) const
Compute the norm of a state.
Definition: SO3StateSpace.cpp:188
State space sampler for SO(3), using quaternion representation.
Definition: SO3StateSpace.h:48
A boost shared pointer wrapper for ompl::base::ProjectionEvaluator.
virtual StateSamplerPtr allocDefaultStateSampler() const
Allocate an instance of the default uniform state sampler for this space.
Definition: SO3StateSpace.cpp:329
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
Definition: StateSpace.h:73
virtual double * getValueAddressAtIndex(State *state, const unsigned int index) const
Many states contain a number of double values. This function provides a means to get the memory addre...
Definition: SO3StateSpace.cpp:381
boost::numeric::ublas::vector< double > EuclideanProjection
The datatype for state projections. This class contains a real vector.
Definition: ProjectionEvaluator.h:62
void setAxisAngle(double ax, double ay, double az, double angle)
Set the quaternion from axis-angle representation.
Definition: SO3StateSpace.cpp:96
virtual void serialize(void *serialization, const State *state) const
Write the binary representation of state to serialization.
Definition: SO3StateSpace.cpp:244
virtual double getMeasure() const
Get a measure of the space (this can be thought of as a generalization of volume) ...
Definition: SO3StateSpace.cpp:182
virtual void deserialize(State *state, const void *serialization) const
Read the binary representation of a state from serialization and write it to state.
Definition: SO3StateSpace.cpp:249
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...
Definition: SO3StateSpace.cpp:112
virtual void printSettings(std::ostream &out) const
Print the settings for this state space to a stream.
Definition: SO3StateSpace.cpp:399
virtual bool equalStates(const State *state1, const State *state2) const
Checks whether two states are equal.
Definition: SO3StateSpace.cpp:288
virtual double distance(const State *state1, const State *state2) const
Computes distance between two states. This function satisfies the properties of a metric if isMetricS...
Definition: SO3StateSpace.cpp:279
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.
Definition: SO3StateSpace.cpp:128
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
Definition: ProjectionEvaluator.h:138
virtual void interpolate(const State *from, const State *to, const double t, State *state) const
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state...
Definition: SO3StateSpace.cpp:298