PathSection.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_SECTION__
40 #define OMPL_MULTILEVEL_PLANNERS_BUNDLESPACE_PATH_SECTION__
41 #include <ompl/multilevel/datastructures/BundleSpaceGraph.h>
42 #include <ompl/util/ClassForward.h>
43 
44 namespace ompl
45 {
46  namespace multilevel
47  {
49 
50  OMPL_CLASS_FORWARD(PathRestriction);
52  OMPL_CLASS_FORWARD(Head);
54 
60  class PathSection
61  {
62  public:
64 
65  PathSection() = delete;
66  PathSection(PathRestriction *);
67  virtual ~PathSection();
68 
76  void interpolateL2(HeadPtr &);
77 
86  void interpolateL1FiberFirst(HeadPtr &);
87 
95  void interpolateL1FiberLast(HeadPtr &);
96 
102  bool checkMotion(HeadPtr &);
103 
105  void sanityCheck();
106  void sanityCheck(HeadPtr &);
107 
109  base::State *at(int k) const;
110  const base::State *back() const;
111  const base::State *front() const;
112  unsigned int size() const;
113 
116  Configuration *addFeasibleSegment(Configuration *xLast, base::State *sNext);
117 
118  void addFeasibleGoalSegment(Configuration *xLast, Configuration *xGoal);
119 
120  friend std::ostream &operator<<(std::ostream &, const PathSection &);
121 
122  void print(std::ostream &) const;
123 
124  protected:
125  PathRestriction *restriction_;
126 
128  std::vector<base::State *> section_;
129 
130  std::vector<int> sectionBaseStateIndices_;
131 
133  std::pair<base::State *, double> lastValid_;
134 
135  int lastValidIndexOnBasePath_;
136 
137  base::State *xBaseTmp_{nullptr};
138  base::State *xBundleTmp_{nullptr};
139 
140  base::State *xFiberStart_{nullptr};
141  base::State *xFiberGoal_{nullptr};
142  base::State *xFiberTmp_{nullptr};
143  };
144  }
145 }
146 #endif
base::State * at(int k) const
Methods to access sections like std::vector.
void interpolateL1FiberLast(HeadPtr &)
Definition of an abstract state.
Definition: State.h:113
bool checkMotion(HeadPtr &)
Checks if section is feasible.
Definition: PathSection.cpp:91
Configuration * addFeasibleSegment(Configuration *xLast, base::State *sNext)
Add vertex for sNext and edge to xLast by assuming motion is valid
std::pair< base::State *, double > lastValid_
Last valid state on feasible segment.
Definition: PathSection.h:235
void interpolateL1FiberFirst(HeadPtr &)
Interpolate along restriction using L1 metric.
void sanityCheck()
checks if section is feasible
std::vector< base::State * > section_
Interpolated section along restriction.
Definition: PathSection.h:230
Main namespace. Contains everything in this library.
Definition: AppBase.h:21