Loading...
Searching...
No Matches
SphereStateSpace.h
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#ifndef OMPL_BASE_SPACES_SPHERE_STATE_SPACE_
40#define OMPL_BASE_SPACES_SPHERE_STATE_SPACE_
41
42#include <ompl/base/StateSpace.h>
43#include <ompl/base/spaces/SO2StateSpace.h>
44#include <ompl/base/spaces/RealVectorStateSpace.h>
45#include <Eigen/Core>
46
47namespace ompl
48{
49 namespace base
50 {
52 class SphereStateSampler : public ompl::base::StateSampler
53 {
54 public:
55 SphereStateSampler(const StateSpace *space);
56
57 void sampleUniform(State *state) override;
58
59 void sampleUniformNear(State *state, const State *near, double distance) override;
60
61 void sampleGaussian(State *state, const State *mean, double stdDev) override;
62 };
63
64 class SphereStateSpace : public CompoundStateSpace
65 {
66 public:
67 class StateType : public CompoundStateSpace::StateType
68 {
69 public:
70 StateType() = default;
71
72 double getTheta() const
73 {
74 return as<SO2StateSpace::StateType>(0)->value;
75 }
76 double getPhi() const
77 {
78 return as<RealVectorStateSpace::StateType>(1)->values[0];
79 }
80
81 void setTheta(double theta)
82 {
83 as<SO2StateSpace::StateType>(0)->value = theta;
84 }
85 void setPhi(double phi)
86 {
87 as<RealVectorStateSpace::StateType>(1)->values[0] = phi;
88 }
89 void setThetaPhi(double theta, double phi)
90 {
91 setTheta(theta);
92 setPhi(phi);
93 }
94 };
95
96 SphereStateSpace(double radius = 1.0);
97
98 virtual ~SphereStateSpace() override = default;
99
100 virtual StateSamplerPtr allocDefaultStateSampler() const override;
101
102 virtual double getMeasure() const override;
103
104 virtual double distance(const State *state1, const State *state2) const override;
105
106 virtual State *allocState() const override;
107
108 /* \brief Convert a state to a 3D vector to visualize the state
109 * space. */
110 Eigen::Vector3f toVector(const State *state) const;
111
112 protected:
113 double radius_{1.0};
114 };
115 }
116}
117
118#endif
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition StateSpace.h:577
CompoundStateSpace()
Construct an empty compound state 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 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.
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
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Main namespace. Contains everything in this library.