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 using namespace boost::math::double_constants;
45 
47 {
48  state->as<SO2StateSpace::StateType>()->value =
49  rng_.uniformReal(-pi, pi);
50 }
51 
52 void ompl::base::SO2StateSampler::sampleUniformNear(State *state, const State *near, const double distance)
53 {
54  state->as<SO2StateSpace::StateType>()->value = rng_.uniformReal(
55  near->as<SO2StateSpace::StateType>()->value - distance, near->as<SO2StateSpace::StateType>()->value + distance);
56  space_->enforceBounds(state);
57 }
58 
59 void ompl::base::SO2StateSampler::sampleGaussian(State *state, const State *mean, const double stdDev)
60 {
61  state->as<SO2StateSpace::StateType>()->value = rng_.gaussian(mean->as<SO2StateSpace::StateType>()->value, stdDev);
62  space_->enforceBounds(state);
63 }
64 
66 {
67  return 1;
68 }
69 
71 {
72  return pi;
73 }
74 
76 {
77  return 2.0 * pi;
78 }
79 
81 {
82  double v = fmod(state->as<StateType>()->value, 2.0 * pi);
83  if (v < -pi)
84  v += 2.0 * pi;
85  else if (v >= pi)
86  v -= 2.0 * pi;
87  state->as<StateType>()->value = v;
88 }
89 
91 {
92  return (state->as<StateType>()->value < pi) &&
93  (state->as<StateType>()->value >= -pi);
94 }
95 
96 void ompl::base::SO2StateSpace::copyState(State *destination, const State *source) const
97 {
98  destination->as<StateType>()->value = source->as<StateType>()->value;
99 }
100 
102 {
103  return sizeof(double);
104 }
105 
106 void ompl::base::SO2StateSpace::serialize(void *serialization, const State *state) const
107 {
108  memcpy(serialization, &state->as<StateType>()->value, sizeof(double));
109 }
110 
111 void ompl::base::SO2StateSpace::deserialize(State *state, const void *serialization) const
112 {
113  memcpy(&state->as<StateType>()->value, serialization, sizeof(double));
114 }
115 
116 double ompl::base::SO2StateSpace::distance(const State *state1, const State *state2) const
117 {
118  // assuming the states 1 & 2 are within bounds
119  double d = fabs(state1->as<StateType>()->value - state2->as<StateType>()->value);
120  BOOST_ASSERT_MSG(satisfiesBounds(state1) && satisfiesBounds(state2), "The states passed to SO2StateSpace::distance "
121  "are not within bounds. Call "
122  "SO2StateSpace::enforceBounds() in, e.g., "
123  "ompl::control::ODESolver::"
124  "PostPropagationEvent, "
125  "ompl::control::StatePropagator, or "
126  "ompl::base::StateValidityChecker");
127  return (d > pi) ? 2.0 * pi - 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) <= pi)
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 * pi - diff;
146  else
147  diff = -2.0 * pi - diff;
148  v = from->as<StateType>()->value - diff * t;
149  // input states are within bounds, so the following check is sufficient
150  if (v > pi)
151  v -= 2.0 * pi;
152  else if (v < -pi)
153  v += 2.0 * pi;
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] = pi / magic::PROJECTION_DIMENSION_SPLITS;
190  bounds_.resize(1);
191  bounds_.low[0] = -pi;
192  bounds_.high[0] = pi;
193  }
194 
195  void project(const State *state, Eigen::Ref<Eigen::VectorXd> 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 }
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:135
State * allocState() const override
Allocate a state that can store a point in the described space.
Definition of an abstract state.
Definition: State.h:114
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.
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.
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.