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