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 
51 using namespace ompl::multilevel;
52 
53 PathRestriction::PathRestriction(BundleSpaceGraph *bundleSpaceGraph) : bundleSpaceGraph_(bundleSpaceGraph)
54 {
55  setFindSectionStrategy(FindSectionType::SIDE_STEP);
56 }
57 
58 void PathRestriction::setFindSectionStrategy(FindSectionType type)
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 
75 PathRestriction::~PathRestriction()
76 {
77 }
78 
79 void PathRestriction::clear()
80 {
81  basePath_.clear();
84  lengthBasePath_ = 0;
85 }
86 
88 {
89  return bundleSpaceGraph_;
90 }
91 
93 {
94  if (!path)
95  return;
96  auto geometricBasePath = std::static_pointer_cast<geometric::PathGeometric>(path);
97  setBasePath(geometricBasePath->getStates());
98 }
99 
100 void 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 
150 const std::vector<ompl::base::State *> &PathRestriction::getBasePath() const
151 {
152  return basePath_;
153 }
154 
156 {
157  return lengthBasePath_;
158 }
159 
160 unsigned 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
172 {
173  return lengthsIntermediateBasePath_.at(k);
174 }
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 
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 
222 void 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 
241 namespace 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 }
BundleSpaceGraph * bundleSpaceGraph_
Pointer to associated bundle space.
std::vector< double > lengthsIntermediateBasePath_
Intermediate lengths between states on base path.
A shared pointer wrapper for ompl::base::Path.
std::vector< double > lengthsCumulativeBasePath_
Cumulative lengths between states on base path.
std::chrono::system_clock::time_point point
Representation of a point in time.
Definition: Time.h:116
Representation of path restriction (union of fibers over a base path).
Definition of an abstract state.
Definition: State.h:113
This namespace contains datastructures and planners to exploit multilevel abstractions,...
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...
point now()
Get the current time point.
Definition: Time.h:122
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.
const ompl::base::SpaceInformationPtr & getBundle() const
Get SpaceInformationPtr for Bundle.
const ompl::base::SpaceInformationPtr & getBase() const
Get SpaceInformationPtr for Base.
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.
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
Definition: Console.h:64
const std::vector< base::State * > & getBasePath() const
Return discrete states representation of base path.
The exception type for ompl.
Definition: Exception.h:78
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.
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
duration seconds(double sec)
Return the time duration representing a given number of seconds.
Definition: Time.h:128
Main namespace. Contains everything in this library.