Loading...
Searching...
No Matches
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
43namespace 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
72 using Configuration = ompl::multilevel::BundleSpaceGraph::Configuration;
73
97
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
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
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
Abstract definition of a path.
Definition Path.h:68
Definition of an abstract state.
Definition State.h:50
Definition of a geometric path.
A graph on a Bundle-space.
A pointer to a specific location on the base path of the path restriction.
Definition Head.h:64
BundleSpaceGraph * bundleSpaceGraph_
Pointer to associated bundle space.
double getLengthBasePath() const
Length of base path.
const base::State * getBaseStateAt(int k) const
Return State at index k on base path.
double lengthBasePath_
Length of set base path.
std::vector< double > lengthsIntermediateBasePath_
Intermediate lengths between states on base path.
void setBasePath(base::PathPtr)
Set base path over which restriction is defined.
BundleSpaceGraph * getBundleSpaceGraph()
Return pointer to underlying bundle space graph.
int getBasePathLastIndexFromLocation(double d)
Given a position d in [0, lengthbasepath_], return the index of the nearest state on base path before...
std::vector< base::State * > basePath_
Base path over which we define the restriction.
double getLengthIntermediateBasePath(int k)
Length between base state indices k and k+1.
FindSectionPtr findSection_
Strategy to find a feasible section (between specific elements on fiber at first base path index and ...
void interpolateBasePath(double t, base::State *&state) const
Interpolate state on base path at position t in [0, lengthbasepath_] (using discrete state representa...
double getLengthBasePathUntil(int k)
Cumulative length until base state index k.
bool hasFeasibleSection(Configuration *const, Configuration *const)
Check if feasible section exists between xStart and xGoal.
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.
std::vector< double > lengthsCumulativeBasePath_
Cumulative lengths between states on base path.
void setFindSectionStrategy(FindSectionType type)
Choose algorithm to find sections over restriction.
Representation of a path section (not necessarily feasible).
Definition PathSection.h:61
This namespace contains sampling based planning routines shared by both planning under geometric cons...
This namespace contains code that is specific to planning under geometric constraints.
This namespace contains datastructures and planners to exploit multilevel abstractions,...
Main namespace. Contains everything in this library.