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 }
void serialize(void *serialization, const State *state) const override
Write the binary representation of state to serialization.
bool satisfiesBounds(const State *state) const override
Check if the value of the state is in the interval [-Pi, Pi)
void deserialize(State *state, const void *serialization) const override
Read the binary representation of a state from serialization and write it to state.
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition: StateSpace.h:134
State * allocState() const override
Allocate a state that can store a point in the described space.
Definition of an abstract state.
Definition: State.h:113
double getMeasure() const override
Get a measure of the space (this can be thought of as a generalization of volume)
unsigned int getDimension() const override
Get the dimension of the space (not the dimension of the surrounding ambient space)
void sampleGaussian(State *state, const State *mean, double stdDev) override
Sample a state using a Gaussian distribution with given mean and standard deviation (stdDev).
void enforceBounds(State *state) const override
Normalize the value of the state to the interval [-Pi, Pi)
bool equalStates(const State *state1, const State *state2) const override
Checks whether two states are equal.
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....
double value
The value of the angle in the interval (-Pi, Pi].
const T * as() const
Cast this instance to a desired type.
Definition: State.h:162
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.
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...
void registerProjections() override
Register the projections for this state space. Usually, this is at least the default projection....
void sampleUniform(State *state) override
Sample a state.
The definition of a state in SO(2)
void printSettings(std::ostream &out) const override
Print the settings for this state space to a stream.
static const double PROJECTION_DIMENSION_SPLITS
When the cell sizes for a projection are automatically computed, this value defines the number of par...
double getMaximumExtent() const override
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
void sampleUniformNear(State *state, const State *near, double distance) override
Sample a state near another, within a neighborhood controlled by a distance parameter.
void printState(const State *state, std::ostream &out) const override
Print a state to a stream.
RNG rng_
An instance of a random number generator.
Definition: StateSampler.h:171
double uniformReal(double lower_bound, double upper_bound)
Generate a random real within given bounds: [lower_bound, upper_bound)
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...
A shared pointer wrapper for ompl::base::StateSampler.
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 copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap.