39#include <ompl/base/spaces/special/TorusStateSpace.h>
40#include <ompl/tools/config/MagicConstants.h>
44#include <boost/math/constants/constants.hpp>
45using namespace boost::math::double_constants;
59 bool acceptedSampleFound =
false;
60 while (!acceptedSampleFound)
62 const double u =
rng_.uniformReal(-pi, pi);
63 const double v =
rng_.uniformReal(-pi, pi);
65 const double &R = T->getMajorRadius();
66 const double &r = T->getMinorRadius();
68 const double vprime = (R + r * std::cos(v)) / (R + r);
70 const double mu =
rng_.uniformReal(0, 1);
75 acceptedSampleFound =
true;
84 T->setS1(
rng_.uniformReal(Tnear->getS1() - distance, Tnear->getS1() + distance));
85 T->setS2(
rng_.uniformReal(Tnear->getS2() - distance, Tnear->getS2() + distance));
86 space_->enforceBounds(state);
93 T->setS1(
rng_.gaussian(Tmean->getS1(), stdDev));
94 T->setS2(
rng_.gaussian(Tmean->getS2(), stdDev));
96 space_->enforceBounds(state);
99TorusStateSpace::TorusStateSpace(
double majorRadius,
double minorRadius)
100 : majorRadius_(majorRadius), minorRadius_(minorRadius)
102 setName(
"Torus" + getName());
104 addSubspace(std::make_shared<SO2StateSpace>(), 1.0);
105 addSubspace(std::make_shared<SO2StateSpace>(), 1.0);
111 return std::make_shared<TorusStateSampler>(
this);
116 const auto *cstate1 =
static_cast<const CompoundState *
>(state1);
117 const auto *cstate2 =
static_cast<const CompoundState *
>(state2);
118 const double x =
components_[0]->distance(cstate1->components[0], cstate2->components[0]);
119 const double y =
components_[1]->distance(cstate1->components[1], cstate2->components[1]);
120 return std::sqrt(x * x + y * y);
130double TorusStateSpace::getMajorRadius()
const
135double TorusStateSpace::getMinorRadius()
const
140Eigen::Vector3f TorusStateSpace::toVector(
const State *state)
const
145 const float theta = s->getS1();
146 const float phi = s->getS2();
148 const double &R = majorRadius_;
149 const double &r = minorRadius_;
151 v[0] = (R + r * std::cos(phi)) * std::cos(theta);
152 v[1] = (R + r * std::cos(phi)) * std::sin(theta);
153 v[2] = r * std::sin(phi);
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
void allocStateComponents(CompoundState *state) const
Allocate the state components. Called by allocState(). Usually called by derived state spaces.
std::vector< StateSpacePtr > components_
The state spaces that make up the compound state space.
Definition of a compound state.
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.
Representation of a space in which planning can be performed. Topology specific sampling,...
Definition of an abstract state.
const T * as() const
Cast this instance to a desired type.
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.
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...
State * allocState() const override
Allocate a state that can store a point in the described space.
StateSamplerPtr allocDefaultStateSampler() const override
Allocate an instance of the default uniform state sampler for this space.
This namespace contains sampling based planning routines shared by both planning under geometric cons...
@ STATE_SPACE_TORUS
ompl::base::TorusStateSpace