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), "The states passed to SO2StateSpace::distance "
124  "are not within bounds. Call "
125  "SO2StateSpace::enforceBounds() in, e.g., "
126  "ompl::control::ODESolver::"
127  "PostPropagationEvent, "
128  "ompl::control::StatePropagator, or "
129  "ompl::base::StateValidityChecker");
130  return (d > boost::math::constants::pi<double>()) ? 2.0 * boost::math::constants::pi<double>() - d : d;
131 }
132 
133 bool ompl::base::SO2StateSpace::equalStates(const State *state1, const State *state2) const
134 {
135  return fabs(state1->as<StateType>()->value - state2->as<StateType>()->value) <
136  std::numeric_limits<double>::epsilon() * 2.0;
137 }
138 
139 void ompl::base::SO2StateSpace::interpolate(const State *from, const State *to, const double t, State *state) const
140 {
141  double diff = to->as<StateType>()->value - from->as<StateType>()->value;
142  if (fabs(diff) <= boost::math::constants::pi<double>())
143  state->as<StateType>()->value = from->as<StateType>()->value + diff * t;
144  else
145  {
146  double &v = state->as<StateType>()->value;
147  if (diff > 0.0)
148  diff = 2.0 * boost::math::constants::pi<double>() - diff;
149  else
150  diff = -2.0 * boost::math::constants::pi<double>() - diff;
151  v = from->as<StateType>()->value - diff * t;
152  // input states are within bounds, so the following check is sufficient
153  if (v > boost::math::constants::pi<double>())
154  v -= 2.0 * boost::math::constants::pi<double>();
155  else if (v < -boost::math::constants::pi<double>())
156  v += 2.0 * boost::math::constants::pi<double>();
157  }
158 }
159 
161 {
162  return std::make_shared<SO2StateSampler>(this);
163 }
164 
166 {
167  return new StateType();
168 }
169 
171 {
172  delete static_cast<StateType *>(state);
173 }
174 
176 {
177  class SO2DefaultProjection : public ProjectionEvaluator
178  {
179  public:
180  SO2DefaultProjection(const StateSpace *space) : ProjectionEvaluator(space)
181  {
182  }
183 
184  unsigned int getDimension() const override
185  {
186  return 1;
187  }
188 
189  void defaultCellSizes() override
190  {
191  cellSizes_.resize(1);
192  cellSizes_[0] = boost::math::constants::pi<double>() / magic::PROJECTION_DIMENSION_SPLITS;
193  bounds_.resize(1);
194  bounds_.low[0] = -boost::math::constants::pi<double>();
195  bounds_.high[0] = boost::math::constants::pi<double>();
196  }
197 
198  void project(const State *state, Eigen::Ref<Eigen::VectorXd> projection) const override
199  {
200  projection(0) = state->as<SO2StateSpace::StateType>()->value;
201  }
202  };
203 
204  registerDefaultProjection(std::make_shared<SO2DefaultProjection>(this));
205 }
206 
207 double *ompl::base::SO2StateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
208 {
209  return index == 0 ? &(state->as<StateType>()->value) : nullptr;
210 }
211 
212 void ompl::base::SO2StateSpace::printState(const State *state, std::ostream &out) const
213 {
214  out << "SO2State [";
215  if (state != nullptr)
216  out << state->as<StateType>()->value;
217  else
218  out << "nullptr";
219  out << ']' << std::endl;
220 }
221 
222 void ompl::base::SO2StateSpace::printSettings(std::ostream &out) const
223 {
224  out << "SO2 state space '" << getName() << "'" << std::endl;
225 }
bool equalStates(const State *state1, const State *state2) const override
Checks whether two states are equal.
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:72
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
Definition: StateSpace.h:70
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.
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)...