37 #include "ompl/base/spaces/SO2StateSpace.h"
41 #include "ompl/tools/config/MagicConstants.h"
42 #include <boost/math/constants/constants.hpp>
45 #ifndef BOOST_ASSERT_MSG
46 #define BOOST_ASSERT_MSG(expr, msg) assert(expr)
52 rng_.
uniformReal(-boost::math::constants::pi<double>(), boost::math::constants::pi<double>());
59 space_->enforceBounds(state);
65 space_->enforceBounds(state);
75 return boost::math::constants::pi<double>();
80 double v = fmod(state->
as<
StateType>()->
value, 2.0 * boost::math::constants::pi<double>());
81 if (v <= -boost::math::constants::pi<double>())
82 v += 2.0 * boost::math::constants::pi<double>();
84 if (v > boost::math::constants::pi<double>())
85 v -= 2.0 * boost::math::constants::pi<double>();
91 return (state->
as<
StateType>()->
value <= boost::math::constants::pi<double>()) &&
92 (state->
as<
StateType>()->value > -boost::math::constants::pi<double>());
102 return sizeof(double);
119 BOOST_ASSERT_MSG(satisfiesBounds(state1) && satisfiesBounds(state2),
120 "The states passed to SO2StateSpace::distance are not within bounds. Call "
121 "SO2StateSpace::enforceBounds() in, e.g., ompl::control::ODESolver::PostPropagationEvent, "
122 "ompl::control::StatePropagator, or ompl::base::StateValidityChecker");
123 return (d > boost::math::constants::pi<double>()) ? 2.0 * boost::math::constants::pi<double>() - d : d;
134 if (fabs(diff) <= boost::math::constants::pi<double>())
140 diff = 2.0 * boost::math::constants::pi<double>() - diff;
142 diff = -2.0 * boost::math::constants::pi<double>() - diff;
145 if (v > boost::math::constants::pi<double>())
146 v -= 2.0 * boost::math::constants::pi<double>();
148 if (v < -boost::math::constants::pi<double>())
149 v += 2.0 * boost::math::constants::pi<double>();
178 virtual unsigned int getDimension(
void)
const
183 virtual void defaultCellSizes(
void)
185 cellSizes_.resize(1);
188 bounds_.low[0] = -boost::math::constants::pi<double>();
189 bounds_.high[0] = boost::math::constants::pi<double>();
198 registerDefaultProjection(
ProjectionEvaluatorPtr(dynamic_cast<ProjectionEvaluator*>(
new SO2DefaultProjection(
this))));
203 return index == 0 ? &(state->
as<
StateType>()->value) : NULL;
213 out <<
']' << std::endl;
218 out <<
"SO2 state space '" << getName() <<
"'" << std::endl;
virtual void copyState(State *destination, const State *source) const
Copy a state to another. The memory of source and destination should NOT overlap. ...
double value
The value of the angle in the interval (-Pi, Pi].
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...
A boost shared pointer wrapper for ompl::base::StateSampler.
RNG rng_
An instance of a random number generator.
virtual State * allocState(void) const
Allocate a state that can store a point in the described space.
virtual unsigned int getSerializationLength(void) const
Get the number of chars in the serialization of a state in this space.
virtual StateSamplerPtr allocDefaultStateSampler(void) const
Allocate an instance of the default uniform state sampler for this space.
virtual void sampleGaussian(State *state, const State *mean, const double stdDev)
Sample a state using a Gaussian distribution with given mean and standard deviation (stdDev) ...
The definition of a state in SO(2)
const T * as(void) const
Cast this instance to a desired type.
virtual void sampleUniform(State *state)
Sample a state.
A boost shared pointer wrapper for ompl::base::ProjectionEvaluator.
static const double PROJECTION_DIMENSION_SPLITS
When the cell sizes for a projection are automatically computed, this value defines the number of par...
virtual void sampleUniformNear(State *state, const State *near, const double distance)
Sample a state near another, within specified distance.
double uniformReal(double lower_bound, double upper_bound)
Generate a random real within given bounds: [lower_bound, upper_bound)
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
State space sampler for SO(2)
boost::numeric::ublas::vector< double > EuclideanProjection
The datatype for state projections. This class contains a real vector.
Definition of an abstract state.
virtual bool satisfiesBounds(const State *state) const
Check if the value of the state is in the interval (-Pi, Pi].
virtual void serialize(void *serialization, const State *state) const
Write the binary representation of state to serialization.
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...
virtual void printSettings(std::ostream &out) const
Print the settings for this state space to a stream.
virtual void registerProjections(void)
Register the projections for this state space. Usually, this is at least the default projection...
virtual void enforceBounds(State *state) const
Normalize the value of the state to the interval (-Pi, Pi].
virtual void printState(const State *state, std::ostream &out) const
Print a state to a stream.
virtual void freeState(State *state) const
Free the memory of the allocated state.
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...
virtual void deserialize(State *state, const void *serialization) const
Read the binary representation of a state from serialization and write it to state.
virtual unsigned int getDimension(void) const
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
virtual double getMaximumExtent(void) const
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
virtual bool equalStates(const State *state1, const State *state2) const
Checks whether two states are equal.