SO2StateSpace.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/SO2StateSpace.h"
38 #include <algorithm>
39 #include <limits>
40 #include <cmath>
41 #include "ompl/tools/config/MagicConstants.h"
42 #include <boost/math/constants/constants.hpp>
43 
44 // Define for boost version < 1.47
45 #ifndef BOOST_ASSERT_MSG
46 #define BOOST_ASSERT_MSG(expr, msg) assert(expr)
47 #endif
48 
50 {
51  state->as<SO2StateSpace::StateType>()->value =
52  rng_.uniformReal(-boost::math::constants::pi<double>(), boost::math::constants::pi<double>());
53 }
54 
55 void ompl::base::SO2StateSampler::sampleUniformNear(State *state, const State *near, const double distance)
56 {
57  state->as<SO2StateSpace::StateType>()->value = rng_.uniformReal(near->as<SO2StateSpace::StateType>()->value - distance,
58  near->as<SO2StateSpace::StateType>()->value + distance);
59  space_->enforceBounds(state);
60 }
61 
62 void ompl::base::SO2StateSampler::sampleGaussian(State *state, const State *mean, const double stdDev)
63 {
64  state->as<SO2StateSpace::StateType>()->value = rng_.gaussian(mean->as<SO2StateSpace::StateType>()->value, stdDev);
65  space_->enforceBounds(state);
66 }
67 
69 {
70  return 1;
71 }
72 
74 {
75  return boost::math::constants::pi<double>();
76 }
77 
79 {
80  return 2.0 * boost::math::constants::pi<double>();
81 }
82 
84 {
85  double v = fmod(state->as<StateType>()->value, 2.0 * boost::math::constants::pi<double>());
86  if (v <= -boost::math::constants::pi<double>())
87  v += 2.0 * boost::math::constants::pi<double>();
88  else
89  if (v > boost::math::constants::pi<double>())
90  v -= 2.0 * boost::math::constants::pi<double>();
91  state->as<StateType>()->value = v;
92 }
93 
95 {
96  return (state->as<StateType>()->value <= boost::math::constants::pi<double>()) &&
97  (state->as<StateType>()->value > -boost::math::constants::pi<double>());
98 }
99 
100 void ompl::base::SO2StateSpace::copyState(State *destination, const State *source) const
101 {
102  destination->as<StateType>()->value = source->as<StateType>()->value;
103 }
104 
106 {
107  return sizeof(double);
108 }
109 
110 void ompl::base::SO2StateSpace::serialize(void *serialization, const State *state) const
111 {
112  memcpy(serialization, &state->as<StateType>()->value, sizeof(double));
113 }
114 
115 void ompl::base::SO2StateSpace::deserialize(State *state, const void *serialization) const
116 {
117  memcpy(&state->as<StateType>()->value, serialization, sizeof(double));
118 }
119 
120 double ompl::base::SO2StateSpace::distance(const State *state1, const State *state2) const
121 {
122  // assuming the states 1 & 2 are within bounds
123  double d = fabs(state1->as<StateType>()->value - state2->as<StateType>()->value);
124  BOOST_ASSERT_MSG(satisfiesBounds(state1) && satisfiesBounds(state2),
125  "The states passed to SO2StateSpace::distance are not within bounds. Call "
126  "SO2StateSpace::enforceBounds() in, e.g., ompl::control::ODESolver::PostPropagationEvent, "
127  "ompl::control::StatePropagator, or ompl::base::StateValidityChecker");
128  return (d > boost::math::constants::pi<double>()) ? 2.0 * boost::math::constants::pi<double>() - d : d;
129 }
130 
131 bool ompl::base::SO2StateSpace::equalStates(const State *state1, const State *state2) const
132 {
133  return fabs(state1->as<StateType>()->value - state2->as<StateType>()->value) < std::numeric_limits<double>::epsilon() * 2.0;
134 }
135 
136 void ompl::base::SO2StateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
137 {
138  double diff = to->as<StateType>()->value - from->as<StateType>()->value;
139  if (fabs(diff) <= boost::math::constants::pi<double>())
140  state->as<StateType>()->value = from->as<StateType>()->value + diff * t;
141  else
142  {
143  double &v = state->as<StateType>()->value;
144  if (diff > 0.0)
145  diff = 2.0 * boost::math::constants::pi<double>() - diff;
146  else
147  diff = -2.0 * boost::math::constants::pi<double>() - diff;
148  v = from->as<StateType>()->value - diff * t;
149  // input states are within bounds, so the following check is sufficient
150  if (v > boost::math::constants::pi<double>())
151  v -= 2.0 * boost::math::constants::pi<double>();
152  else
153  if (v < -boost::math::constants::pi<double>())
154  v += 2.0 * boost::math::constants::pi<double>();
155  }
156 }
157 
159 {
160  return StateSamplerPtr(new SO2StateSampler(this));
161 }
162 
164 {
165  return new StateType();
166 }
167 
169 {
170  delete static_cast<StateType*>(state);
171 }
172 
174 {
175  class SO2DefaultProjection : public ProjectionEvaluator
176  {
177  public:
178 
179  SO2DefaultProjection(const StateSpace *space) : ProjectionEvaluator(space)
180  {
181  }
182 
183  virtual unsigned int getDimension() const
184  {
185  return 1;
186  }
187 
188  virtual void defaultCellSizes()
189  {
190  cellSizes_.resize(1);
191  cellSizes_[0] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS;
192  bounds_.resize(1);
193  bounds_.low[0] = -boost::math::constants::pi<double>();
194  bounds_.high[0] = boost::math::constants::pi<double>();
195  }
196 
197  virtual void project(const State *state, EuclideanProjection &projection) const
198  {
199  projection(0) = state->as<SO2StateSpace::StateType>()->value;
200  }
201  };
202 
203  registerDefaultProjection(ProjectionEvaluatorPtr(dynamic_cast<ProjectionEvaluator*>(new SO2DefaultProjection(this))));
204 }
205 
206 double* ompl::base::SO2StateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
207 {
208  return index == 0 ? &(state->as<StateType>()->value) : NULL;
209 }
210 
211 void ompl::base::SO2StateSpace::printState(const State *state, std::ostream &out) const
212 {
213  out << "SO2State [";
214  if (state)
215  out << state->as<StateType>()->value;
216  else
217  out << "NULL";
218  out << ']' << std::endl;
219 }
220 
221 void ompl::base::SO2StateSpace::printSettings(std::ostream &out) const
222 {
223  out << "SO2 state space '" << getName() << "'" << std::endl;
224 }
const StateSpace * space_
The state space this sampler samples.
Definition: StateSampler.h:91
double value
The value of the angle in the interval (-Pi, Pi].
Definition: SO2StateSpace.h:81
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...
A boost shared pointer wrapper for ompl::base::StateSampler.
RNG rng_
An instance of a random number generator.
Definition: StateSampler.h:94
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 double getMeasure() const
Get a measure of the space (this can be thought of as a generalization of volume) ...
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...
SO2StateSampler(const StateSpace *space)
Constructor.
Definition: SO2StateSpace.h:53
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) ...
The definition of a state in SO(2)
Definition: SO2StateSpace.h:70
virtual unsigned int getSerializationLength() const
Get the number of chars in the serialization of a state in this space.
const T * as() const
Cast this instance to a desired type.
Definition: State.h:74
virtual void printSettings(std::ostream &out) const
Print the settings for this state space to a stream.
virtual unsigned int getDimension() const
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
virtual void printState(const State *state, std::ostream &out) const
Print a state to a stream.
virtual void sampleUniform(State *state)
Sample a state.
virtual bool satisfiesBounds(const State *state) const
Check if the value of the state is in the interval (-Pi, Pi].
A boost shared pointer wrapper for ompl::base::ProjectionEvaluator.
static const double PROJECTION_DIMENSION_SPLITS
When the cell sizes for a projection are automatically computed, this value defines the number of par...
virtual void sampleUniformNear(State *state, const State *near, const double distance)
Sample a state near another, within specified distance.
double uniformReal(double lower_bound, double upper_bound)
Generate a random real within given bounds: [lower_bound, upper_bound)
Definition: RandomNumbers.h:68
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
Definition: StateSpace.h:73
virtual State * allocState() const
Allocate a state that can store a point in the described space.
boost::numeric::ublas::vector< double > EuclideanProjection
The datatype for state projections. This class contains a real vector.
Definition of an abstract state.
Definition: State.h:50
virtual void serialize(void *serialization, const State *state) const
Write the binary representation of state to serialization.
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-...
virtual void registerProjections()
Register the projections for this state space. Usually, this is at least the default 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 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 bool equalStates(const State *state1, const State *state2) const
Checks whether two states are equal.
virtual void freeState(State *state) const
Free the memory of the allocated state.
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
virtual void copyState(State *destination, const State *source) const
Copy a state to another. The memory of source and destination should NOT overlap. ...
virtual void enforceBounds(State *state) const
Normalize the value of the state to the interval (-Pi, Pi].
double gaussian(double mean, double stddev)
Generate a random real using a normal distribution with given mean and variance.
Definition: RandomNumbers.h:94