Loading...
Searching...
No Matches
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
45using namespace boost::math::double_constants; // pi
46using namespace ompl::base;
47
48SphereStateSampler::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
61void 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
70void 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
84SphereStateSpace::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
98double 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();
136 return state;
137}
138
139Eigen::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}
double gaussian(double mean, double stddev)
Generate a random real using a normal distribution with given mean and variance.
double uniformReal(double lower_bound, double upper_bound)
Generate a random real within given bounds: [lower_bound, upper_bound)
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition StateSpace.h:577
void allocStateComponents(CompoundState *state) const
Allocate the state components. Called by allocState(). Usually called by derived state spaces.
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...
void lock()
Lock this state space. This means no further spaces can be added as components. This function can be ...
A state space representing Rn. The distance function is the L2 norm.
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 sampleUniform(State *state) override
Sample a state.
void sampleUniformNear(State *state, const State *near, double distance) override
Sample a state near another, within a neighborhood controlled by a distance parameter.
virtual double getMeasure() const override
Get a measure of the space (this can be thought of as a generalization of volume)
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...
virtual State * allocState() const override
Allocate a state that can store a point in the described space.
virtual StateSamplerPtr allocDefaultStateSampler() const override
Allocate an instance of the default uniform state sampler for this space.
A shared pointer wrapper for ompl::base::StateSampler.
Abstract definition of a state space sampler.
RNG rng_
An instance of a random number generator.
const StateSpace * space_
The state space this sampler samples.
A shared pointer wrapper for ompl::base::StateSpace.
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition StateSpace.h:71
int type_
A type assigned for this state space.
Definition StateSpace.h:531
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-...
void setName(const std::string &name)
Set the name of the state space.
const std::string & getName() const
Get the name of the state space.
Definition of an abstract state.
Definition State.h:50
const T * as() const
Cast this instance to a desired type.
Definition State.h:66
This namespace contains sampling based planning routines shared by both planning under geometric cons...
@ STATE_SPACE_SPHERE
ompl::base::SphereStateSpace