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 {
50 class TorusStateSampler : public StateSampler
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
62 class TorusStateSpace : public CompoundStateSpace
63 {
64 public:
65 class StateType : public CompoundStateSpace::StateType
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
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.
A shared pointer wrapper for ompl::base::StateSampler.
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
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...
Main namespace. Contains everything in this library.