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 <algorithm>
41 #include <cstring>
42 #include <limits>
43 #include <cmath>
44 
46 {
47  const unsigned int dim = space_->getDimension();
48  const RealVectorBounds &bounds = static_cast<const RealVectorStateSpace *>(space_)->getBounds();
49 
50  auto *rstate = static_cast<RealVectorStateSpace::StateType *>(state);
51  for (unsigned int i = 0; i < dim; ++i)
52  rstate->values[i] = rng_.uniformReal(bounds.low[i], bounds.high[i]);
53 }
54 
55 void ompl::base::RealVectorStateSampler::sampleUniformNear(State *state, const State *near, const double distance)
56 {
57  const unsigned int dim = space_->getDimension();
58  const RealVectorBounds &bounds = static_cast<const RealVectorStateSpace *>(space_)->getBounds();
59 
60  auto *rstate = static_cast<RealVectorStateSpace::StateType *>(state);
61  const auto *rnear = static_cast<const RealVectorStateSpace::StateType *>(near);
62  for (unsigned int i = 0; i < dim; ++i)
63  rstate->values[i] = rng_.uniformReal(std::max(bounds.low[i], rnear->values[i] - distance),
64  std::min(bounds.high[i], rnear->values[i] + distance));
65 }
66 
67 void ompl::base::RealVectorStateSampler::sampleGaussian(State *state, const State *mean, const double stdDev)
68 {
69  const unsigned int dim = space_->getDimension();
70  const RealVectorBounds &bounds = static_cast<const RealVectorStateSpace *>(space_)->getBounds();
71 
72  auto *rstate = static_cast<RealVectorStateSpace::StateType *>(state);
73  const auto *rmean = static_cast<const RealVectorStateSpace::StateType *>(mean);
74  for (unsigned int i = 0; i < dim; ++i)
75  {
76  double v = rng_.gaussian(rmean->values[i], stdDev);
77  if (v < bounds.low[i])
78  v = bounds.low[i];
79  else if (v > bounds.high[i])
80  v = bounds.high[i];
81  rstate->values[i] = v;
82  }
83 }
84 
86 {
87  // compute a default random projection
88  if (dimension_ > 0)
89  {
90  if (dimension_ > 2)
91  {
92  int p = std::max(2, (int)ceil(log((double)dimension_)));
93  registerDefaultProjection(std::make_shared<RealVectorRandomLinearProjectionEvaluator>(this, p));
94  }
95  else
96  registerDefaultProjection(std::make_shared<RealVectorIdentityProjectionEvaluator>(this));
97  }
98 }
99 
101 {
102  bounds_.check();
104 }
105 
106 void ompl::base::RealVectorStateSpace::addDimension(const std::string &name, double minBound, double maxBound)
107 {
108  addDimension(minBound, maxBound);
109  setDimensionName(dimension_ - 1, name);
110 }
111 
112 void ompl::base::RealVectorStateSpace::addDimension(double minBound, double maxBound)
113 {
114  dimension_++;
115  stateBytes_ = dimension_ * sizeof(double);
116  bounds_.low.push_back(minBound);
117  bounds_.high.push_back(maxBound);
118  dimensionNames_.resize(dimension_, "");
119 }
120 
122 {
123  bounds.check();
124  if (bounds.low.size() != dimension_)
125  throw Exception("Bounds do not match dimension of state space: expected dimension " +
126  std::to_string(dimension_) + " but got dimension " + std::to_string(bounds.low.size()));
127  bounds_ = bounds;
128 }
129 
130 void ompl::base::RealVectorStateSpace::setBounds(double low, double high)
131 {
132  RealVectorBounds bounds(dimension_);
133  bounds.setLow(low);
134  bounds.setHigh(high);
135  setBounds(bounds);
136 }
137 
139 {
140  return dimension_;
141 }
142 
143 const std::string &ompl::base::RealVectorStateSpace::getDimensionName(unsigned int index) const
144 {
145  if (index < dimensionNames_.size())
146  return dimensionNames_[index];
147  throw Exception("Index out of bounds");
148 }
149 
150 int ompl::base::RealVectorStateSpace::getDimensionIndex(const std::string &name) const
151 {
152  auto it = dimensionIndex_.find(name);
153  return it != dimensionIndex_.end() ? (int)it->second : -1;
154 }
155 
156 void ompl::base::RealVectorStateSpace::setDimensionName(unsigned int index, const std::string &name)
157 {
158  if (index < dimensionNames_.size())
159  {
160  dimensionNames_[index] = name;
161  dimensionIndex_[name] = index;
162  }
163  else
164  throw Exception("Cannot set dimension name. Index out of bounds");
165 }
166 
168 {
169  double e = 0.0;
170  for (unsigned int i = 0; i < dimension_; ++i)
171  {
172  double d = bounds_.high[i] - bounds_.low[i];
173  e += d * d;
174  }
175  return sqrt(e);
176 }
177 
179 {
180  double m = 1.0;
181  for (unsigned int i = 0; i < dimension_; ++i)
182  {
183  m *= bounds_.high[i] - bounds_.low[i];
184  }
185  return m;
186 }
187 
189 {
190  auto *rstate = static_cast<StateType *>(state);
191  for (unsigned int i = 0; i < dimension_; ++i)
192  {
193  if (rstate->values[i] > bounds_.high[i])
194  rstate->values[i] = bounds_.high[i];
195  else if (rstate->values[i] < bounds_.low[i])
196  rstate->values[i] = bounds_.low[i];
197  }
198 }
199 
201 {
202  const auto *rstate = static_cast<const StateType *>(state);
203  for (unsigned int i = 0; i < dimension_; ++i)
204  if (rstate->values[i] - std::numeric_limits<double>::epsilon() > bounds_.high[i] ||
205  rstate->values[i] + std::numeric_limits<double>::epsilon() < bounds_.low[i])
206  return false;
207  return true;
208 }
209 
210 void ompl::base::RealVectorStateSpace::copyState(State *destination, const State *source) const
211 {
212  memcpy(static_cast<StateType *>(destination)->values, static_cast<const StateType *>(source)->values, stateBytes_);
213 }
214 
216 {
217  return stateBytes_;
218 }
219 
220 void ompl::base::RealVectorStateSpace::serialize(void *serialization, const State *state) const
221 {
222  memcpy(serialization, state->as<StateType>()->values, stateBytes_);
223 }
224 
225 void ompl::base::RealVectorStateSpace::deserialize(State *state, const void *serialization) const
226 {
227  memcpy(state->as<StateType>()->values, serialization, stateBytes_);
228 }
229 
230 double ompl::base::RealVectorStateSpace::distance(const State *state1, const State *state2) const
231 {
232  double dist = 0.0;
233  const double *s1 = static_cast<const StateType *>(state1)->values;
234  const double *s2 = static_cast<const StateType *>(state2)->values;
235 
236  for (unsigned int i = 0; i < dimension_; ++i)
237  {
238  double diff = (*s1++) - (*s2++);
239  dist += diff * diff;
240  }
241  return sqrt(dist);
242 }
243 
244 bool ompl::base::RealVectorStateSpace::equalStates(const State *state1, const State *state2) const
245 {
246  const double *s1 = static_cast<const StateType *>(state1)->values;
247  const double *s2 = static_cast<const StateType *>(state2)->values;
248  for (unsigned int i = 0; i < dimension_; ++i)
249  {
250  double diff = (*s1++) - (*s2++);
251  if (fabs(diff) > std::numeric_limits<double>::epsilon() * 2.0)
252  return false;
253  }
254  return true;
255 }
256 
257 void ompl::base::RealVectorStateSpace::interpolate(const State *from, const State *to, const double t,
258  State *state) const
259 {
260  const auto *rfrom = static_cast<const StateType *>(from);
261  const auto *rto = static_cast<const StateType *>(to);
262  const StateType *rstate = static_cast<StateType *>(state);
263  for (unsigned int i = 0; i < dimension_; ++i)
264  rstate->values[i] = rfrom->values[i] + (rto->values[i] - rfrom->values[i]) * t;
265 }
266 
268 {
269  return std::make_shared<RealVectorStateSampler>(this);
270 }
271 
273 {
274  auto *rstate = new StateType();
275  rstate->values = new double[dimension_];
276  return rstate;
277 }
278 
280 {
281  auto *rstate = static_cast<StateType *>(state);
282  delete[] rstate->values;
283  delete rstate;
284 }
285 
286 double *ompl::base::RealVectorStateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
287 {
288  return index < dimension_ ? static_cast<StateType *>(state)->values + index : nullptr;
289 }
290 
291 void ompl::base::RealVectorStateSpace::printState(const State *state, std::ostream &out) const
292 {
293  out << "RealVectorState [";
294  if (state != nullptr)
295  {
296  const auto *rstate = static_cast<const StateType *>(state);
297  for (unsigned int i = 0; i < dimension_; ++i)
298  {
299  out << rstate->values[i];
300  if (i + 1 < dimension_)
301  out << ' ';
302  }
303  }
304  else
305  out << "nullptr" << std::endl;
306  out << ']' << std::endl;
307 }
308 
310 {
311  out << "Real vector state space '" << getName() << "' of dimension " << dimension_ << " with bounds: " << std::endl;
312  out << " - min: ";
313  for (unsigned int i = 0; i < dimension_; ++i)
314  out << bounds_.low[i] << " ";
315  out << std::endl;
316  out << " - max: ";
317  for (unsigned int i = 0; i < dimension_; ++i)
318  out << bounds_.high[i] << " ";
319  out << std::endl;
320 
321  bool printNames = false;
322  for (unsigned int i = 0; i < dimension_; ++i)
323  if (!dimensionNames_[i].empty())
324  printNames = true;
325  if (printNames)
326  {
327  out << " and dimension names: ";
328  for (unsigned int i = 0; i < dimension_; ++i)
329  out << "'" << dimensionNames_[i] << "' ";
330  out << std::endl;
331  }
332 }
bool satisfiesBounds(const State *state) const override
Check if a state is inside the bounding box. For unbounded spaces this function can always return tru...
const StateSpace * space_
The state space this sampler samples.
Definition: StateSampler.h:104
void enforceBounds(State *state) const override
Bring the state within the bounds of the state space. For unbounded spaces this function can be a no-...
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:120
std::vector< double > low
Lower bound.
A shared pointer wrapper for ompl::base::StateSampler.
RNG rng_
An instance of a random number generator.
Definition: StateSampler.h:107
void registerProjections() override
Register the projections for this state space. Usually, this is at least the default projection...
virtual unsigned int getDimension() const =0
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
void printState(const State *state, std::ostream &out) const override
Print a state to a stream.
double * getValueAddressAtIndex(State *state, unsigned int index) const override
Many states contain a number of double values. This function provides a means to get the memory addre...
void copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap. ...
State * allocState() const override
Allocate a state that can store a point in the described space.
void sampleGaussian(State *state, const State *mean, double stdDev) override
Sample a state such that each component state[i] has a Gaussian distribution with mean mean[i] and st...
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...
void setLow(double value)
Set the lower bound in each dimension to a specific value.
bool equalStates(const State *state1, const State *state2) const override
Checks whether two states are equal.
void serialize(void *serialization, const State *state) const override
Write the binary representation of state to serialization.
void setHigh(double value)
Set the upper bound in each dimension to a specific value.
void freeState(State *state) const override
Free the memory of the allocated state.
void sampleUniform(State *state) override
Sample a state.
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
std::vector< double > high
Upper bound.
void interpolate(const State *from, const State *to, double t, State *state) const override
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state...
StateSamplerPtr allocDefaultStateSampler() const override
Allocate an instance of the default uniform state sampler for this space.
void check() const
Check if the bounds are valid (same length for low and high, high[i] > low[i]). Throw an exception if...
double uniformReal(double lower_bound, double upper_bound)
Generate a random real within given bounds: [lower_bound, upper_bound)
Definition: RandomNumbers.h:74
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:248
Definition of an abstract state.
Definition: State.h:49
void setDimensionName(unsigned int index, const std::string &name)
Set the name of a dimension.
double getMaximumExtent() const override
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
The exception type for ompl.
Definition: Exception.h:46
unsigned int getSerializationLength() const override
Get the number of chars in the serialization of a state in this space.
double distance(const State *state1, const State *state2) const override
Computes distance between two states. This function satisfies the properties of a metric if isMetricS...
The lower and upper bounds for an Rn space.
double * values
The value of the actual vector in Rn
void deserialize(State *state, const void *serialization) const override
Read the binary representation of a state from serialization and write it to state.
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...
double getMeasure() const override
Get a measure of the space (this can be thought of as a generalization of volume) ...
void printSettings(std::ostream &out) const override
Print the settings for this state space to a stream.
unsigned int getDimension() const override
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
void sampleUniformNear(State *state, const State *near, double distance) override
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.
void setup() override
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.
int getDimensionIndex(const std::string &name) const
Get the index of a specific dimension, by name. Return -1 if name is not found.
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.