Loading...
Searching...
No Matches
PathRestriction.cpp
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#include <ompl/multilevel/datastructures/pathrestriction/PathRestriction.h>
40#include <ompl/multilevel/datastructures/pathrestriction/Head.h>
41#include <ompl/multilevel/datastructures/pathrestriction/FindSection.h>
42#include <ompl/multilevel/datastructures/pathrestriction/FindSectionSideStep.h>
43#include <ompl/multilevel/datastructures/graphsampler/GraphSampler.h>
44#include <ompl/base/objectives/PathLengthOptimizationObjective.h>
45
46#include <ompl/base/Path.h>
47#include <ompl/geometric/PathGeometric.h>
48#include <numeric>
49#include <ompl/util/Time.h>
50
51using namespace ompl::multilevel;
52
53PathRestriction::PathRestriction(BundleSpaceGraph *bundleSpaceGraph) : bundleSpaceGraph_(bundleSpaceGraph)
54{
55 setFindSectionStrategy(FindSectionType::SIDE_STEP);
56}
57
59{
60 switch (type)
61 {
62 case FindSectionType::SIDE_STEP:
63 findSection_ = std::make_shared<FindSectionSideStep>(this);
64 break;
65 case FindSectionType::NONE:
66 findSection_ = nullptr;
67 break;
68 default:
69 OMPL_ERROR("Find section strategy unknown: %s", type);
70 throw ompl::Exception("Unknown Strategy");
71 break;
72 }
73}
74
75PathRestriction::~PathRestriction()
76{
77}
78
79void PathRestriction::clear()
80{
81 basePath_.clear();
85}
86
91
92void PathRestriction::setBasePath(ompl::base::PathPtr path)
93{
94 if (!path)
95 return;
96 auto geometricBasePath = std::static_pointer_cast<geometric::PathGeometric>(path);
97 setBasePath(geometricBasePath->getStates());
98}
99
100void PathRestriction::setBasePath(std::vector<ompl::base::State *> basePath)
101{
102 basePath_.clear();
105 lengthBasePath_ = 0.0;
106
107 basePath_ = basePath;
108
109 for (unsigned int k = 1; k < basePath_.size(); k++)
110 {
111 double lk = bundleSpaceGraph_->getBase()->distance(basePath_.at(k - 1), basePath_.at(k));
112 lengthsIntermediateBasePath_.push_back(lk);
113 lengthBasePath_ += lk;
115 }
116 OMPL_DEBUG("Set new base path with %d states and length %f.", basePath_.size(), lengthBasePath_);
117}
118
120{
121 base::SpaceInformationPtr base = bundleSpaceGraph_->getBase();
122
123 if (t <= 0)
124 {
125 base->copyState(state, basePath_.front());
126 return;
127 }
128 if (t >= lengthBasePath_)
129 {
130 base->copyState(state, basePath_.back());
131 return;
132 }
133
134 unsigned int ctr = 0;
135 while (t > lengthsCumulativeBasePath_.at(ctr) && ctr < lengthsCumulativeBasePath_.size() - 1)
136 {
137 ctr++;
138 }
139
140 base::State *s1 = basePath_.at(ctr);
141 base::State *s2 = basePath_.at(ctr + 1);
142 double d = lengthsIntermediateBasePath_.at(ctr);
143
144 double dCum = (ctr > 0 ? lengthsCumulativeBasePath_.at(ctr - 1) : 0.0);
145 double step = (t - dCum) / d;
146
147 base->getStateSpace()->interpolate(s1, s2, step, state);
148}
149
150const std::vector<ompl::base::State *> &PathRestriction::getBasePath() const
151{
152 return basePath_;
153}
154
156{
157 return lengthBasePath_;
158}
159
160unsigned int PathRestriction::size() const
161{
162 return basePath_.size();
163}
164
166{
167 return basePath_.at(k);
168}
169
170// distance between base states k and k+1
175
177{
178 if (k > (int)size())
179 {
180 OMPL_ERROR("Wrong index k=%d/%d", k, size());
181 throw Exception("WrongIndex");
182 }
183 if (k <= 0)
184 return 0;
185 else
186 {
187 return lengthsCumulativeBasePath_.at(k - 1);
188 }
189}
190
192{
193 if (d > lengthBasePath_)
194 {
195 return size() - 1;
196 }
197 unsigned int ctr = 0;
198 while (d >= lengthsCumulativeBasePath_.at(ctr) && ctr < lengthsCumulativeBasePath_.size() - 1)
199 {
200 ctr++;
201 }
202 return ctr;
203}
204
205bool PathRestriction::hasFeasibleSection(Configuration *const xStart, Configuration *const xGoal)
206{
207 if (findSection_ == nullptr)
208 return false;
209
210 HeadPtr head = std::make_shared<Head>(this, xStart, xGoal);
211
213 bool foundFeasibleSection = findSection_->solve(head);
215
216 OMPL_DEBUG("FindSection terminated after %.2fs (%d/%d vertices/edges).", ompl::time::seconds(t1 - tStart),
217 bundleSpaceGraph_->getNumberOfVertices(), bundleSpaceGraph_->getNumberOfEdges());
218
219 return foundFeasibleSection;
220}
221
222void PathRestriction::print(std::ostream &out) const
223{
224 const base::SpaceInformationPtr bundle = bundleSpaceGraph_->getBundle();
225 const base::SpaceInformationPtr base = bundleSpaceGraph_->getBase();
226
227 out << std::string(80, '-') << std::endl;
228 out << "PATH RESTRICTION" << std::endl;
229 out << std::string(80, '-') << std::endl;
230
231 for (unsigned int k = 0; k < basePath_.size(); k++)
232 {
233 if (k > 5 && (int)k < std::max(0, (int)basePath_.size() - 5))
234 continue;
235 const base::State *bk = basePath_.at(k);
236 base->printState(bk, out);
237 }
238 out << std::string(80, '-') << std::endl;
239}
240
241namespace ompl
242{
243 namespace multilevel
244 {
245 std::ostream &operator<<(std::ostream &out, const PathRestriction &r)
246 {
247 r.print(out);
248 return out;
249 }
250 }
251}
The exception type for ompl.
Definition Exception.h:47
Definition of an abstract state.
Definition State.h:50
A graph on a Bundle-space.
const ompl::base::SpaceInformationPtr & getBundle() const
Get SpaceInformationPtr for Bundle.
const ompl::base::SpaceInformationPtr & getBase() const
Get SpaceInformationPtr for Base.
Representation of path restriction (union of fibers over a base path).
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.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition Console.h:64
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition Console.h:70
This namespace contains sampling based planning routines shared by both planning under geometric cons...
This namespace contains datastructures and planners to exploit multilevel abstractions,...
std::chrono::system_clock::time_point point
Representation of a point in time.
Definition Time.h:52
point now()
Get the current time point.
Definition Time.h:58
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition Time.h:64
Main namespace. Contains everything in this library.