FindSection.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/PathSection.h>
41 #include <ompl/multilevel/datastructures/pathrestriction/Head.h>
42 #include <ompl/multilevel/datastructures/pathrestriction/FindSection.h>
43 #include <ompl/multilevel/datastructures/graphsampler/GraphSampler.h>
44 #include <ompl/multilevel/datastructures/Projection.h>
45 #include <ompl/multilevel/datastructures/projections/FiberedProjection.h>
46 
47 namespace ompl
48 {
49  namespace magic
50  {
51  static const unsigned int PATH_SECTION_MAX_FIBER_SAMPLING = 10;
52  }
53 }
54 
55 using namespace ompl::multilevel;
56 
57 FindSection::FindSection(PathRestriction *restriction) : restriction_(restriction)
58 {
59  BundleSpaceGraph *graph = restriction_->getBundleSpaceGraph();
60 
61  if (!graph->getProjection()->isFibered())
62  {
63  OMPL_DEBUG("Finding section with non-fibered projection.");
64  return;
65  }
66 
67  FiberedProjectionPtr projection = std::static_pointer_cast<FiberedProjection>(graph->getProjection());
68  if (graph->getCoDimension() > 0)
69  {
70  base::StateSpacePtr fiber = projection->getFiberSpace();
71  xFiberStart_ = fiber->allocState();
72  xFiberGoal_ = fiber->allocState();
73  xFiberTmp_ = fiber->allocState();
74  validFiberSpaceSegmentLength_ = fiber->getLongestValidSegmentLength();
75  }
76  if (graph->getBaseDimension() > 0)
77  {
78  base::SpaceInformationPtr base = graph->getBase();
79  xBaseTmp_ = base->allocState();
80  validBaseSpaceSegmentLength_ = base->getStateSpace()->getLongestValidSegmentLength();
81  }
82  base::SpaceInformationPtr bundle = graph->getBundle();
83  xBundleTmp_ = bundle->allocState();
84 
85  validBundleSpaceSegmentLength_ = bundle->getStateSpace()->getLongestValidSegmentLength();
86 
87  neighborhoodRadiusBaseSpaceLambda_ = 1e-4;
88 
89  neighborhoodRadiusBaseSpace_.setLambda(neighborhoodRadiusBaseSpaceLambda_);
90  neighborhoodRadiusBaseSpace_.setValueInit(0.0);
91  neighborhoodRadiusBaseSpace_.setValueTarget(10 * validBaseSpaceSegmentLength_);
92 }
93 
94 FindSection::~FindSection()
95 {
97  FiberedProjectionPtr projection = std::static_pointer_cast<FiberedProjection>(graph->getProjection());
98 
99  if (graph->getCoDimension() > 0)
100  {
101  base::StateSpacePtr fiber = projection->getFiberSpace();
102  fiber->freeState(xFiberStart_);
103  fiber->freeState(xFiberGoal_);
104  fiber->freeState(xFiberTmp_);
105  }
106  if (graph->getBaseDimension() > 0)
107  {
108  base::SpaceInformationPtr base = graph->getBase();
109  base->freeState(xBaseTmp_);
110  }
111  base::SpaceInformationPtr bundle = graph->getBundle();
112  bundle->freeState(xBundleTmp_);
113 }
114 
116 {
117  unsigned int ctr = 0;
118  bool found = false;
119 
121 
122  FiberedProjectionPtr projection = std::static_pointer_cast<FiberedProjection>(graph->getProjection());
123  base::SpaceInformationPtr bundle = graph->getBundle();
124  base::SpaceInformationPtr base = graph->getBundle();
125 
126  const ompl::base::StateSamplerPtr samplerFiber = projection->getFiberSamplerPtr();
127 
128  if (graph->getCoDimension() > 0)
129  {
130  while (ctr++ < magic::PATH_SECTION_MAX_FIBER_SAMPLING && !found)
131  {
132  samplerFiber->sampleUniform(xFiberTmp_);
133 
134  projection->lift(xBase, xFiberTmp_, xBundle);
135 
136  // New sample must be valid AND not reachable from last valid
137  if (bundle->isValid(xBundle))
138  {
139  found = true;
140  }
141  }
142  }
143  else
144  {
145  base->copyState(xBundle, xBase);
146  }
147  return found;
148 }
149 
150 bool FindSection::tripleStep(HeadPtr &head, const ompl::base::State *sBundleGoal, double locationOnBasePathGoal)
151 {
153  base::SpaceInformationPtr bundle = graph->getBundle();
154  base::SpaceInformationPtr base = graph->getBase();
155 
156  base::State *xBundleStartTmp = bundle->allocState();
157  base::State *xBundleGoalTmp = bundle->allocState();
158  base::State *xBase = base->cloneState(head->getStateBase());
159  const base::State *sBundleStart = head->getState();
160 
161  FiberedProjectionPtr projection = std::static_pointer_cast<FiberedProjection>(graph->getProjection());
162  projection->projectFiber(sBundleStart, xFiberStart_);
163  projection->projectFiber(sBundleGoal, xFiberGoal_);
164  base::StateSpacePtr fiber = projection->getFiberSpace();
165 
166  double fiberDist = fiber->distance(xFiberStart_, xFiberGoal_);
167  if (fiberDist < 1e-3)
168  return false;
169 
170  bool found = false;
171 
172  // mid point heuristic
173  fiber->interpolate(xFiberStart_, xFiberGoal_, 0.5, xFiberTmp_);
174 
175  double location = head->getLocationOnBasePath() - validBaseSpaceSegmentLength_;
176 
177  // Triple step connection attempt
178  // xBundleStartTmp <------- xBundleStart
179  // |
180  // |
181  // |
182  // v
183  // xBundleGoalTmp -------> xBundleGoal
184 
185  while (!found && location >= 0)
186  {
187  restriction_->interpolateBasePath(location, xBase);
188 
189  projection->lift(xBase, xFiberTmp_, xBundleStartTmp);
190 
191  if (bundle->isValid(xBundleStartTmp))
192  {
193  projection->lift(xBase, xFiberStart_, xBundleStartTmp);
194  projection->lift(xBase, xFiberGoal_, xBundleGoalTmp);
195 
196  if (bundle->isValid(xBundleStartTmp) && bundle->isValid(xBundleGoalTmp))
197  {
198  if (bundle->checkMotion(xBundleStartTmp, xBundleGoalTmp))
199  {
200  bool feasible = true;
201 
202  double fiberStepSize = 2 * validFiberSpaceSegmentLength_;
203 
204  if (!bundle->checkMotion(sBundleStart, xBundleStartTmp))
205  {
206  feasible = false;
207 
208  double fiberLocation = 0.25 * fiberDist;
209  do
210  {
211  fiberLocation -= fiberStepSize;
212 
213  fiber->interpolate(xFiberStart_, xFiberGoal_, fiberLocation / fiberDist, xFiberTmp_);
214 
215  projection->lift(xBase, xFiberTmp_, xBundleStartTmp);
216 
217  if (bundle->checkMotion(sBundleStart, xBundleStartTmp) &&
218  bundle->checkMotion(xBundleStartTmp, xBundleGoalTmp))
219  {
220  feasible = true;
221  break;
222  }
223  } while (fiberLocation > -0.25 * fiberDist);
224  // try to repair
225  }
226  if (feasible && !bundle->checkMotion(xBundleGoalTmp, sBundleGoal))
227  {
228  feasible = false;
229 
230  double fiberLocation = 0.25 * fiberDist;
231  do
232  {
233  fiberLocation += fiberStepSize;
234 
235  fiber->interpolate(xFiberStart_, xFiberGoal_, fiberLocation / fiberDist, xFiberTmp_);
236 
237  projection->lift(xBase, xFiberTmp_, xBundleGoalTmp);
238 
239  if (bundle->checkMotion(xBundleGoalTmp, sBundleGoal) &&
240  bundle->checkMotion(xBundleStartTmp, xBundleGoalTmp))
241  {
242  feasible = true;
243  break;
244  }
245 
246  } while (fiberLocation < 1.25 * fiberDist);
247  }
248  if (feasible)
249  {
250  found = true;
251  }
252  break;
253  }
254  }
255  }
256 
257  location -= validBaseSpaceSegmentLength_;
258  }
259 
260  if (found)
261  {
262  Configuration *xBackStep = new Configuration(bundle, xBundleStartTmp);
263  graph->addConfiguration(xBackStep);
264  graph->addBundleEdge(head->getConfiguration(), xBackStep);
265 
266  Configuration *xSideStep = new Configuration(bundle, xBundleGoalTmp);
267  graph->addConfiguration(xSideStep);
268  graph->addBundleEdge(xBackStep, xSideStep);
269 
270  // xBaseTmp_ is on last valid fiber.
271  Configuration *xGoal = new Configuration(bundle, sBundleGoal);
272  graph->addConfiguration(xGoal);
273  graph->addBundleEdge(xSideStep, xGoal);
274 
275  head->setCurrent(xGoal, locationOnBasePathGoal);
276  }
277 
278  bundle->freeState(xBundleStartTmp);
279  bundle->freeState(xBundleGoalTmp);
280  base->freeState(xBase);
281  return found;
282 }
double validBaseSpaceSegmentLength_
Step size to check validity.
Definition: FindSection.h:146
Representation of path restriction (union of fibers over a base path).
PathRestriction * restriction_
Pointer to associated bundle space.
Definition: FindSection.h:128
Definition of an abstract state.
Definition: State.h:113
This namespace contains datastructures and planners to exploit multilevel abstractions,...
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.
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.
ProjectionPtr getProjection() const
Get ProjectionPtr from Bundle to Base.
bool findFeasibleStateOnFiber(const base::State *xBase, base::State *xBundle)
Sample state on fiber while keeping base state fixed.
unsigned int getCoDimension() const
Dimension of Bundle Space - Dimension of Base Space.
unsigned int getBaseDimension() const
Dimension of Base Space.
virtual Vertex addConfiguration(Configuration *q)
Add configuration to graph. Return its vertex in boost graph.
virtual void addBundleEdge(const Configuration *a, const Configuration *b)
Add edge between configuration a and configuration b to graph.
A shared pointer wrapper for ompl::base::StateSampler.
bool tripleStep(HeadPtr &head, const base::State *sBundleGoal, double locationOnBasePathGoal)
Triple step pattern.
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
Definition: Console.h:70
Main namespace. Contains everything in this library.
Definition: AppBase.h:21