RealVectorStateSpace.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
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>
41 #include <algorithm>
42 #include <cstring>
43 #include <limits>
44 #include <cmath>
45 
47 {
48  const unsigned int dim = space_->getDimension();
49  const RealVectorBounds &bounds = static_cast<const RealVectorStateSpace*>(space_)->getBounds();
50 
52  for (unsigned int i = 0 ; i < dim ; ++i)
53  rstate->values[i] = rng_.uniformReal(bounds.low[i], bounds.high[i]);
54 }
55 
56 void ompl::base::RealVectorStateSampler::sampleUniformNear(State *state, const State *near, const double distance)
57 {
58  const unsigned int dim = space_->getDimension();
59  const RealVectorBounds &bounds = static_cast<const RealVectorStateSpace*>(space_)->getBounds();
60 
62  const RealVectorStateSpace::StateType *rnear = static_cast<const RealVectorStateSpace::StateType*>(near);
63  for (unsigned int i = 0 ; i < dim ; ++i)
64  rstate->values[i] =
65  rng_.uniformReal(std::max(bounds.low[i], rnear->values[i] - distance),
66  std::min(bounds.high[i], rnear->values[i] + distance));
67 }
68 
69 void ompl::base::RealVectorStateSampler::sampleGaussian(State *state, const State *mean, const double stdDev)
70 {
71  const unsigned int dim = space_->getDimension();
72  const RealVectorBounds &bounds = static_cast<const RealVectorStateSpace*>(space_)->getBounds();
73 
75  const RealVectorStateSpace::StateType *rmean = static_cast<const RealVectorStateSpace::StateType*>(mean);
76  for (unsigned int i = 0 ; i < dim ; ++i)
77  {
78  double v = rng_.gaussian(rmean->values[i], stdDev);
79  if (v < bounds.low[i])
80  v = bounds.low[i];
81  else
82  if (v > bounds.high[i])
83  v = bounds.high[i];
84  rstate->values[i] = v;
85  }
86 }
87 
89 {
90  // compute a default random projection
91  if (dimension_ > 0)
92  {
93  if (dimension_ > 2)
94  {
95  int p = std::max(2, (int)ceil(log((double)dimension_)));
96  registerDefaultProjection(ProjectionEvaluatorPtr(new RealVectorRandomLinearProjectionEvaluator(this, p)));
97  }
98  else
99  registerDefaultProjection(ProjectionEvaluatorPtr(new RealVectorIdentityProjectionEvaluator(this)));
100  }
101 }
102 
104 {
105  bounds_.check();
107 }
108 
109 void ompl::base::RealVectorStateSpace::addDimension(const std::string &name, double minBound, double maxBound)
110 {
111  addDimension(minBound, maxBound);
112  setDimensionName(dimension_ - 1, name);
113 }
114 
115 void ompl::base::RealVectorStateSpace::addDimension(double minBound, double maxBound)
116 {
117  dimension_++;
118  stateBytes_ = dimension_ * sizeof(double);
119  bounds_.low.push_back(minBound);
120  bounds_.high.push_back(maxBound);
121  dimensionNames_.resize(dimension_, "");
122 }
123 
125 {
126  bounds.check();
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()));
131  bounds_ = bounds;
132 }
133 
134 void ompl::base::RealVectorStateSpace::setBounds(double low, double high)
135 {
136  RealVectorBounds bounds(dimension_);
137  bounds.setLow(low);
138  bounds.setHigh(high);
139  setBounds(bounds);
140 }
141 
143 {
144  return dimension_;
145 }
146 
147 const std::string& ompl::base::RealVectorStateSpace::getDimensionName(unsigned int index) const
148 {
149  if (index < dimensionNames_.size())
150  return dimensionNames_[index];
151  throw Exception("Index out of bounds");
152 }
153 
154 int ompl::base::RealVectorStateSpace::getDimensionIndex(const std::string &name) const
155 {
156  std::map<std::string, unsigned int>::const_iterator it = dimensionIndex_.find(name);
157  return it != dimensionIndex_.end() ? (int)it->second : -1;
158 }
159 
160 void ompl::base::RealVectorStateSpace::setDimensionName(unsigned int index, const std::string &name)
161 {
162  if (index < dimensionNames_.size())
163  {
164  dimensionNames_[index] = name;
165  dimensionIndex_[name] = index;
166  }
167  else
168  throw Exception("Cannot set dimension name. Index out of bounds");
169 }
170 
172 {
173  double e = 0.0;
174  for (unsigned int i = 0 ; i < dimension_ ; ++i)
175  {
176  double d = bounds_.high[i] - bounds_.low[i];
177  e += d*d;
178  }
179  return sqrt(e);
180 }
181 
183 {
184  double m = 1.0;
185  for (unsigned int i = 0 ; i < dimension_ ; ++i)
186  {
187  m *= bounds_.high[i] - bounds_.low[i];
188  }
189  return m;
190 }
191 
193 {
194  StateType *rstate = static_cast<StateType*>(state);
195  for (unsigned int i = 0 ; i < dimension_ ; ++i)
196  {
197  if (rstate->values[i] > bounds_.high[i])
198  rstate->values[i] = bounds_.high[i];
199  else
200  if (rstate->values[i] < bounds_.low[i])
201  rstate->values[i] = bounds_.low[i];
202  }
203 }
204 
206 {
207  const StateType *rstate = static_cast<const StateType*>(state);
208  for (unsigned int i = 0 ; i < dimension_ ; ++i)
209  if (rstate->values[i] - std::numeric_limits<double>::epsilon() > bounds_.high[i] ||
210  rstate->values[i] + std::numeric_limits<double>::epsilon() < bounds_.low[i])
211  return false;
212  return true;
213 }
214 
215 void ompl::base::RealVectorStateSpace::copyState(State *destination, const State *source) const
216 {
217  memcpy(static_cast<StateType*>(destination)->values,
218  static_cast<const StateType*>(source)->values, stateBytes_);
219 }
220 
222 {
223  return stateBytes_;
224 }
225 
226 void ompl::base::RealVectorStateSpace::serialize(void *serialization, const State *state) const
227 {
228  memcpy(serialization, state->as<StateType>()->values, stateBytes_);
229 }
230 
231 void ompl::base::RealVectorStateSpace::deserialize(State *state, const void *serialization) const
232 {
233  memcpy(state->as<StateType>()->values, serialization, stateBytes_);
234 }
235 
236 double ompl::base::RealVectorStateSpace::distance(const State *state1, const State *state2) const
237 {
238  double dist = 0.0;
239  const double *s1 = static_cast<const StateType*>(state1)->values;
240  const double *s2 = static_cast<const StateType*>(state2)->values;
241 
242  for (unsigned int i = 0 ; i < dimension_ ; ++i)
243  {
244  double diff = (*s1++) - (*s2++);
245  dist += diff * diff;
246  }
247  return sqrt(dist);
248 }
249 
250 bool ompl::base::RealVectorStateSpace::equalStates(const State *state1, const State *state2) const
251 {
252  const double *s1 = static_cast<const StateType*>(state1)->values;
253  const double *s2 = static_cast<const StateType*>(state2)->values;
254  for (unsigned int i = 0 ; i < dimension_ ; ++i)
255  {
256  double diff = (*s1++) - (*s2++);
257  if (fabs(diff) > std::numeric_limits<double>::epsilon() * 2.0)
258  return false;
259  }
260  return true;
261 }
262 
263 void ompl::base::RealVectorStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
264 {
265  const StateType *rfrom = static_cast<const StateType*>(from);
266  const StateType *rto = static_cast<const StateType*>(to);
267  const StateType *rstate = static_cast<StateType*>(state);
268  for (unsigned int i = 0 ; i < dimension_ ; ++i)
269  rstate->values[i] = rfrom->values[i] + (rto->values[i] - rfrom->values[i]) * t;
270 }
271 
273 {
274  return StateSamplerPtr(new RealVectorStateSampler(this));
275 }
276 
278 {
279  StateType *rstate = new StateType();
280  rstate->values = new double[dimension_];
281  return rstate;
282 }
283 
285 {
286  StateType *rstate = static_cast<StateType*>(state);
287  delete[] rstate->values;
288  delete rstate;
289 }
290 
291 double* ompl::base::RealVectorStateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
292 {
293  return index < dimension_ ? static_cast<StateType*>(state)->values + index : NULL;
294 }
295 
296 void ompl::base::RealVectorStateSpace::printState(const State *state, std::ostream &out) const
297 {
298  out << "RealVectorState [";
299  if (state)
300  {
301  const StateType *rstate = static_cast<const StateType*>(state);
302  for (unsigned int i = 0 ; i < dimension_ ; ++i)
303  {
304  out << rstate->values[i];
305  if (i + 1 < dimension_)
306  out << ' ';
307  }
308  }
309  else
310  out << "NULL" << std::endl;
311  out << ']' << std::endl;
312 }
313 
315 {
316  out << "Real vector state space '" << getName() << "' of dimension " << dimension_ << " with bounds: " << std::endl;
317  out << " - min: ";
318  for (unsigned int i = 0 ; i < dimension_ ; ++i)
319  out << bounds_.low[i] << " ";
320  out << std::endl;
321  out << " - max: ";
322  for (unsigned int i = 0 ; i < dimension_ ; ++i)
323  out << bounds_.high[i] << " ";
324  out << std::endl;
325 
326  bool printNames = false;
327  for (unsigned int i = 0 ; i < dimension_ ; ++i)
328  if (!dimensionNames_[i].empty())
329  printNames = true;
330  if (printNames)
331  {
332  out << " and dimension names: ";
333  for (unsigned int i = 0 ; i < dimension_ ; ++i)
334  out << "'" << dimensionNames_[i] << "' ";
335  out << std::endl;
336  }
337 }
const StateSpace * space_
The state space this sampler samples.
Definition: StateSampler.h:91
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...
Definition: Console.cpp:104
std::vector< double > low
Lower bound.
A boost shared pointer wrapper for ompl::base::StateSampler.
RNG rng_
An instance of a random number generator.
Definition: StateSampler.h:94
virtual unsigned int getDimension() const =0
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
virtual void serialize(void *serialization, const State *state) const
Write the binary representation of state to serialization.
virtual unsigned int getSerializationLength() const
Get the number of chars in the serialization of a state in this space.
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 void printSettings(std::ostream &out) const
Print the settings for this state space to a stream.
virtual void freeState(State *state) const
Free the memory of the allocated state.
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 double getMeasure() const
Get a measure of the space (this can be thought of as a generalization of volume) ...
virtual void registerProjections()
Register the projections for this state space. Usually, this is at least the default projection...
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 for a class computing a random linear projections.
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 bool satisfiesBounds(const State *state) const
Check if a state is inside the bounding box. For unbounded spaces this function can always return tru...
const T * as() const
Cast this instance to a desired type.
Definition: State.h:74
RealVectorStateSampler(const StateSpace *space)
Constructor.
std::vector< double > high
Upper bound.
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 printState(const State *state, std::ostream &out) const
Print a state to a stream.
void check() const
Check if the bounds are valid (same length for low and high, high[i] > low[i]). Throw an exception if...
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)
Definition: RandomNumbers.h:68
A state space representing Rn. The distance function is the L2 norm.
virtual void setup()
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.
Definition: StateSpace.cpp:231
virtual bool equalStates(const State *state1, const State *state2) const
Checks whether two states are equal.
Definition of an abstract state.
Definition: State.h:50
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.
virtual void deserialize(State *state, const void *serialization) const
Read the binary representation of a state from serialization and write it to state.
The exception type for ompl.
Definition: Exception.h:47
The lower and upper bounds for an Rn space.
double * values
The value of the actual vector in Rn
virtual double getMaximumExtent() const
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
virtual State * allocState() const
Allocate a state that can store a point in the described space.
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 * 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 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 StateSamplerPtr allocDefaultStateSampler() const
Allocate an instance of the default uniform state sampler for this space.
virtual void sampleUniform(State *state)
Sample a state.
virtual void copyState(State *destination, const State *source) const
Copy a state to another. The memory of source and destination should NOT overlap. ...
virtual unsigned int getDimension() const
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
int getDimensionIndex(const std::string &name) const
Get the index of a specific dimension, by name. Return -1 if name is not found.
virtual void setup()
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.
void setBounds(const RealVectorBounds &bounds)
Set the bounds of this state space. This defines the range of the space in which sampling is performe...
double gaussian(double mean, double stddev)
Generate a random real using a normal distribution with given mean and variance.
Definition: RandomNumbers.h:94