DiscreteStateSpace.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, 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: Elizabeth Fudge */
36 
37 #include "ompl/base/spaces/DiscreteStateSpace.h"
38 #include "ompl/util/Exception.h"
39 #include <limits>
40 #include <cstdlib>
41 
43 {
44  state->as<DiscreteStateSpace::StateType>()->value =
46  space_->as<DiscreteStateSpace>()->getUpperBound());
47 }
48 
49 void ompl::base::DiscreteStateSampler::sampleUniformNear(State *state, const State *near, const double distance)
50 {
51  const int d = (int)floor(distance + 0.5);
52  state->as<DiscreteStateSpace::StateType>()->value =
53  rng_.uniformInt(near->as<DiscreteStateSpace::StateType>()->value - d,
54  near->as<DiscreteStateSpace::StateType>()->value + d);
55  space_->enforceBounds(state);
56 }
57 
58 void ompl::base::DiscreteStateSampler::sampleGaussian(State *state, const State *mean, const double stdDev)
59 {
60  state->as<DiscreteStateSpace::StateType>()->value =
61  (int)floor(rng_.gaussian(mean->as<DiscreteStateSpace::StateType>()->value, stdDev) + 0.5);
62  space_->enforceBounds(state);
63 }
64 
66 {
67  return true;
68 }
69 
71 {
72  return 1;
73 }
74 
76 {
77  return upperBound_ - lowerBound_;
78 }
79 
81 {
82  return upperBound_ - lowerBound_ + 1.0;
83 }
84 
86 {
87  if (state->as<StateType>()->value < lowerBound_)
88  state->as<StateType>()->value = lowerBound_;
89  else
90  if (state->as<StateType>()->value > upperBound_)
91  state->as<StateType>()->value = upperBound_;
92 }
93 
95 {
96  return state->as<StateType>()->value >= lowerBound_ && state->as<StateType>()->value <= upperBound_;
97 }
98 
99 void ompl::base::DiscreteStateSpace::copyState(State *destination, const State *source) const
100 {
101  destination->as<StateType>()->value = source->as<StateType>()->value;
102 }
103 
105 {
106  return sizeof(int);
107 }
108 
109 void ompl::base::DiscreteStateSpace::serialize(void *serialization, const State *state) const
110 {
111  memcpy(serialization, &state->as<StateType>()->value, sizeof(int));
112 }
113 
114 void ompl::base::DiscreteStateSpace::deserialize(State *state, const void *serialization) const
115 {
116  memcpy(&state->as<StateType>()->value, serialization, sizeof(int));
117 }
118 
119 double ompl::base::DiscreteStateSpace::distance(const State *state1, const State *state2) const
120 {
121  return abs(state1->as<StateType>()->value - state2->as<StateType>()->value);
122 }
123 
124 bool ompl::base::DiscreteStateSpace::equalStates(const State *state1, const State *state2) const
125 {
126  return state1->as<StateType>()->value == state2->as<StateType>()->value;
127 }
128 
129 void ompl::base::DiscreteStateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
130 {
131  state->as<StateType>()->value = (int)floor(from->as<StateType>()->value +
132  (to->as<StateType>()->value - from->as<StateType>()->value) * t + 0.5);
133 }
134 
136 {
137  return StateSamplerPtr(new DiscreteStateSampler(this));
138 }
139 
141 {
142  return new StateType();
143 }
144 
146 {
147  delete static_cast<StateType*>(state);
148 }
149 
151 {
152  class DiscreteDefaultProjection : public ProjectionEvaluator
153  {
154  public:
155 
156  DiscreteDefaultProjection(const StateSpace *space) : ProjectionEvaluator(space)
157  {
158  }
159 
160  virtual unsigned int getDimension() const
161  {
162  return 1;
163  }
164 
165  virtual void defaultCellSizes()
166  {
167  bounds_.resize(1);
168  bounds_.low[0] = space_->as<DiscreteStateSpace>()->lowerBound_;
169  bounds_.high[0] = space_->as<DiscreteStateSpace>()->upperBound_;
170  cellSizes_.resize(1);
171  cellSizes_[0] = 1.0;
172  }
173 
174  virtual void project(const State *state, EuclideanProjection &projection) const
175  {
176  projection(0) = state->as<DiscreteStateSpace::StateType>()->value;
177  }
178  };
179 
180  registerDefaultProjection(ProjectionEvaluatorPtr(dynamic_cast<ProjectionEvaluator*>(new DiscreteDefaultProjection(this))));
181 }
182 
184 {
185  if (lowerBound_ > upperBound_)
186  throw Exception("Lower bound cannot be larger than upper bound for a discrete space");
188 }
189 
190 void ompl::base::DiscreteStateSpace::printState(const State *state, std::ostream &out) const
191 {
192  out << "DiscreteState [";
193  if (state)
194  out << state->as<StateType>()->value;
195  else
196  out << "NULL";
197  out << ']' << std::endl;
198 }
199 
201 {
202  out << "Discrete state space '" << getName() << "' with bounds [" << lowerBound_ << ", " << upperBound_ << "]" << std::endl;
203 }
const StateSpace * space_
The state space this sampler samples.
Definition: StateSampler.h:91
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. ...
The definition of a discrete state.
virtual unsigned int getDimension() const
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
A boost shared pointer wrapper for ompl::base::StateSampler.
RNG rng_
An instance of a random number generator.
Definition: StateSampler.h:94
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) ...
virtual bool equalStates(const State *state1, const State *state2) const
Checks whether two states are equal.
virtual unsigned int getSerializationLength() const
Get the number of chars in the serialization of a state in this space.
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.
virtual void deserialize(State *state, const void *serialization) const
Read the binary representation of a state from serialization and write it to state.
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 enforceBounds(State *state) const
Bring the state within the bounds of the state space. For unbounded spaces this function can be a no-...
virtual void sampleUniformNear(State *state, const State *near, const double distance)
Sample a state near another, within specified distance.
T * as()
Cast this instance to a desired type.
Definition: StateSpace.h:87
virtual State * allocState() const
Allocate a state that can store a point in the described space.
virtual void printState(const State *state, std::ostream &out) const
Print a state to a stream.
virtual double getMeasure() const
Get a measure of the space (this can be thought of as a generalization of volume) ...
virtual StateSamplerPtr allocDefaultStateSampler() const
Allocate an instance of the default uniform state sampler for this space.
virtual double getMaximumExtent() const
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
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 bool isDiscrete() const
Check if the set of states is discrete.
State space sampler for discrete states.
A boost shared pointer wrapper for ompl::base::ProjectionEvaluator.
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
Definition: StateSpace.h:73
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
boost::numeric::ublas::vector< double > EuclideanProjection
The datatype for state projections. This class contains a real vector.
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 of an abstract state.
Definition: State.h:50
virtual void printSettings(std::ostream &out) const
Print the settings for this state space to a stream.
The exception type for ompl.
Definition: Exception.h:47
int getLowerBound() const
Returns the lowest possible state.
A space representing discrete states; i.e. there are a small number of discrete states the system can...
virtual void serialize(void *serialization, const State *state) const
Write the binary representation of state to serialization.
const T * as() const
Cast this instance to a desired type.
Definition: State.h:74
int value
The current state - an int in range [lowerBound, upperBound].
virtual void freeState(State *state) const
Free the memory of the allocated state.
virtual void registerProjections()
Register the projections for this state space. Usually, this is at least the default projection...
int uniformInt(int lower_bound, int upper_bound)
Generate a random integer within given bounds: [lower_bound, upper_bound].
Definition: RandomNumbers.h:75
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...