Loading...
Searching...
No Matches
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
47namespace ompl
48{
49 namespace magic
50 {
51 static const unsigned int PATH_SECTION_MAX_FIBER_SAMPLING = 10;
52 }
53}
54
55using namespace ompl::multilevel;
56
57FindSection::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
94FindSection::~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
150bool 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
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}
A shared pointer wrapper for ompl::base::StateSampler.
Definition of an abstract state.
Definition State.h:50
A graph on a Bundle-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.
ProjectionPtr getProjection() const
Get ProjectionPtr from Bundle to Base.
unsigned int getBaseDimension() const
Dimension of Base Space.
unsigned int getCoDimension() const
Dimension of Bundle Space - Dimension of Base Space.
const ompl::base::SpaceInformationPtr & getBundle() const
Get SpaceInformationPtr for Bundle.
const ompl::base::SpaceInformationPtr & getBase() const
Get SpaceInformationPtr for Base.
bool tripleStep(HeadPtr &head, const base::State *sBundleGoal, double locationOnBasePathGoal)
Triple step pattern.
bool findFeasibleStateOnFiber(const base::State *xBase, base::State *xBundle)
Sample state on fiber while keeping base state fixed.
double validBaseSpaceSegmentLength_
Step size to check validity.
PathRestriction * restriction_
Pointer to associated bundle space.
Definition FindSection.h:94
Representation of path restriction (union of fibers over a base path).
BundleSpaceGraph * getBundleSpaceGraph()
Return pointer to underlying bundle space graph.
void interpolateBasePath(double t, base::State *&state) const
Interpolate state on base path at position t in [0, lengthbasepath_] (using discrete state representa...
This namespace contains datastructures and planners to exploit multilevel abstractions,...
Main namespace. Contains everything in this library.