FindSectionSideStep.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/FindSectionSideStep.h>
43 #include <ompl/multilevel/datastructures/graphsampler/GraphSampler.h>
44 #include <ompl/multilevel/datastructures/projections/FiberedProjection.h>
45 
46 namespace ompl
47 {
48  namespace magic
49  {
50  static const unsigned int PATH_SECTION_TREE_MAX_DEPTH = 3;
51  static const unsigned int PATH_SECTION_TREE_MAX_BRANCHING = 10;
52  }
53 }
54 
55 using namespace ompl::multilevel;
56 
57 FindSectionSideStep::FindSectionSideStep(PathRestriction *restriction) : BaseT(restriction)
58 {
59 }
60 
61 FindSectionSideStep::~FindSectionSideStep()
62 {
63 }
64 
65 bool FindSectionSideStep::solve(HeadPtr &head)
66 {
67  Configuration *q = head->getConfiguration();
68 
69  HeadPtr head2(head);
70 
71  bool foundFeasibleSection = recursiveSideStep(head);
72 
73  if (!foundFeasibleSection)
74  {
75  head->setCurrent(q, 0);
76  foundFeasibleSection = recursiveSideStep(head, false);
77  }
78 
79  std::stringstream buffer;
80  buffer << *head;
81  OMPL_DEVMSG1("Stopped section method at %s.", buffer.str().c_str());
82 
83  return foundFeasibleSection;
84 }
85 
86 bool FindSectionSideStep::recursiveSideStep(HeadPtr &head, bool interpolateFiberFirst, unsigned int depth)
87 {
89  FiberedProjectionPtr projection = std::static_pointer_cast<FiberedProjection>(graph->getProjection());
90  base::SpaceInformationPtr bundle = graph->getBundle();
91  base::SpaceInformationPtr base = graph->getBase();
92 
93  PathSectionPtr section = std::make_shared<PathSection>(restriction_);
94 
95  if (interpolateFiberFirst)
96  {
97  section->interpolateL1FiberFirst(head);
98  }
99  else
100  {
101  section->interpolateL1FiberLast(head);
102  }
103 
104  if (section->checkMotion(head))
105  {
106  return true;
107  }
108 
109  static_cast<BundleSpaceGraph *>(graph->getChild())
110  ->getGraphSampler()
111  ->setPathBiasStartSegment(head->getLocationOnBasePath());
112 
113  //############################################################################
114  // Get last valid state information
115  //############################################################################
116 
117  if (depth + 1 >= magic::PATH_SECTION_TREE_MAX_DEPTH)
118  {
119  return false;
120  }
121 
122  double location = head->getLocationOnBasePath();
123 
124  base::State *xBase = base->allocState();
125 
126  restriction_->interpolateBasePath(location, xBase);
127 
128  bool found = false;
129 
130  for (unsigned int j = 0; j < magic::PATH_SECTION_TREE_MAX_BRANCHING; j++)
131  {
132  if (!findFeasibleStateOnFiber(xBase, xBundleTmp_))
133  {
134  continue;
135  }
136 
137  if (bundle->checkMotion(head->getState(), xBundleTmp_))
138  {
139  Configuration *xSideStep = new Configuration(bundle, xBundleTmp_);
140  graph->addConfiguration(xSideStep);
141  graph->addBundleEdge(head->getConfiguration(), xSideStep);
142 
143  HeadPtr newHead(head);
144 
145  newHead->setCurrent(xSideStep, location);
146 
147  bool feasibleSection = recursiveSideStep(newHead, !interpolateFiberFirst, depth + 1);
148 
149  if (feasibleSection)
150  {
151  head = newHead;
152  found = true;
153  break;
154  }
155  }
156  }
157  base->freeState(xBase);
158  return found;
159 }
Representation of path restriction (union of fibers over a base path).
PathRestriction * restriction_
Pointer to associated bundle space.
Definition: FindSection.h:128
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.
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.
BundleSpace * getChild() const
Return k-1 th bundle space (locally the base space)
Main namespace. Contains everything in this library.