PathRestriction.h
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2020,
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_MULTILEVEL_PLANNERS_BUNDLESPACE_PATH_RESTRICTION__
40 #define OMPL_MULTILEVEL_PLANNERS_BUNDLESPACE_PATH_RESTRICTION__
41 #include <ompl/multilevel/datastructures/BundleSpaceGraph.h>
42 
43 namespace ompl
44 {
45  namespace base
46  {
48 
49  OMPL_CLASS_FORWARD(Path);
51  }
52  namespace geometric
53  {
55 
56  OMPL_CLASS_FORWARD(PathGeometric);
58  }
59  namespace multilevel
60  {
62 
63  OMPL_CLASS_FORWARD(BundleSpaceGraph);
65  OMPL_CLASS_FORWARD(PathSection);
67  OMPL_CLASS_FORWARD(Head);
69  OMPL_CLASS_FORWARD(FindSection);
71 
73 
98  class PathRestriction
99  {
100  public:
101  PathRestriction() = delete;
102  PathRestriction(BundleSpaceGraph *);
103 
104  virtual ~PathRestriction();
105 
106  virtual void clear();
107 
109  void setBasePath(base::PathPtr);
110 
112  void setBasePath(std::vector<base::State *>);
113 
115  const std::vector<base::State *> &getBasePath() const;
116 
118  void setFindSectionStrategy(FindSectionType type);
119 
126  bool hasFeasibleSection(Configuration *const, Configuration *const);
127 
129  BundleSpaceGraph *getBundleSpaceGraph();
130 
132  double getLengthBasePath() const;
133 
135  unsigned int size() const;
136 
138  const base::State *getBaseStateAt(int k) const;
139 
141  double getLengthIntermediateBasePath(int k);
142 
144  double getLengthBasePathUntil(int k);
145 
148  int getBasePathLastIndexFromLocation(double d);
149 
152  void interpolateBasePath(double t, base::State *&state) const;
153 
154  friend std::ostream &operator<<(std::ostream &, const PathRestriction &);
155 
156  virtual void print(std::ostream &) const;
157 
158  protected:
161 
163  std::vector<base::State *> basePath_;
164 
166  double lengthBasePath_{0.0};
167 
169  std::vector<double> lengthsIntermediateBasePath_;
170 
172  std::vector<double> lengthsCumulativeBasePath_;
173 
177  FindSectionPtr findSection_;
178  };
179  }
180 }
181 
182 #endif
BundleSpaceGraph * bundleSpaceGraph_
Pointer to associated bundle space.
std::vector< double > lengthsIntermediateBasePath_
Intermediate lengths between states on base path.
std::vector< double > lengthsCumulativeBasePath_
Cumulative lengths between states on base path.
Representation of path restriction (union of fibers over a base path).
Definition of an abstract state.
Definition: State.h:113
void setFindSectionStrategy(FindSectionType type)
Choose algorithm to find sections over restriction.
int getBasePathLastIndexFromLocation(double d)
Given a position d in [0, lengthbasepath_], return the index of the nearest state on base path before...
void interpolateBasePath(double t, base::State *&state) const
Interpolate state on base path at position t in [0, lengthbasepath_] (using discrete state representa...
BundleSpaceGraph * getBundleSpaceGraph()
Return pointer to underlying bundle space graph.
std::vector< base::State * > basePath_
Base path over which we define the restriction.
A graph on a Bundle-space.
double getLengthIntermediateBasePath(int k)
Length between base state indices k and k+1.
void setBasePath(base::PathPtr)
Set base path over which restriction is defined.
double lengthBasePath_
Length of set base path.
double getLengthBasePath() const
Length of base path.
FindSectionPtr findSection_
Strategy to find a feasible section (between specific elements on fiber at first base path index and ...
double getLengthBasePathUntil(int k)
Cumulative length until base state index k.
unsigned int size() const
Return number of discrete states in base path.
const std::vector< base::State * > & getBasePath() const
Return discrete states representation of base path.
bool hasFeasibleSection(Configuration *const, Configuration *const)
Check if feasible section exists between xStart and xGoal.
const base::State * getBaseStateAt(int k) const
Return State at index k on base path.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21