Loading...
Searching...
No Matches
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
44using namespace boost::math::double_constants;
45
47{
48 state->as<SO2StateSpace::StateType>()->value =
49 rng_.uniformReal(-pi, pi);
50}
51
52void 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
59void 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
96void 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
106void ompl::base::SO2StateSpace::serialize(void *serialization, const State *state) const
107{
108 memcpy(serialization, &state->as<StateType>()->value, sizeof(double));
109}
110
111void ompl::base::SO2StateSpace::deserialize(State *state, const void *serialization) const
112{
113 memcpy(&state->as<StateType>()->value, serialization, sizeof(double));
114}
115
116double 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
130bool 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
136void 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
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
204double *ompl::base::SO2StateSpace::getValueAddressAtIndex(State *state, const unsigned int index) const
205{
206 return index == 0 ? &(state->as<StateType>()->value) : nullptr;
207}
208
209void 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
219void ompl::base::SO2StateSpace::printSettings(std::ostream &out) const
220{
221 out << "SO2 state space '" << getName() << "'" << std::endl;
222}
double uniformReal(double lower_bound, double upper_bound)
Generate a random real within given bounds: [lower_bound, upper_bound)
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).
void sampleUniformNear(State *state, const State *near, double distance) override
Sample a state near another, within a neighborhood controlled by a distance parameter.
void sampleUniform(State *state) override
Sample a state.
The definition of a state in SO(2)
double value
The value of the angle in the interval (-Pi, Pi].
void enforceBounds(State *state) const override
Normalize the value of the state to the interval [-Pi, Pi)
double getMaximumExtent() const override
Get the maximum value a call to distance() can return (or an upper bound). For unbounded state spaces...
double getMeasure() const override
Get a measure of the space (this can be thought of as a generalization of volume)
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....
unsigned int getSerializationLength() const override
Get the number of chars in the serialization of a state in this space.
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 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 printSettings(std::ostream &out) const override
Print the settings for this state space to a stream.
void serialize(void *serialization, const State *state) const override
Write the binary representation of state to serialization.
void printState(const State *state, std::ostream &out) const override
Print a state to a stream.
State * allocState() const override
Allocate a state that can store a point in the described space.
unsigned int getDimension() const override
Get the dimension of the space (not the dimension of the surrounding ambient space)
bool satisfiesBounds(const State *state) const override
Check if the value of the state is in the interval [-Pi, Pi)
void registerProjections() override
Register the projections for this state space. Usually, this is at least the default projection....
void copyState(State *destination, const State *source) const override
Copy a state to another. The memory of source and destination should NOT overlap.
void deserialize(State *state, const void *serialization) const override
Read the binary representation of a state from serialization and write it to state.
bool equalStates(const State *state1, const State *state2) const override
Checks whether two states are equal.
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.
RNG rng_
An instance of a random number generator.
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition StateSpace.h:71
Definition of an abstract state.
Definition State.h:50
const T * as() const
Cast this instance to a desired type.
Definition State.h:66