DubinsStateSpace.h
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Mark Moll */
36 
37 #ifndef OMPL_BASE_SPACES_DUBINS_STATE_SPACE_
38 #define OMPL_BASE_SPACES_DUBINS_STATE_SPACE_
39 
40 #include "ompl/base/spaces/SE2StateSpace.h"
41 #include "ompl/base/MotionValidator.h"
42 #include <boost/math/constants/constants.hpp>
43 
44 namespace ompl
45 {
46  namespace base
47  {
48 
66  {
67  public:
68 
70  enum DubinsPathSegmentType { DUBINS_LEFT=0, DUBINS_STRAIGHT=1, DUBINS_RIGHT=2 };
74  class DubinsPath
75  {
76  public:
77  DubinsPath(const DubinsPathSegmentType* type = dubinsPathType[0],
78  double t=0., double p=std::numeric_limits<double>::max(), double q=0.)
79  : type_(type), reverse_(false)
80  {
81  length_[0] = t;
82  length_[1] = p;
83  length_[2] = q;
84  assert(t >= 0.);
85  assert(p >= 0.);
86  assert(q >= 0.);
87  }
88  double length() const
89  {
90  return length_[0] + length_[1] + length_[2];
91  }
92 
96  double length_[3];
98  bool reverse_;
99  };
100 
101  DubinsStateSpace(double turningRadius = 1.0, bool isSymmetric = false)
102  : SE2StateSpace(), rho_(turningRadius), isSymmetric_(isSymmetric)
103  {
104  }
105 
106  virtual bool isMetricSpace() const
107  {
108  return false;
109  }
110 
111  virtual double distance(const State *state1, const State *state2) const;
112 
113  virtual void interpolate(const State *from, const State *to, const double t,
114  State *state) const;
115  virtual void interpolate(const State *from, const State *to, const double t,
116  bool &firstTime, DubinsPath &path, State *state) const;
117 
118  virtual bool hasSymmetricDistance() const
119  {
120  return isSymmetric_;
121  }
122 
123  virtual bool hasSymmetricInterpolate() const
124  {
125  return isSymmetric_;
126  }
127 
128  virtual void sanityChecks() const
129  {
130  double zero = std::numeric_limits<double>::epsilon();
131  double eps = std::numeric_limits<float>::epsilon();
133  if (!isSymmetric_)
135  StateSpace::sanityChecks(zero, eps, flags);
136  }
137 
139  DubinsPath dubins(const State *state1, const State *state2) const;
140 
141  protected:
142  virtual void interpolate(const State *from, const DubinsPath &path, const double t,
143  State *state) const;
144 
146  double rho_;
147 
156  };
157 
165  {
166  public:
168  {
169  defaultSettings();
170  }
172  {
173  defaultSettings();
174  }
175  virtual ~DubinsMotionValidator()
176  {
177  }
178  virtual bool checkMotion(const State *s1, const State *s2) const;
179  virtual bool checkMotion(const State *s1, const State *s2, std::pair<State*, double> &lastValid) const;
180  private:
181  DubinsStateSpace *stateSpace_;
182  void defaultSettings();
183  };
184 
185  }
186 }
187 
188 #endif
virtual double distance(const State *state1, const State *state2) const
Computes distance between two states. This function satisfies the properties of a metric if isMetricS...
virtual bool isMetricSpace() const
Return true if the distance function associated with the space is a metric.
Check whether the StateSpace::distance() is bounded by StateSpace::getExtent()
Definition: StateSpace.h:147
An SE(2) state space where distance is measured by the length of Dubins curves.
virtual void interpolate(const State *from, const State *to, const double t, State *state) const
Computes the state that lies at time t in [0, 1] on the segment that connects from state to to state...
DubinsPathSegmentType
The Dubins path segment type.
const DubinsPathSegmentType * type_
Abstract definition for a class checking the validity of motions – path segments between states...
virtual bool hasSymmetricDistance() const
Check if the distance function on this state space is symmetric, i.e. distance(s1,s2) = distance(s2,s1). Default implementation returns true.
Main namespace. Contains everything in this library.
Definition: Cost.h:42
virtual void sanityChecks() const
Convenience function that allows derived state spaces to choose which checks should pass (see SanityC...
A state space representing SE(2)
Definition: SE2StateSpace.h:50
bool isSymmetric_
Whether the distance is "symmetrized".
A boost shared pointer wrapper for ompl::base::SpaceInformation.
The base class for space information. This contains all the information about the space planning is d...
Definition of an abstract state.
Definition: State.h:50
Check whether the triangle inequality holds when using StateSpace::interpolate() and StateSpace::dist...
Definition: StateSpace.h:144
virtual void sanityChecks() const
Convenience function that allows derived state spaces to choose which checks should pass (see SanityC...
Definition: StateSpace.cpp:591
A Dubins motion validator that only uses the state validity checker. Motions are checked for validity...
DubinsPath dubins(const State *state1, const State *state2) const
Return the shortest Dubins path from SE(2) state state1 to SE(2) state state2.
Check whether the distance function is symmetric (StateSpace::distance())
Definition: StateSpace.h:138
Check whether calling StateSpace::interpolate() works as expected.
Definition: StateSpace.h:141
static const DubinsPathSegmentType dubinsPathType[6][3]
Dubins path types.
virtual bool hasSymmetricInterpolate() const
Check if the interpolation function on this state space is symmetric, i.e. interpolate(from, to, t, state) = interpolate(to, from, 1-t, state). Default implementation returns true.
Complete description of a Dubins path.
double rho_
Turning radius.