Loading...
Searching...
No Matches
TorusStateSpace.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_TORUS_STATE_SPACE_
40#define OMPL_BASE_SPACES_TORUS_STATE_SPACE_
41
42#include <ompl/base/StateSpace.h>
43#include <ompl/base/spaces/SO2StateSpace.h>
44
45namespace ompl
46{
47 namespace base
48 {
51 {
52 public:
53 TorusStateSampler(const StateSpace *space);
54
55 void sampleUniform(State *state) override;
56
57 void sampleUniformNear(State *state, const State *near, double distance) override;
58
59 void sampleGaussian(State *state, const State *mean, double stdDev) override;
60 };
61
63 {
64 public:
66 {
67 public:
68 StateType() = default;
69
70 double getS1() const
71 {
72 return as<SO2StateSpace::StateType>(0)->value;
73 }
74 double getS2() const
75 {
76 return as<SO2StateSpace::StateType>(1)->value;
77 }
78
79 void setS1(double s)
80 {
81 as<SO2StateSpace::StateType>(0)->value = s;
82 }
83 void setS2(double s)
84 {
85 as<SO2StateSpace::StateType>(1)->value = s;
86 }
87 void setS1S2(double s, double t)
88 {
89 setS1(s);
90 setS2(t);
91 }
92 };
93
94 TorusStateSpace(double majorRadius = 1, double minorRadius = 0.5);
95
96 virtual ~TorusStateSpace() override = default;
97
99
100 double distance(const State *state1, const State *state2) const override;
101
102 State *allocState() const override;
103
104 double getMajorRadius() const;
105
106 double getMinorRadius() const;
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 private:
113 double majorRadius_;
114
115 double minorRadius_;
116 };
117 }
118}
119
120#endif
A space to allow the composition of state spaces.
Definition StateSpace.h:574
Definition of a compound state.
Definition State.h:87
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
State sampler for the torus 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.
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.
Main namespace. Contains everything in this library.