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(
58  near->as<SO2StateSpace::StateType>()->value - distance, 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 if (v >= boost::math::constants::pi<double>())
89  v -= 2.0 * boost::math::constants::pi<double>();
90  state->as<StateType>()->value = v;
91 }
92 
94 {
95  return (state->as<StateType>()->value < boost::math::constants::pi<double>()) &&
96  (state->as<StateType>()->value >= -boost::math::constants::pi<double>());
97 }
98 
99 void ompl::base::SO2StateSpace::copyState(State *destination, const State *source) const
100 {
101  destination->as<StateType>()->value = source->as<StateType>()->value;
102 }
103 
105 {
106  return sizeof(double);
107 }
108 
109 void ompl::base::SO2StateSpace::serialize(void *serialization, const State *state) const
110 {
111  memcpy(serialization, &state->as<StateType>()->value, sizeof(double));
112 }
113 
114 void ompl::base::SO2StateSpace::deserialize(State *state, const void *serialization) const
115 {
116  memcpy(&state->as<StateType>()->value, serialization, sizeof(double));
117 }
118 
119 double ompl::base::SO2StateSpace::distance(const State *state1, const State *state2) const
120 {
121  // assuming the states 1 & 2 are within bounds
122  double d = fabs(state1->as<StateType>()->value - state2->as<StateType>()->value);
123  BOOST_ASSERT_MSG(satisfiesBounds(state1) && satisfiesBounds(state2),
124  "The states passed to SO2StateSpace::distance are not within bounds. Call "
125  "SO2StateSpace::enforceBounds() in, e.g., ompl::control::ODESolver::PostPropagationEvent, "
126  "ompl::control::StatePropagator, or ompl::base::StateValidityChecker");
127  return (d > boost::math::constants::pi<double>()) ? 2.0 * boost::math::constants::pi<double>() - d : d;
128 }
129 
130 bool ompl::base::SO2StateSpace::equalStates(const State *state1, const State *state2) const
131 {
132  return fabs(state1->as<StateType>()->value - state2->as<StateType>()->value) <
133  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 if (v < -boost::math::constants::pi<double>())
153  v += 2.0 * boost::math::constants::pi<double>();
154  }
155 }
156 
158 {
159  return std::make_shared<SO2StateSampler>(this);
160 }
161 
163 {
164  return new StateType();
165 }
166 
168 {
169  delete static_cast<StateType *>(state);
170 }
171 
173 {
174  class SO2DefaultProjection : public ProjectionEvaluator
175  {
176  public:
177  SO2DefaultProjection(const StateSpace *space) : ProjectionEvaluator(space)
178  {
179  }
180 
181  unsigned int getDimension() const override
182  {
183  return 1;
184  }
185 
186  void defaultCellSizes() override
187  {
188  cellSizes_.resize(1);
189  cellSizes_[0] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS;
190  bounds_.resize(1);
191  bounds_.low[0] = -boost::math::constants::pi<double>();
192  bounds_.high[0] = boost::math::constants::pi<double>();
193  }
194 
195  void project(const State *state, EuclideanProjection &projection) const override
196  {
197  projection(0) = state->as<SO2StateSpace::StateType>()->value;
198  }
199  };
200 
201  registerDefaultProjection(std::make_shared<SO2DefaultProjection>(this));
202 }
203 
204 double *ompl::base::SO2StateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
205 {
206  return index == 0 ? &(state->as<StateType>()->value) : nullptr;
207 }
208 
209 void ompl::base::SO2StateSpace::printState(const State *state, std::ostream &out) const
210 {
211  out << "SO2State [";
212  if (state != nullptr)
213  out << state->as<StateType>()->value;
214  else
215  out << "nullptr";
216  out << ']' << std::endl;
217 }
218 
219 void ompl::base::SO2StateSpace::printSettings(std::ostream &out) const
220 {
221  out << "SO2 state space '" << getName() << "'" << std::endl;
222 }
bool equalStates(const State *state1, const State *state2) const override
Checks whether two states are equal.
const StateSpace * space_
The state space this sampler samples.
Definition: StateSampler.h:104
double value
The value of the angle in the interval (-Pi, Pi].
Definition: SO2StateSpace.h:77
void printSettings(std::ostream &out) const override
Print the settings for this state space to a stream.
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...
double getMaximumExtent() const override
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
void sampleUniform(State *state) override
Sample a state.
void copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap. ...
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 serialize(void *serialization, const State *state) const override
Write the binary representation of state to serialization.
The definition of a state in SO(2)
Definition: SO2StateSpace.h:67
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
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...
bool satisfiesBounds(const State *state) const override
Check if the value of the state is in the interval [-Pi, Pi)
unsigned int getDimension() const override
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
static const double PROJECTION_DIMENSION_SPLITS
When the cell sizes for a projection are automatically computed, this value defines the number of par...
double uniformReal(double lower_bound, double upper_bound)
Generate a random real within given bounds: [lower_bound, upper_bound)
Definition: RandomNumbers.h:74
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
Definition: StateSpace.h:70
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:49
double getMeasure() const override
Get a measure of the space (this can be thought of as a generalization of volume) ...
void deserialize(State *state, const void *serialization) const override
Read the binary representation of a state from serialization and write it to state.
void sampleUniformNear(State *state, const State *near, double distance) override
Sample a state near another, within a neighborhood controlled by a distance parameter.
void enforceBounds(State *state) const override
Normalize the value of the state to the interval [-Pi, Pi)
State * allocState() const override
Allocate a state that can store a point in the described space.
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 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...
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.
void printState(const State *state, std::ostream &out) const override
Print a state to a stream.
unsigned int getSerializationLength() const override
Get the number of chars in the serialization of a state in this space.
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
void sampleGaussian(State *state, const State *mean, double stdDev) override
Sample a state using a Gaussian distribution with given mean and standard deviation (stdDev)...
double gaussian(double mean, double stddev)
Generate a random real using a normal distribution with given mean and variance.