37 #include "ompl/base/spaces/RealVectorStateSpace.h"
38 #include "ompl/base/spaces/RealVectorStateProjections.h"
39 #include "ompl/util/Exception.h"
40 #include <boost/lexical_cast.hpp>
52 for (
unsigned int i = 0 ; i < dim ; ++i)
58 const unsigned int dim = space_->getDimension();
63 for (
unsigned int i = 0 ; i < dim ; ++i)
65 rng_.uniformReal(std::max(bounds.
low[i], rnear->
values[i] - distance),
66 std::min(bounds.
high[i], rnear->
values[i] + distance));
71 const unsigned int dim = space_->getDimension();
76 for (
unsigned int i = 0 ; i < dim ; ++i)
78 double v = rng_.gaussian(rmean->
values[i], stdDev);
79 if (v < bounds.
low[i])
82 if (v > bounds.
high[i])
95 int p = std::max(2, (
int)ceil(
log((
double)dimension_)));
111 addDimension(minBound, maxBound);
112 setDimensionName(dimension_ - 1, name);
118 stateBytes_ = dimension_ *
sizeof(double);
119 bounds_.low.push_back(minBound);
120 bounds_.high.push_back(maxBound);
121 dimensionNames_.resize(dimension_,
"");
127 if (bounds.
low.size() != dimension_)
128 throw Exception(
"Bounds do not match dimension of state space: expected dimension " +
129 boost::lexical_cast<std::string>(dimension_) +
" but got dimension " +
130 boost::lexical_cast<std::string>(bounds.
low.size()));
149 if (index < dimensionNames_.size())
150 return dimensionNames_[index];
156 std::map<std::string, unsigned int>::const_iterator it = dimensionIndex_.find(name);
157 return it != dimensionIndex_.end() ? (int)it->second : -1;
162 if (index < dimensionNames_.size())
164 dimensionNames_[index] = name;
165 dimensionIndex_[name] = index;
168 throw Exception(
"Cannot set dimension name. Index out of bounds");
174 for (
unsigned int i = 0 ; i < dimension_ ; ++i)
176 double d = bounds_.high[i] - bounds_.low[i];
185 for (
unsigned int i = 0 ; i < dimension_ ; ++i)
187 if (rstate->
values[i] > bounds_.high[i])
188 rstate->
values[i] = bounds_.high[i];
190 if (rstate->
values[i] < bounds_.low[i])
191 rstate->
values[i] = bounds_.low[i];
198 for (
unsigned int i = 0 ; i < dimension_ ; ++i)
199 if (rstate->
values[i] - std::numeric_limits<double>::epsilon() > bounds_.high[i] ||
200 rstate->
values[i] + std::numeric_limits<double>::epsilon() < bounds_.low[i])
207 memcpy(static_cast<StateType*>(destination)->values,
208 static_cast<const StateType*>(source)->values, stateBytes_);
229 const double *s1 =
static_cast<const StateType*
>(state1)->values;
230 const double *s2 =
static_cast<const StateType*
>(state2)->values;
232 for (
unsigned int i = 0 ; i < dimension_ ; ++i)
234 double diff = (*s1++) - (*s2++);
242 const double *s1 =
static_cast<const StateType*
>(state1)->values;
243 const double *s2 =
static_cast<const StateType*
>(state2)->values;
244 for (
unsigned int i = 0 ; i < dimension_ ; ++i)
246 double diff = (*s1++) - (*s2++);
247 if (fabs(diff) > std::numeric_limits<double>::epsilon() * 2.0)
258 for (
unsigned int i = 0 ; i < dimension_ ; ++i)
270 rstate->
values =
new double[dimension_];
283 return index < dimension_ ? static_cast<StateType*>(state)->values + index : NULL;
288 out <<
"RealVectorState [";
292 for (
unsigned int i = 0 ; i < dimension_ ; ++i)
295 if (i + 1 < dimension_)
300 out <<
"NULL" << std::endl;
301 out <<
']' << std::endl;
306 out <<
"Real vector state space '" << getName() <<
"' of dimension " << dimension_ <<
" with bounds: " << std::endl;
308 for (
unsigned int i = 0 ; i < dimension_ ; ++i)
309 out << bounds_.low[i] <<
" ";
312 for (
unsigned int i = 0 ; i < dimension_ ; ++i)
313 out << bounds_.high[i] <<
" ";
316 bool printNames =
false;
317 for (
unsigned int i = 0 ; i < dimension_ ; ++i)
318 if (!dimensionNames_[i].empty())
322 out <<
" and dimension names: ";
323 for (
unsigned int i = 0 ; i < dimension_ ; ++i)
324 out <<
"'" << dimensionNames_[i] <<
"' ";
int getDimensionIndex(const std::string &name) const
Get the index of a specific dimension, by name. Return -1 if name is not found.
const StateSpace * space_
The state space this sampler samples.
State sampler for the Rn state space.
virtual void setup(void)
Perform final setup steps. This function is automatically called by the SpaceInformation. If any default projections are to be registered, this call will set them and call their setup() functions. It is safe to call this function multiple times. At a subsequent call, projections that have been previously user configured are not re-instantiated, but their setup() method is still called.
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...
void log(const char *file, int line, LogLevel level, const char *m,...)
Root level logging function. This should not be invoked directly, but rather used via a logging macro...
virtual StateSamplerPtr allocDefaultStateSampler(void) const
Allocate an instance of the default uniform state sampler for this space.
std::vector< double > low
Lower bound.
virtual void registerProjections(void)
Register the projections for this state space. Usually, this is at least the default projection...
virtual void copyState(State *destination, const State *source) const
Copy a state to another. The memory of source and destination should NOT overlap. ...
virtual State * allocState(void) const
Allocate a state that can store a point in the described space.
A boost shared pointer wrapper for ompl::base::StateSampler.
RNG rng_
An instance of a random number generator.
virtual unsigned int getDimension(void) const =0
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
const std::string & getDimensionName(unsigned int index) const
Each dimension can optionally have a name associated to it. If it does, this function returns that na...
virtual void sampleUniformNear(State *state, const State *near, const double distance)
Sample a state such that each component state[i] is uniformly sampled from [near[i]-distance, near[i]+distance]. If this interval exceeds the state space bounds, the interval is truncated.
virtual double getMaximumExtent(void) const
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
Define the identity projection.
virtual void deserialize(State *state, const void *serialization) const
Read the binary representation of a state from serialization and write it to state.
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-...
virtual unsigned int getDimension(void) const
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
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 for a class computing a random linear projections.
const T * as(void) const
Cast this instance to a desired type.
virtual void freeState(State *state) const
Free the memory of the allocated state.
void setLow(double value)
Set the lower bound in each dimension to a specific value.
void setHigh(double value)
Set the upper bound in each dimension to a specific value.
virtual void printSettings(std::ostream &out) const
Print the settings for this state space to a stream.
std::vector< double > high
Upper bound.
A boost shared pointer wrapper for ompl::base::ProjectionEvaluator.
double uniformReal(double lower_bound, double upper_bound)
Generate a random real within given bounds: [lower_bound, upper_bound)
A state space representing Rn. The distance function is the L2 norm.
Definition of an abstract state.
virtual void sampleGaussian(State *state, const State *mean, const double stdDev)
Sample a state such that each component state[i] has a Gaussian distribution with mean mean[i] and st...
void setDimensionName(unsigned int index, const std::string &name)
Set the name of a dimension.
void check(void) const
Check if the bounds are valid (same length for low and high, high[i] > low[i]). Throw an exception if...
virtual bool equalStates(const State *state1, const State *state2) const
Checks whether two states are equal.
The exception type for ompl.
The lower and upper bounds for an Rn space.
double * values
The value of the actual vector in Rn
The definition of a state in Rn
virtual unsigned int getSerializationLength(void) const
Get the number of chars in the serialization of a state in this space.
virtual void printState(const State *state, std::ostream &out) const
Print a state to a stream.
virtual void serialize(void *serialization, const State *state) const
Write the binary representation of state to serialization.
void addDimension(double minBound=0.0, double maxBound=0.0)
Increase the dimensionality of the state space by 1. Optionally, bounds can be specified for this add...
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...
virtual void sampleUniform(State *state)
Sample a state.
virtual void setup(void)
Perform final setup steps. This function is automatically called by the SpaceInformation. If any default projections are to be registered, this call will set them and call their setup() functions. It is safe to call this function multiple times. At a subsequent call, projections that have been previously user configured are not re-instantiated, but their setup() method is still called.
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...
void setBounds(const RealVectorBounds &bounds)
Set the bounds of this state space. This defines the range of the space in which sampling is performe...