39#include <ompl/multilevel/datastructures/pathrestriction/PathSection.h>
40#include <ompl/multilevel/datastructures/pathrestriction/PathRestriction.h>
41#include <ompl/multilevel/datastructures/pathrestriction/Head.h>
42#include <ompl/multilevel/datastructures/projections/FiberedProjection.h>
46PathSection::PathSection(
PathRestriction *restriction) : restriction_(restriction)
49 FiberedProjectionPtr projection = std::static_pointer_cast<FiberedProjection>(graph->
getProjection());
52 base::StateSpacePtr fiber = projection->getFiberSpace();
53 xFiberStart_ = fiber->allocState();
54 xFiberGoal_ = fiber->allocState();
55 xFiberTmp_ = fiber->allocState();
59 base::SpaceInformationPtr base = graph->
getBase();
60 xBaseTmp_ = base->allocState();
62 base::SpaceInformationPtr bundle = graph->
getBundle();
63 xBundleTmp_ = bundle->allocState();
64 lastValid_.first = bundle->allocState();
67PathSection::~PathSection()
70 base::SpaceInformationPtr bundle = graph->
getBundle();
74 FiberedProjectionPtr projection = std::static_pointer_cast<FiberedProjection>(graph->
getProjection());
75 base::StateSpacePtr fiber = projection->getFiberSpace();
76 fiber->freeState(xFiberStart_);
77 fiber->freeState(xFiberGoal_);
78 fiber->freeState(xFiberTmp_);
82 base::SpaceInformationPtr base = graph->
getBase();
83 base->freeState(xBaseTmp_);
88 bundle->freeState(xBundleTmp_);
95 base::SpaceInformationPtr bundle = graph->
getBundle();
96 base::SpaceInformationPtr base = graph->
getBase();
98 for (
unsigned int k = 1; k <
section_.size(); k++)
108 head->setCurrent(xLast, locationOnBasePath);
112 addFeasibleGoalSegment(head->getConfiguration(), head->getTargetConfiguration());
118 lastValidIndexOnBasePath_ = sectionBaseStateIndices_.at(k - 1);
124 double distBaseSegment = base->distance(lastValidBaseState, xBaseTmp_);
126 double locationOnBasePath =
137 graph->
addBundleEdge(head->getConfiguration(), xBundleLastValid);
139 head->setCurrent(xBundleLastValid, locationOnBasePath);
165 sectionBaseStateIndices_.clear();
168 base::SpaceInformationPtr base = graph->
getBase();
169 base::SpaceInformationPtr bundle = graph->
getBundle();
171 int size = head->getNumberOfRemainingStates() + 1;
173 FiberedProjectionPtr projection = std::static_pointer_cast<FiberedProjection>(graph->
getProjection());
177 const base::State *xFiberStart = head->getStateFiber();
178 const base::State *xFiberGoal = head->getStateTargetFiber();
184 projection->lift(head->getBaseStateAt(0), xFiberStart,
section_.front());
186 sectionBaseStateIndices_.push_back(head->getBaseStateIndexAt(0));
188 for (
unsigned int k = 1; k <
section_.size(); k++)
190 projection->lift(head->getBaseStateAt(k - 1), xFiberGoal,
section_.at(k));
191 sectionBaseStateIndices_.push_back(head->getBaseStateIndexAt(k - 1));
200 for (
int k = 0; k < size; k++)
202 bundle->copyState(
section_.at(k), head->getBaseStateAt(k));
203 sectionBaseStateIndices_.push_back(head->getBaseStateIndexAt(k));
211 sectionBaseStateIndices_.clear();
214 const base::SpaceInformationPtr bundle = graph->
getBundle();
215 const base::SpaceInformationPtr base = graph->
getBase();
217 int size = head->getNumberOfRemainingStates() + 1;
219 FiberedProjectionPtr projection = std::static_pointer_cast<FiberedProjection>(graph->
getProjection());
222 const base::State *xFiberStart = head->getStateFiber();
223 const base::State *xFiberGoal = head->getStateTargetFiber();
229 for (
int k = 0; k < size; k++)
231 projection->lift(head->getBaseStateAt(k), xFiberStart,
section_.at(k));
232 sectionBaseStateIndices_.push_back(head->getBaseStateIndexAt(k));
234 projection->lift(head->getBaseStateAt(size - 1), xFiberGoal,
section_.back());
235 sectionBaseStateIndices_.push_back(head->getBaseStateIndexAt(size - 1));
241 for (
int k = 0; k < size; k++)
243 bundle->copyState(
section_.at(k), head->getBaseStateAt(k));
244 sectionBaseStateIndices_.push_back(head->getBaseStateIndexAt(k));
253 sectionBaseStateIndices_.clear();
256 base::SpaceInformationPtr bundle = graph->
getBundle();
257 const std::vector<base::State *> basePath = restriction_->
getBasePath();
259 int size = head->getNumberOfRemainingStates() + 1;
266 const base::State *xFiberStart = head->getStateFiber();
267 const base::State *xFiberGoal = head->getStateTargetFiber();
271 FiberedProjectionPtr projection = std::static_pointer_cast<FiberedProjection>(graph->
getProjection());
272 base::StateSpacePtr fiber = projection->getFiberSpace();
274 for (
unsigned int k = 0; k < restriction_->
size(); k++)
277 double step = lengthCurrent / totalLengthBasePath;
279 fiber->interpolate(xFiberStart, xFiberGoal, step, xFiberTmp_);
283 sectionBaseStateIndices_.push_back(k);
288 for (
unsigned int k = 0; k < basePath.size(); k++)
290 bundle->copyState(
section_.at(k), basePath.at(k));
291 sectionBaseStateIndices_.push_back(k);
300 base::SpaceInformationPtr bundle = graph->
getBundle();
313 if (xGoal->
index < 0)
328 base::State *xg = head->getTargetConfiguration()->state;
331 base::SpaceInformationPtr bundle = graph->
getBundle();
332 base::SpaceInformationPtr base = graph->
getBase();
334 double d1 = bundle->distance(
section_.front(), xi);
335 double d2 = bundle->distance(
section_.back(), xg);
337 if (d1 > 1e-5 || d2 > 1e-5)
339 std::stringstream buffer;
340 buffer <<
"START STATE" << std::endl;
341 bundle->printState(xi, buffer);
342 bundle->printState(
section_.front(), buffer);
343 buffer <<
"Distance: " << d1 << std::endl;
344 buffer <<
"GOAL STATE" << std::endl;
345 bundle->printState(xg, buffer);
346 buffer <<
"GOAL STATE (SECTION)" << std::endl;
347 bundle->printState(
section_.back(), buffer);
348 buffer <<
"Dist:" << d2 << std::endl;
349 int size = head->getNumberOfRemainingStates();
350 buffer <<
"Section size: " <<
section_.size() << std::endl;
351 buffer <<
"Remaining states: " << size << std::endl;
352 buffer <<
"Restriction size:" << restriction_->
size() << std::endl;
353 buffer <<
"Base states:" << std::endl;
354 buffer << *restriction_ << std::endl;
355 OMPL_ERROR(
"Invalid Section: %s", buffer.str().c_str());
364 base::SpaceInformationPtr bundle = graph->
getBundle();
365 bool feasible =
true;
366 for (
unsigned int k = 1; k <
section_.size(); k++)
370 if (!bundle->checkMotion(sk1, sk2))
373 OMPL_ERROR(
"Error between states %d and %d.", k - 1, k);
374 bundle->printState(sk1);
375 bundle->printState(sk2);
381 throw Exception(
"Reported feasible path section, \
382 but path section is infeasible.");
386unsigned int PathSection::size()
const
391void PathSection::print(std::ostream &out)
const
394 base::SpaceInformationPtr bundle = graph->
getBundle();
395 base::SpaceInformationPtr base = graph->
getBase();
397 out << std::string(80,
'-') << std::endl;
398 out <<
"PATH SECTION" << std::endl;
399 out << std::string(80,
'-') << std::endl;
401 out <<
section_.size() <<
" states over " << restriction_->
size() <<
" base states." << std::endl;
404 for (
int k = 0; k < (int)
section_.size(); k++)
406 if (k > maxDisplay && k < std::max(0, (
int)
section_.size() - maxDisplay))
408 int idx = sectionBaseStateIndices_.at(k);
409 out <<
"State " << k <<
": ";
411 out <<
"Over Base state (idx " << idx <<
") ";
412 base->printState(restriction_->
getBasePath().at(idx));
416 out << std::string(80,
'-') << std::endl;
The exception type for ompl.
Definition of an abstract state.
A configuration in Bundle-space.
normalized_index_type index
Index of configuration in boost::graph. Usually in the interval [0,num_vertices(graph)],...
Configuration * parent
parent index for {qrrt*}
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.
void addGoalConfiguration(Configuration *x)
Add configuration to graph as goal vertex.
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.
void project(const ompl::base::State *xBundle, ompl::base::State *xBase) const
Bundle Space Projection Operator onto first component ProjectBase: Bundle \rightarrow Base.
Representation of path restriction (union of fibers over a base path).
double getLengthBasePath() const
Length of base path.
const base::State * getBaseStateAt(int k) const
Return State at index k on base path.
BundleSpaceGraph * getBundleSpaceGraph()
Return pointer to underlying bundle space graph.
double getLengthBasePathUntil(int k)
Cumulative length until base state index k.
unsigned int size() const
Return number of discrete states in base path.
const std::vector< base::State * > & getBasePath() const
Return discrete states representation of base path.
Representation of a path section (not necessarily feasible).
bool checkMotion(HeadPtr &)
Checks if section is feasible.
void interpolateL1FiberFirst(HeadPtr &)
Interpolate along restriction using L1 metric.
void interpolateL1FiberLast(HeadPtr &)
base::State * at(int k) const
Methods to access sections like std::vector.
void interpolateL2(HeadPtr &)
std::vector< base::State * > section_
Interpolated section along restriction.
void sanityCheck()
checks if section is feasible
std::pair< base::State *, double > lastValid_
Last valid state on feasible segment.
Configuration * addFeasibleSegment(Configuration *xLast, base::State *sNext)
Add vertex for sNext and edge to xLast by assuming motion is valid
#define OMPL_ERROR(fmt,...)
Log a formatted error string.
std::ostream & operator<<(std::ostream &stream, Cost c)
Output operator for Cost.
This namespace contains datastructures and planners to exploit multilevel abstractions,...
Main namespace. Contains everything in this library.