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>
51 static const unsigned int PATH_SECTION_MAX_FIBER_SAMPLING = 10;
57FindSection::FindSection(
PathRestriction *restriction) : restriction_(restriction)
63 OMPL_DEBUG(
"Finding section with non-fibered projection.");
67 FiberedProjectionPtr projection = std::static_pointer_cast<FiberedProjection>(graph->
getProjection());
70 base::StateSpacePtr fiber = projection->getFiberSpace();
71 xFiberStart_ = fiber->allocState();
72 xFiberGoal_ = fiber->allocState();
73 xFiberTmp_ = fiber->allocState();
74 validFiberSpaceSegmentLength_ = fiber->getLongestValidSegmentLength();
78 base::SpaceInformationPtr base = graph->
getBase();
79 xBaseTmp_ = base->allocState();
80 validBaseSpaceSegmentLength_ = base->getStateSpace()->getLongestValidSegmentLength();
82 base::SpaceInformationPtr bundle = graph->
getBundle();
83 xBundleTmp_ = bundle->allocState();
85 validBundleSpaceSegmentLength_ = bundle->getStateSpace()->getLongestValidSegmentLength();
87 neighborhoodRadiusBaseSpaceLambda_ = 1e-4;
89 neighborhoodRadiusBaseSpace_.setLambda(neighborhoodRadiusBaseSpaceLambda_);
90 neighborhoodRadiusBaseSpace_.setValueInit(0.0);
91 neighborhoodRadiusBaseSpace_.setValueTarget(10 * validBaseSpaceSegmentLength_);
94FindSection::~FindSection()
97 FiberedProjectionPtr projection = std::static_pointer_cast<FiberedProjection>(graph->
getProjection());
101 base::StateSpacePtr fiber = projection->getFiberSpace();
102 fiber->freeState(xFiberStart_);
103 fiber->freeState(xFiberGoal_);
104 fiber->freeState(xFiberTmp_);
108 base::SpaceInformationPtr base = graph->
getBase();
109 base->freeState(xBaseTmp_);
111 base::SpaceInformationPtr bundle = graph->
getBundle();
112 bundle->freeState(xBundleTmp_);
117 unsigned int ctr = 0;
122 FiberedProjectionPtr projection = std::static_pointer_cast<FiberedProjection>(graph->
getProjection());
123 base::SpaceInformationPtr bundle = graph->
getBundle();
124 base::SpaceInformationPtr base = graph->
getBundle();
130 while (ctr++ < magic::PATH_SECTION_MAX_FIBER_SAMPLING && !found)
132 samplerFiber->sampleUniform(xFiberTmp_);
134 projection->lift(xBase, xFiberTmp_, xBundle);
137 if (bundle->isValid(xBundle))
145 base->copyState(xBundle, xBase);
153 base::SpaceInformationPtr bundle = graph->
getBundle();
154 base::SpaceInformationPtr base = graph->
getBase();
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();
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();
166 double fiberDist = fiber->distance(xFiberStart_, xFiberGoal_);
167 if (fiberDist < 1e-3)
173 fiber->interpolate(xFiberStart_, xFiberGoal_, 0.5, xFiberTmp_);
185 while (!found && location >= 0)
189 projection->lift(xBase, xFiberTmp_, xBundleStartTmp);
191 if (bundle->isValid(xBundleStartTmp))
193 projection->lift(xBase, xFiberStart_, xBundleStartTmp);
194 projection->lift(xBase, xFiberGoal_, xBundleGoalTmp);
196 if (bundle->isValid(xBundleStartTmp) && bundle->isValid(xBundleGoalTmp))
198 if (bundle->checkMotion(xBundleStartTmp, xBundleGoalTmp))
200 bool feasible =
true;
202 double fiberStepSize = 2 * validFiberSpaceSegmentLength_;
204 if (!bundle->checkMotion(sBundleStart, xBundleStartTmp))
208 double fiberLocation = 0.25 * fiberDist;
211 fiberLocation -= fiberStepSize;
213 fiber->interpolate(xFiberStart_, xFiberGoal_, fiberLocation / fiberDist, xFiberTmp_);
215 projection->lift(xBase, xFiberTmp_, xBundleStartTmp);
217 if (bundle->checkMotion(sBundleStart, xBundleStartTmp) &&
218 bundle->checkMotion(xBundleStartTmp, xBundleGoalTmp))
223 }
while (fiberLocation > -0.25 * fiberDist);
226 if (feasible && !bundle->checkMotion(xBundleGoalTmp, sBundleGoal))
230 double fiberLocation = 0.25 * fiberDist;
233 fiberLocation += fiberStepSize;
235 fiber->interpolate(xFiberStart_, xFiberGoal_, fiberLocation / fiberDist, xFiberTmp_);
237 projection->lift(xBase, xFiberTmp_, xBundleGoalTmp);
239 if (bundle->checkMotion(xBundleGoalTmp, sBundleGoal) &&
240 bundle->checkMotion(xBundleStartTmp, xBundleGoalTmp))
246 }
while (fiberLocation < 1.25 * fiberDist);
275 head->setCurrent(xGoal, locationOnBasePathGoal);
278 bundle->freeState(xBundleStartTmp);
279 bundle->freeState(xBundleGoalTmp);
280 base->freeState(xBase);
A shared pointer wrapper for ompl::base::StateSampler.
Definition of an abstract state.
A configuration in Bundle-space.
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.
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...
#define OMPL_DEBUG(fmt,...)
Log a formatted debugging string.
This namespace contains datastructures and planners to exploit multilevel abstractions,...
Main namespace. Contains everything in this library.