Loading...
Searching...
No Matches
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
46namespace 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
55using namespace ompl::multilevel;
56
57FindSectionSideStep::FindSectionSideStep(PathRestriction *restriction) : BaseT(restriction)
58{
59}
60
61FindSectionSideStep::~FindSectionSideStep()
62{
63}
64
65bool 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
86bool FindSectionSideStep::recursiveSideStep(HeadPtr &head, bool interpolateFiberFirst, unsigned int depth)
87{
88 BundleSpaceGraph *graph = restriction_->getBundleSpaceGraph();
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}
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.
ProjectionPtr getProjection() const
Get ProjectionPtr from Bundle to Base.
BundleSpace * getChild() const
Return k-1 th bundle space (locally the base space)
const ompl::base::SpaceInformationPtr & getBundle() const
Get SpaceInformationPtr for Bundle.
const ompl::base::SpaceInformationPtr & getBase() const
Get SpaceInformationPtr for Base.
bool findFeasibleStateOnFiber(const base::State *xBase, base::State *xBundle)
Sample state on fiber while keeping base state fixed.
PathRestriction * restriction_
Pointer to associated bundle space.
Definition FindSection.h:94
Representation of path restriction (union of fibers over a base path).
This namespace includes magic constants used in various places in OMPL.
Definition Constraint.h:52
This namespace contains datastructures and planners to exploit multilevel abstractions,...
Main namespace. Contains everything in this library.