Loading...
Searching...
No Matches
MobiusStateSpace.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/MobiusStateSpace.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
48MobiusStateSpace::MobiusStateSpace(double intervalMax, double radius) : radius_(radius)
49{
50 setName("Mobius" + getName());
51 type_ = STATE_SPACE_MOBIUS;
52
53 StateSpacePtr SO2(std::make_shared<SO2StateSpace>());
54 StateSpacePtr R1(std::make_shared<RealVectorStateSpace>(1));
55 R1->as<RealVectorStateSpace>()->setBounds(-intervalMax, +intervalMax);
56
57 addSubspace(SO2, 1.0);
58 addSubspace(R1, 1.0);
59 lock();
60}
61
62double MobiusStateSpace::distance(const State *state1, const State *state2) const
63{
64 double theta1 = state1->as<MobiusStateSpace::StateType>()->getU();
65 double theta2 = state2->as<MobiusStateSpace::StateType>()->getU();
66
67 double diff = theta2 - theta1;
68
69 if (std::abs(diff) <= pi)
70 {
71 return CompoundStateSpace::distance(state1, state2);
72 }
73 else
74 {
75 // requires interpolation over the gluing strip
76 const auto *cstate1 = static_cast<const CompoundState *>(state1);
77 const auto *cstate2 = static_cast<const CompoundState *>(state2);
78
79 // distance on S1 as usual
80 double dist = 0.0;
81 dist += weights_[0] * components_[0]->distance(cstate1->components[0], cstate2->components[0]);
82
83 double r1 = state1->as<MobiusStateSpace::StateType>()->getV();
84 double r2 = state2->as<MobiusStateSpace::StateType>()->getV();
85
86 r2 = -r2;
87
88 dist += std::sqrt((r2 - r1) * (r2 - r1));
89 return dist;
90 }
91}
92
93void MobiusStateSpace::interpolate(const State *from, const State *to, double t, State *state) const
94{
95 double theta1 = from->as<MobiusStateSpace::StateType>()->getU();
96 double theta2 = to->as<MobiusStateSpace::StateType>()->getU();
97
98 double diff = theta2 - theta1;
99
100 if (std::abs(diff) <= pi)
101 {
102 // interpolate as it would be a cylinder
103 CompoundStateSpace::interpolate(from, to, t, state);
104 }
105 else
106 {
107 // requires interpolation over the gluing strip
108 const auto *cfrom = static_cast<const CompoundState *>(from);
109 const auto *cto = static_cast<const CompoundState *>(to);
110 auto *cstate = static_cast<CompoundState *>(state);
111
112 // interpolate S1 as usual
113 components_[0]->interpolate(cfrom->components[0], cto->components[0], t, cstate->components[0]);
114
115 double r1 = from->as<MobiusStateSpace::StateType>()->getV();
116 double r2 = to->as<MobiusStateSpace::StateType>()->getV();
117
118 // Need to mirror point for interpolation
119 r2 = -r2;
120
121 double r = r1 + (r2 - r1) * t;
122
123 // check again if we need to invert (only if we already crossed gluing
124 // line)
125 double thetaNew = state->as<MobiusStateSpace::StateType>()->getU();
126 double diff2 = theta2 - thetaNew;
127
128 if (std::abs(diff2) <= pi)
129 {
130 r = -r;
131 }
132
133 state->as<MobiusStateSpace::StateType>()->setV(r);
134 }
135}
136
138{
139 auto *state = new StateType();
141 return state;
142}
143
144Eigen::Vector3f MobiusStateSpace::toVector(const State *state) const
145{
146 Eigen::Vector3f vec;
147
148 const auto *s = state->as<MobiusStateSpace::StateType>();
149 float u = s->getU();
150 float v = s->getV();
151
152 double R = radius_ + v * std::cos(0.5 * u);
153 vec[0] = R * std::cos(u);
154 vec[1] = R * std::sin(u);
155 vec[2] = v * std::sin(0.5 * u);
156 return vec;
157}
void allocStateComponents(CompoundState *state) const
Allocate the state components. Called by allocState(). Usually called by derived state spaces.
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...
void interpolate(const State *from, const State *to, double t, State *state) const override
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state....
std::vector< double > weights_
The weight assigned to each component of the state space when computing the compound distance.
Definition StateSpace.h:741
std::vector< StateSpacePtr > components_
The state spaces that make up the compound state space.
Definition StateSpace.h:735
Definition of a compound state.
Definition State.h:87
The definition of a state (u,v) in the Mobius strip state space. The variable u is the position on th...
virtual State * allocState() const override
Allocate a state that can store a point in the described space.
virtual void interpolate(const State *from, const State *to, double t, State *state) const override
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state....
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...
A state space representing Rn. The distance function is the L2 norm.
A shared pointer wrapper for ompl::base::StateSpace.
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_MOBIUS
ompl::base::MobiusStateSpace