SphereStateSpace.cpp
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2021,
5  * Max Planck Institute for Intelligent Systems (MPI-IS).
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of the MPI-IS nor the names
19  * of its contributors may be used to endorse or promote products
20  * derived from this software without specific prior written
21  * permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *********************************************************************/
36 
37 /* Author: Andreas Orthey */
38 
39 #include <ompl/base/spaces/special/SphereStateSpace.h>
40 #include <ompl/tools/config/MagicConstants.h>
41 #include <cstring>
42 #include <cmath>
43 #include <boost/math/constants/constants.hpp>
44 
45 using namespace boost::math::double_constants; // pi
46 using namespace ompl::base;
47 
48 SphereStateSampler::SphereStateSampler(const StateSpace *space) : StateSampler(space)
49 {
50 }
51 
53 {
54  // see for example http://corysimon.github.io/articles/uniformdistn-on-sphere/
55  const double theta = 2.0 * pi * rng_.uniformReal(0, 1) - pi; // uniform in [-pi,+pi]
56  const double phi = acos(1.0 - 2.0 * rng_.uniformReal(0, 1)); // in [0,+pi]
57  auto *S = state->as<SphereStateSpace::StateType>();
58  S->setThetaPhi(theta, phi);
59 }
60 
61 void SphereStateSampler::sampleUniformNear(State *state, const State *near, double distance)
62 {
63  auto *S = state->as<SphereStateSpace::StateType>();
64  const auto *Snear = near->as<SphereStateSpace::StateType>();
65  S->setTheta(rng_.uniformReal(Snear->getTheta() - distance, Snear->getTheta() + distance));
66  S->setPhi(rng_.uniformReal(Snear->getPhi() - distance, Snear->getPhi() + distance));
67  space_->enforceBounds(state);
68 }
69 
70 void SphereStateSampler::sampleGaussian(State *state, const State *mean, double stdDev)
71 {
72  auto *S = state->as<SphereStateSpace::StateType>();
73  const auto *Smean = mean->as<SphereStateSpace::StateType>();
74  S->setTheta(rng_.gaussian(Smean->getTheta(), stdDev));
75  S->setPhi(rng_.gaussian(Smean->getPhi(), stdDev));
76  space_->enforceBounds(state);
77 }
78 
80 {
81  return std::make_shared<SphereStateSampler>(this);
82 }
83 
84 SphereStateSpace::SphereStateSpace(double radius) : radius_(radius)
85 {
86  setName("Sphere" + getName());
88 
89  StateSpacePtr SO2(std::make_shared<SO2StateSpace>());
90  StateSpacePtr R1(std::make_shared<RealVectorStateSpace>(1));
91  R1->as<RealVectorStateSpace>()->setBounds(0, pi);
92 
93  addSubspace(SO2, 1.0);
94  addSubspace(R1, 1.0);
95  lock();
96 }
97 
98 double SphereStateSpace::distance(const State *state1, const State *state2) const
99 {
100  // https://en.wikipedia.org/wiki/Great-circle_distance#Formulae
101 
102  const auto *S1 = state1->as<SphereStateSpace::StateType>();
103  const auto *S2 = state2->as<SphereStateSpace::StateType>();
104 
105  // Note: Formula assumes phi in [-pi/2,+pi/2]
106  const float t1 = S1->getTheta();
107  const float phi1 = S1->getPhi() - pi / 2.0;
108 
109  const float t2 = S2->getTheta();
110  const float phi2 = S2->getPhi() - pi / 2.0;
111 
112  // This is the Vincenty formula, but it is less numerically stable
113  // double dt = t2 - t1;
114  // double d1 = std::pow(cos(phi2) * std::sin(dt), 2);
115  // double d2 = std::pow(cos(phi1) * std::sin(phi2) - std::sin(phi1) * cos(phi2) * cos(dt), 2);
116  // double numerator = std::sqrt(d1 + d2);
117  // double denumerator = std::sin(phi1) * std::sin(phi2) + cos(phi1) * cos(phi2) * cos(dt);
118  // return radius_ * atan2(numerator, denumerator);
119 
120  // Haversine formula
121  const float s = 0.5 * (phi1 - phi2);
122  const float t = 0.5 * (t1 - t2);
123  const float d = std::sqrt(std::sin(s) * std::sin(s) + std::cos(phi1) * std::cos(phi2) * std::sin(t) * std::sin(t));
124  return 2 * radius_ * asin(d);
125 }
126 
128 {
129  return 4 * pi * radius_ * radius_;
130 }
131 
133 {
134  auto *state = new StateType();
135  allocStateComponents(state);
136  return state;
137 }
138 
139 Eigen::Vector3f SphereStateSpace::toVector(const State *state) const
140 {
141  Eigen::Vector3f v;
142 
143  const auto *S1 = state->as<SphereStateSpace::StateType>();
144  const float theta = S1->getTheta();
145  const float phi = S1->getPhi();
146 
147  v[0] = radius_ * std::sin(phi) * std::cos(theta);
148  v[1] = radius_ * std::sin(phi) * std::sin(theta);
149  v[2] = radius_ * std::cos(phi);
150 
151  return v;
152 }
@ STATE_SPACE_SPHERE
ompl::base::SphereStateSpace
void sampleUniformNear(State *state, const State *near, double distance) override
Sample a state near another, within a neighborhood controlled by a distance parameter.
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition: StateSpace.h:134
virtual StateSamplerPtr allocDefaultStateSampler() const override
Allocate an instance of the default uniform state sampler for this space.
virtual 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
This namespace contains sampling based planning routines shared by both planning under geometric cons...
void lock()
Lock this state space. This means no further spaces can be added as components. This function can be ...
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-...
const T * as() const
Cast this instance to a desired type.
Definition: State.h:162
A state space representing Rn. The distance function is the L2 norm.
const StateSpace * space_
The state space this sampler samples.
Definition: StateSampler.h:168
virtual 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...
double gaussian(double mean, double stddev)
Generate a random real using a normal distribution with given mean and variance.
const std::string & getName() const
Get the name of the state space.
Definition: StateSpace.cpp:196
void setName(const std::string &name)
Set the name of the state space.
Definition: StateSpace.cpp:201
int type_
A type assigned for this state space.
Definition: StateSpace.h:595
void allocStateComponents(CompoundState *state) const
Allocate the state components. Called by allocState(). Usually called by derived state spaces.
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:641
RNG rng_
An instance of a random number generator.
Definition: StateSampler.h:171
void sampleUniform(State *state) override
Sample a state.
double uniformReal(double lower_bound, double upper_bound)
Generate a random real within given bounds: [lower_bound, upper_bound)
Abstract definition of a state space sampler.
Definition: StateSampler.h:128
A shared pointer wrapper for ompl::base::StateSpace.
A shared pointer wrapper for ompl::base::StateSampler.
void addSubspace(const StateSpacePtr &component, double weight)
Adds a new state space as part of the compound state space. For computing distances within the compou...
Definition: StateSpace.cpp:871
virtual double getMeasure() const override
Get a measure of the space (this can be thought of as a generalization of volume)
void sampleGaussian(State *state, const State *mean, double stdDev) override
Sample a state using a Gaussian distribution with given mean and standard deviation (stdDev).